Remove global .frc971.control_loops.drivetrain_queue object
Change-Id: I424f09dcc8bc210e49cbdc805d1a423a72332617
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index 4d3f3fc..c78a90d 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -12,7 +12,6 @@
#include "frc971/control_loops/drivetrain/localizer.q.h"
#include "y2019/control_loops/drivetrain/target_selector.q.h"
-using ::frc971::control_loops::drivetrain_queue;
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
namespace this_thread = ::std::this_thread;
@@ -30,27 +29,44 @@
target_selector_hint_sender_(
event_loop->MakeSender<
::y2019::control_loops::drivetrain::TargetSelectorHint>(
- ".y2019.control_loops.drivetrain.target_selector_hint")) {}
+ ".y2019.control_loops.drivetrain.target_selector_hint")),
+ drivetrain_goal_sender_(
+ event_loop
+ ->MakeSender<::frc971::control_loops::DrivetrainQueue::Goal>(
+ ".frc971.control_loops.drivetrain_queue.goal")),
+ drivetrain_status_fetcher_(
+ event_loop
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
+ ".frc971.control_loops.drivetrain_queue.status")),
+ drivetrain_goal_fetcher_(
+ event_loop
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Goal>(
+ ".frc971.control_loops.drivetrain_queue.goal")) {}
void BaseAutonomousActor::ResetDrivetrain() {
LOG(INFO, "resetting the drivetrain\n");
max_drivetrain_voltage_ = 12.0;
goal_spline_handle_ = 0;
- drivetrain_queue.goal.MakeWithBuilder()
- .controller_type(0)
- .highgear(true)
- .wheel(0.0)
- .throttle(0.0)
- .left_goal(initial_drivetrain_.left)
- .right_goal(initial_drivetrain_.right)
- .max_ss_voltage(max_drivetrain_voltage_)
- .Send();
+
+ auto drivetrain_goal_message = drivetrain_goal_sender_.MakeMessage();
+ drivetrain_goal_message->controller_type = 0;
+ drivetrain_goal_message->highgear = true;
+ drivetrain_goal_message->wheel = 0.0;
+ drivetrain_goal_message->throttle = 0.0;
+ drivetrain_goal_message->left_goal = initial_drivetrain_.left;
+ drivetrain_goal_message->right_goal = initial_drivetrain_.right;
+ drivetrain_goal_message->max_ss_voltage = max_drivetrain_voltage_;
+ drivetrain_goal_message.Send();
}
void BaseAutonomousActor::InitializeEncoders() {
- drivetrain_queue.status.FetchAnother();
- initial_drivetrain_.left = drivetrain_queue.status->estimated_left_position;
- initial_drivetrain_.right = drivetrain_queue.status->estimated_right_position;
+ // Spin until we get a message.
+ WaitUntil([this]() { return drivetrain_status_fetcher_.Fetch(); });
+
+ initial_drivetrain_.left =
+ drivetrain_status_fetcher_->estimated_left_position;
+ initial_drivetrain_.right =
+ drivetrain_status_fetcher_->estimated_right_position;
}
void BaseAutonomousActor::StartDrive(double distance, double angle,
@@ -63,7 +79,7 @@
initial_drivetrain_.right += distance + dangle;
}
- auto drivetrain_message = drivetrain_queue.goal.MakeMessage();
+ auto drivetrain_message = drivetrain_goal_sender_.MakeMessage();
drivetrain_message->controller_type = 1;
drivetrain_message->highgear = true;
drivetrain_message->wheel = 0.0;
@@ -108,7 +124,7 @@
return false;
}
phased_loop.SleepUntilNext();
- drivetrain_queue.status.FetchLatest();
+ drivetrain_status_fetcher_.Fetch();
if (IsDriveDone()) {
return true;
}
@@ -120,18 +136,18 @@
static constexpr double kVelocityTolerance = 0.10;
static constexpr double kProfileTolerance = 0.001;
- if (drivetrain_queue.status.get()) {
- if (::std::abs(drivetrain_queue.status->profiled_left_position_goal -
+ if (drivetrain_status_fetcher_.get()) {
+ if (::std::abs(drivetrain_status_fetcher_->profiled_left_position_goal -
initial_drivetrain_.left) < kProfileTolerance &&
- ::std::abs(drivetrain_queue.status->profiled_right_position_goal -
+ ::std::abs(drivetrain_status_fetcher_->profiled_right_position_goal -
initial_drivetrain_.right) < kProfileTolerance &&
- ::std::abs(drivetrain_queue.status->estimated_left_position -
+ ::std::abs(drivetrain_status_fetcher_->estimated_left_position -
initial_drivetrain_.left) < kPositionTolerance &&
- ::std::abs(drivetrain_queue.status->estimated_right_position -
+ ::std::abs(drivetrain_status_fetcher_->estimated_right_position -
initial_drivetrain_.right) < kPositionTolerance &&
- ::std::abs(drivetrain_queue.status->estimated_left_velocity) <
+ ::std::abs(drivetrain_status_fetcher_->estimated_left_velocity) <
kVelocityTolerance &&
- ::std::abs(drivetrain_queue.status->estimated_right_velocity) <
+ ::std::abs(drivetrain_status_fetcher_->estimated_right_velocity) <
kVelocityTolerance) {
LOG(INFO, "Finished drive\n");
return true;
@@ -149,12 +165,12 @@
return false;
}
phased_loop.SleepUntilNext();
- drivetrain_queue.status.FetchLatest();
+ drivetrain_status_fetcher_.Fetch();
if (IsDriveDone()) {
return true;
}
- if (drivetrain_queue.status.get()) {
- if (drivetrain_queue.status->ground_angle > angle) {
+ if (drivetrain_status_fetcher_.get()) {
+ if (drivetrain_status_fetcher_->ground_angle > angle) {
return true;
}
}
@@ -170,12 +186,12 @@
return false;
}
phased_loop.SleepUntilNext();
- drivetrain_queue.status.FetchLatest();
+ drivetrain_status_fetcher_.Fetch();
if (IsDriveDone()) {
return true;
}
- if (drivetrain_queue.status.get()) {
- if (drivetrain_queue.status->ground_angle < angle) {
+ if (drivetrain_status_fetcher_.get()) {
+ if (drivetrain_status_fetcher_->ground_angle < angle) {
return true;
}
}
@@ -192,15 +208,15 @@
return false;
}
phased_loop.SleepUntilNext();
- drivetrain_queue.status.FetchLatest();
+ drivetrain_status_fetcher_.Fetch();
if (IsDriveDone()) {
return true;
}
- if (drivetrain_queue.status.get()) {
- if (drivetrain_queue.status->ground_angle > max_angle) {
- max_angle = drivetrain_queue.status->ground_angle;
+ if (drivetrain_status_fetcher_.get()) {
+ if (drivetrain_status_fetcher_->ground_angle > max_angle) {
+ max_angle = drivetrain_status_fetcher_->ground_angle;
}
- if (drivetrain_queue.status->ground_angle < max_angle - angle) {
+ if (drivetrain_status_fetcher_->ground_angle < max_angle - angle) {
return true;
}
}
@@ -223,21 +239,21 @@
return false;
}
phased_loop.SleepUntilNext();
- drivetrain_queue.status.FetchLatest();
- if (drivetrain_queue.status.get()) {
+ drivetrain_status_fetcher_.Fetch();
+ if (drivetrain_status_fetcher_.get()) {
const double left_profile_error =
(initial_drivetrain_.left -
- drivetrain_queue.status->profiled_left_position_goal);
+ drivetrain_status_fetcher_->profiled_left_position_goal);
const double right_profile_error =
(initial_drivetrain_.right -
- drivetrain_queue.status->profiled_right_position_goal);
+ drivetrain_status_fetcher_->profiled_right_position_goal);
const double left_error =
(initial_drivetrain_.left -
- drivetrain_queue.status->estimated_left_position);
+ drivetrain_status_fetcher_->estimated_left_position);
const double right_error =
(initial_drivetrain_.right -
- drivetrain_queue.status->estimated_right_position);
+ drivetrain_status_fetcher_->estimated_right_position);
const double profile_distance_to_go =
(left_profile_error + right_profile_error) / 2.0;
@@ -285,20 +301,20 @@
return false;
}
phased_loop.SleepUntilNext();
- drivetrain_queue.status.FetchLatest();
+ drivetrain_status_fetcher_.Fetch();
const Eigen::Matrix<double, 7, 1> current_error =
(Eigen::Matrix<double, 7, 1>()
<< initial_drivetrain_.left -
- drivetrain_queue.status->profiled_left_position_goal,
+ drivetrain_status_fetcher_->profiled_left_position_goal,
0.0, initial_drivetrain_.right -
- drivetrain_queue.status->profiled_right_position_goal,
+ drivetrain_status_fetcher_->profiled_right_position_goal,
0.0, 0.0, 0.0, 0.0)
.finished();
const Eigen::Matrix<double, 2, 1> linear_error =
dt_config_.LeftRightToLinear(current_error);
- if (drivetrain_queue.status.get()) {
+ if (drivetrain_status_fetcher_.get()) {
if (::std::abs(linear_error(0)) < tolerance) {
LOG(INFO, "Finished drive\n");
return true;
@@ -321,20 +337,20 @@
return false;
}
phased_loop.SleepUntilNext();
- drivetrain_queue.status.FetchLatest();
+ drivetrain_status_fetcher_.Fetch();
const Eigen::Matrix<double, 7, 1> current_error =
(Eigen::Matrix<double, 7, 1>()
<< initial_drivetrain_.left -
- drivetrain_queue.status->profiled_left_position_goal,
+ drivetrain_status_fetcher_->profiled_left_position_goal,
0.0, initial_drivetrain_.right -
- drivetrain_queue.status->profiled_right_position_goal,
+ drivetrain_status_fetcher_->profiled_right_position_goal,
0.0, 0.0, 0.0, 0.0)
.finished();
const Eigen::Matrix<double, 2, 1> angular_error =
dt_config_.LeftRightToAngular(current_error);
- if (drivetrain_queue.status.get()) {
+ if (drivetrain_status_fetcher_.get()) {
if (::std::abs(angular_error(0)) < tolerance) {
LOG(INFO, "Finished turn\n");
return true;
@@ -349,15 +365,14 @@
}
double BaseAutonomousActor::DriveDistanceLeft() {
- using ::frc971::control_loops::drivetrain_queue;
- drivetrain_queue.status.FetchLatest();
- if (drivetrain_queue.status.get()) {
+ drivetrain_status_fetcher_.Fetch();
+ if (drivetrain_status_fetcher_.get()) {
const double left_error =
(initial_drivetrain_.left -
- drivetrain_queue.status->estimated_left_position);
+ drivetrain_status_fetcher_->estimated_left_position);
const double right_error =
(initial_drivetrain_.right -
- drivetrain_queue.status->estimated_right_position);
+ drivetrain_status_fetcher_->estimated_right_position);
return (left_error + right_error) / 2.0;
} else {
@@ -367,11 +382,12 @@
bool BaseAutonomousActor::SplineHandle::SplineDistanceRemaining(
double distance) {
- drivetrain_queue.status.FetchLatest();
- if (drivetrain_queue.status.get()) {
- return drivetrain_queue.status->trajectory_logging.is_executing &&
- drivetrain_queue.status->trajectory_logging.distance_remaining <
- distance;
+ base_autonomous_actor_->drivetrain_status_fetcher_.Fetch();
+ if (base_autonomous_actor_->drivetrain_status_fetcher_.get()) {
+ return base_autonomous_actor_->drivetrain_status_fetcher_
+ ->trajectory_logging.is_executing &&
+ base_autonomous_actor_->drivetrain_status_fetcher_
+ ->trajectory_logging.distance_remaining < distance;
}
return false;
}
@@ -393,7 +409,7 @@
}
void BaseAutonomousActor::LineFollowAtVelocity(double velocity, int hint) {
- auto drivetrain_message = drivetrain_queue.goal.MakeMessage();
+ auto drivetrain_message = drivetrain_goal_sender_.MakeMessage();
drivetrain_message->controller_type = 3;
// TODO(james): Currently the 4.0 is copied from the
// line_follow_drivetrain.cc, but it is somewhat year-specific, so we should
@@ -411,14 +427,15 @@
int32_t spline_handle = (++spline_handle_) | ((getpid() & 0xFFFF) << 15);
- drivetrain_queue.goal.FetchLatest();
+ drivetrain_goal_fetcher_.Fetch();
- auto drivetrain_message = drivetrain_queue.goal.MakeMessage();
+ auto drivetrain_message =
+ drivetrain_goal_sender_.MakeMessage();
int controller_type = 2;
- if (drivetrain_queue.goal.get()) {
- controller_type = drivetrain_queue.goal->controller_type;
- drivetrain_message->throttle = drivetrain_queue.goal->throttle;
+ if (drivetrain_goal_fetcher_.get()) {
+ controller_type = drivetrain_goal_fetcher_->controller_type;
+ drivetrain_message->throttle = drivetrain_goal_fetcher_->throttle;
}
drivetrain_message->controller_type = controller_type;
@@ -436,14 +453,16 @@
}
bool BaseAutonomousActor::SplineHandle::IsPlanned() {
- drivetrain_queue.status.FetchLatest();
- LOG_STRUCT(DEBUG, "dts", *drivetrain_queue.status.get());
- if (drivetrain_queue.status.get() &&
- ((drivetrain_queue.status->trajectory_logging.planning_spline_idx ==
- spline_handle_ &&
- drivetrain_queue.status->trajectory_logging.planning_state == 3) ||
- drivetrain_queue.status->trajectory_logging.current_spline_idx ==
- spline_handle_)) {
+ base_autonomous_actor_->drivetrain_status_fetcher_.Fetch();
+ LOG_STRUCT(DEBUG, "dts",
+ *(base_autonomous_actor_->drivetrain_status_fetcher_.get()));
+ if (base_autonomous_actor_->drivetrain_status_fetcher_.get() &&
+ ((base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging
+ .planning_spline_idx == spline_handle_ &&
+ base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging
+ .planning_state == 3) ||
+ base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging
+ .current_spline_idx == spline_handle_)) {
return true;
}
return false;
@@ -466,7 +485,8 @@
}
void BaseAutonomousActor::SplineHandle::Start() {
- auto drivetrain_message = drivetrain_queue.goal.MakeMessage();
+ auto drivetrain_message =
+ base_autonomous_actor_->drivetrain_goal_sender_.MakeMessage();
drivetrain_message->controller_type = 2;
LOG(INFO, "Starting spline\n");
@@ -480,20 +500,22 @@
}
bool BaseAutonomousActor::SplineHandle::IsDone() {
- drivetrain_queue.status.FetchLatest();
- LOG_STRUCT(INFO, "dts", *drivetrain_queue.status.get());
+ base_autonomous_actor_->drivetrain_status_fetcher_.Fetch();
+ LOG_STRUCT(INFO, "dts",
+ *(base_autonomous_actor_->drivetrain_status_fetcher_.get()));
// We check that the spline we are waiting on is neither currently planning
// nor executing (we check is_executed because it is possible to receive
// status messages with is_executing false before the execution has started).
// We check for planning so that the user can go straight from starting the
// planner to executing without a WaitForPlan in between.
- if (drivetrain_queue.status.get() &&
- ((!drivetrain_queue.status->trajectory_logging.is_executed &&
- drivetrain_queue.status->trajectory_logging.current_spline_idx ==
- spline_handle_) ||
- drivetrain_queue.status->trajectory_logging.planning_spline_idx ==
- spline_handle_)) {
+ if (base_autonomous_actor_->drivetrain_status_fetcher_.get() &&
+ ((!base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging
+ .is_executed &&
+ base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging
+ .current_spline_idx == spline_handle_) ||
+ base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging
+ .planning_spline_idx == spline_handle_)) {
return false;
}
return true;
diff --git a/frc971/autonomous/base_autonomous_actor.h b/frc971/autonomous/base_autonomous_actor.h
index 263ce73..5a5bcea 100644
--- a/frc971/autonomous/base_autonomous_actor.h
+++ b/frc971/autonomous/base_autonomous_actor.h
@@ -113,11 +113,17 @@
};
InitialDrivetrain initial_drivetrain_;
- private:
- friend class SplineHandle;
::aos::Sender<::y2019::control_loops::drivetrain::TargetSelectorHint>
target_selector_hint_sender_;
+ ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Goal>
+ drivetrain_goal_sender_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ drivetrain_status_fetcher_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Goal>
+ drivetrain_goal_fetcher_;
+ private:
+ friend class SplineHandle;
double max_drivetrain_voltage_ = 12.0;
// Unique counter so we get unique spline handles.