Remove global .frc971.control_loops.drivetrain_queue object
Change-Id: I424f09dcc8bc210e49cbdc805d1a423a72332617
diff --git a/aos/input/action_joystick_input.h b/aos/input/action_joystick_input.h
index 17f1363..2536a8d 100644
--- a/aos/input/action_joystick_input.h
+++ b/aos/input/action_joystick_input.h
@@ -34,7 +34,7 @@
: ::aos::input::JoystickInput(event_loop),
input_config_(input_config),
drivetrain_input_reader_(
- DrivetrainInputReader::Make(input_type, dt_config)),
+ DrivetrainInputReader::Make(event_loop, input_type, dt_config)),
dt_config_(dt_config),
autonomous_action_factory_(
::frc971::autonomous::BaseAutonomousActor::MakeFactory(event_loop)),
diff --git a/aos/input/drivetrain_input.cc b/aos/input/drivetrain_input.cc
index 8c0fa6b..eddbdc5 100644
--- a/aos/input/drivetrain_input.cc
+++ b/aos/input/drivetrain_input.cc
@@ -10,7 +10,6 @@
#include "aos/logging/logging.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-using ::frc971::control_loops::drivetrain_queue;
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::ControlBit;
using ::aos::input::driver_station::JoystickAxis;
@@ -32,9 +31,9 @@
const double throttle_torque = wheel_and_throttle.throttle_torque;
const bool high_gear = wheel_and_throttle.high_gear;
- drivetrain_queue.status.FetchLatest();
- if (drivetrain_queue.status.get()) {
- robot_velocity_ = drivetrain_queue.status->robot_speed;
+ drivetrain_status_fetcher_.Fetch();
+ if (drivetrain_status_fetcher_.get()) {
+ robot_velocity_ = drivetrain_status_fetcher_->robot_speed;
}
// If we have a vision align function, and it is in control, don't run the
@@ -70,11 +69,11 @@
}
}
- if (drivetrain_queue.status.get()) {
+ if (drivetrain_status_fetcher_.get()) {
if (is_control_loop_driving && !last_is_control_loop_driving_) {
- left_goal_ = drivetrain_queue.status->estimated_left_position +
+ left_goal_ = drivetrain_status_fetcher_->estimated_left_position +
wheel * wheel_multiplier_;
- right_goal_ = drivetrain_queue.status->estimated_right_position -
+ right_goal_ = drivetrain_status_fetcher_->estimated_right_position -
wheel * wheel_multiplier_;
}
}
@@ -83,7 +82,7 @@
left_goal_ - wheel * wheel_multiplier_ + throttle * 0.3;
const double current_right_goal =
right_goal_ + wheel * wheel_multiplier_ + throttle * 0.3;
- auto new_drivetrain_goal = drivetrain_queue.goal.MakeMessage();
+ auto new_drivetrain_goal = drivetrain_goal_sender_.MakeMessage();
new_drivetrain_goal->wheel = wheel;
new_drivetrain_goal->wheel_velocity = wheel_velocity;
new_drivetrain_goal->wheel_torque = wheel_torque;
@@ -224,14 +223,15 @@
}
std::unique_ptr<SteeringWheelDrivetrainInputReader>
-SteeringWheelDrivetrainInputReader::Make(bool default_high_gear) {
+SteeringWheelDrivetrainInputReader::Make(::aos::EventLoop *event_loop,
+ bool default_high_gear) {
const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
const ButtonLocation kQuickTurn(1, 5);
const ButtonLocation kTurn1(1, 7);
const ButtonLocation kTurn2(1, 11);
std::unique_ptr<SteeringWheelDrivetrainInputReader> result(
new SteeringWheelDrivetrainInputReader(
- kSteeringWheel, kDriveThrottle, kQuickTurn, kTurn1,
+ event_loop, kSteeringWheel, kDriveThrottle, kQuickTurn, kTurn1,
TurnButtonUse::kControlLoopDriving, kTurn2,
TurnButtonUse::kControlLoopDriving));
result.get()->set_default_high_gear(default_high_gear);
@@ -240,7 +240,8 @@
}
std::unique_ptr<PistolDrivetrainInputReader> PistolDrivetrainInputReader::Make(
- bool default_high_gear, TopButtonUse top_button_use) {
+ ::aos::EventLoop *event_loop, bool default_high_gear,
+ TopButtonUse top_button_use) {
// Pistol Grip controller
const JoystickAxis kTriggerHigh(1, 1), kTriggerLow(1, 4),
kTriggerVelocityHigh(1, 2), kTriggerVelocityLow(1, 5),
@@ -274,17 +275,18 @@
std::unique_ptr<PistolDrivetrainInputReader> result(
new PistolDrivetrainInputReader(
- kWheelHigh, kWheelLow, kTriggerVelocityHigh, kTriggerVelocityLow,
- kTriggerTorqueHigh, kTriggerTorqueLow, kTriggerHigh, kTriggerLow,
- kWheelVelocityHigh, kWheelVelocityLow, kWheelTorqueHigh,
- kWheelTorqueLow, kQuickTurn, kShiftHigh, kShiftLow, kTurn1, kTurn2,
- BottomButton));
+ event_loop, kWheelHigh, kWheelLow, kTriggerVelocityHigh,
+ kTriggerVelocityLow, kTriggerTorqueHigh, kTriggerTorqueLow,
+ kTriggerHigh, kTriggerLow, kWheelVelocityHigh, kWheelVelocityLow,
+ kWheelTorqueHigh, kWheelTorqueLow, kQuickTurn, kShiftHigh, kShiftLow,
+ kTurn1, kTurn2, BottomButton));
result->set_default_high_gear(default_high_gear);
return result;
}
-std::unique_ptr<XboxDrivetrainInputReader> XboxDrivetrainInputReader::Make() {
+std::unique_ptr<XboxDrivetrainInputReader> XboxDrivetrainInputReader::Make(
+ ::aos::EventLoop *event_loop) {
// xbox
const JoystickAxis kSteeringWheel(1, 5), kDriveThrottle(1, 2);
const ButtonLocation kQuickTurn(1, 5);
@@ -294,14 +296,14 @@
const ButtonLocation kTurn2(1, 2);
std::unique_ptr<XboxDrivetrainInputReader> result(
- new XboxDrivetrainInputReader(kSteeringWheel, kDriveThrottle, kQuickTurn,
- kTurn1, TurnButtonUse::kControlLoopDriving,
- kTurn2,
+ new XboxDrivetrainInputReader(event_loop, kSteeringWheel, kDriveThrottle,
+ kQuickTurn, kTurn1,
+ TurnButtonUse::kControlLoopDriving, kTurn2,
TurnButtonUse::kControlLoopDriving));
return result;
}
::std::unique_ptr<DrivetrainInputReader> DrivetrainInputReader::Make(
- InputType type,
+ ::aos::EventLoop *event_loop, InputType type,
const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
&dt_config) {
std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader;
@@ -310,18 +312,18 @@
switch (type) {
case InputType::kSteeringWheel:
- drivetrain_input_reader =
- SteeringWheelDrivetrainInputReader::Make(dt_config.default_high_gear);
+ drivetrain_input_reader = SteeringWheelDrivetrainInputReader::Make(
+ event_loop, dt_config.default_high_gear);
break;
case InputType::kPistol:
drivetrain_input_reader = PistolDrivetrainInputReader::Make(
- dt_config.default_high_gear,
+ event_loop, dt_config.default_high_gear,
dt_config.pistol_grip_shift_enables_line_follow
? PistolDrivetrainInputReader::TopButtonUse::kLineFollow
: PistolDrivetrainInputReader::TopButtonUse::kShift);
break;
case InputType::kXbox:
- drivetrain_input_reader = XboxDrivetrainInputReader::Make();
+ drivetrain_input_reader = XboxDrivetrainInputReader::Make(event_loop);
break;
}
return drivetrain_input_reader;
diff --git a/aos/input/drivetrain_input.h b/aos/input/drivetrain_input.h
index 2b7d73d..26dfe95 100644
--- a/aos/input/drivetrain_input.h
+++ b/aos/input/drivetrain_input.h
@@ -43,7 +43,8 @@
kLineFollow,
};
// Inputs driver station button and joystick locations
- DrivetrainInputReader(driver_station::JoystickAxis wheel,
+ DrivetrainInputReader(::aos::EventLoop *event_loop,
+ driver_station::JoystickAxis wheel,
driver_station::JoystickAxis throttle,
driver_station::ButtonLocation quick_turn,
driver_station::ButtonLocation turn1,
@@ -56,7 +57,15 @@
turn1_(turn1),
turn1_use_(turn1_use),
turn2_(turn2),
- turn2_use_(turn2_use) {}
+ turn2_use_(turn2_use),
+ drivetrain_status_fetcher_(
+ event_loop
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
+ ".frc971.control_loops.drivetrain_queue.status")),
+ drivetrain_goal_sender_(
+ event_loop
+ ->MakeSender<::frc971::control_loops::DrivetrainQueue::Goal>(
+ ".frc971.control_loops.drivetrain_queue.goal")) {}
virtual ~DrivetrainInputReader() = default;
@@ -69,7 +78,7 @@
// Constructs the appropriate DrivetrainInputReader.
static std::unique_ptr<DrivetrainInputReader> Make(
- InputType type,
+ ::aos::EventLoop *event_loop, InputType type,
const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
&dt_config);
@@ -121,6 +130,11 @@
virtual WheelAndThrottle GetWheelAndThrottle(
const ::aos::input::driver_station::Data &data) = 0;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ drivetrain_status_fetcher_;
+ ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Goal>
+ drivetrain_goal_sender_;
+
double robot_velocity_ = 0.0;
// Goals to send to the drivetrain in closed loop mode.
double left_goal_ = 0.0;
@@ -141,7 +155,7 @@
// Creates a DrivetrainInputReader with the corresponding joystick ports and
// axis for the big steering wheel and throttle stick.
static std::unique_ptr<SteeringWheelDrivetrainInputReader> Make(
- bool default_high_gear);
+ ::aos::EventLoop *event_loop, bool default_high_gear);
// Sets the default shifter position
void set_default_high_gear(bool default_high_gear) {
@@ -171,11 +185,12 @@
// Creates a DrivetrainInputReader with the corresponding joystick ports and
// axis for the (cheap) pistol grip controller.
static std::unique_ptr<PistolDrivetrainInputReader> Make(
- bool default_high_gear, TopButtonUse top_button_use);
+ ::aos::EventLoop *event_loop, bool default_high_gear,
+ TopButtonUse top_button_use);
private:
PistolDrivetrainInputReader(
- driver_station::JoystickAxis wheel_high,
+ ::aos::EventLoop *event_loop, driver_station::JoystickAxis wheel_high,
driver_station::JoystickAxis wheel_low,
driver_station::JoystickAxis wheel_velocity_high,
driver_station::JoystickAxis wheel_velocity_low,
@@ -193,8 +208,8 @@
driver_station::ButtonLocation turn1,
driver_station::ButtonLocation turn2,
driver_station::ButtonLocation slow_down)
- : DrivetrainInputReader(wheel_high, throttle_high, quick_turn, turn1,
- TurnButtonUse::kLineFollow, turn2,
+ : DrivetrainInputReader(event_loop, wheel_high, throttle_high, quick_turn,
+ turn1, TurnButtonUse::kLineFollow, turn2,
TurnButtonUse::kControlLoopDriving),
wheel_low_(wheel_low),
wheel_velocity_high_(wheel_velocity_high),
@@ -245,7 +260,8 @@
// Creates a DrivetrainInputReader with the corresponding joystick ports and
// axis for the Xbox controller.
- static std::unique_ptr<XboxDrivetrainInputReader> Make();
+ static std::unique_ptr<XboxDrivetrainInputReader> Make(
+ ::aos::EventLoop *event_loop);
private:
WheelAndThrottle GetWheelAndThrottle(
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.
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index c6af732..c7a307e 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -86,6 +86,7 @@
float rel_theta;
};
+// Published on ".frc971.control_loops.drivetrain_queue"
queue_group DrivetrainQueue {
implements aos.control_loops.ControlLoop;
@@ -221,5 +222,3 @@
queue Output output;
queue Status status;
};
-
-queue_group DrivetrainQueue drivetrain_queue;
diff --git a/frc971/control_loops/drivetrain/replay_drivetrain.cc b/frc971/control_loops/drivetrain/replay_drivetrain.cc
index 6749329..2f63c9e 100644
--- a/frc971/control_loops/drivetrain/replay_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/replay_drivetrain.cc
@@ -15,10 +15,17 @@
::aos::InitNRT();
+ ::frc971::control_loops::DrivetrainQueue drivetrain_queue(
+ ".frc971.control_loops.drivetrain_queue",
+ ".frc971.control_loops.drivetrain_queue.goal",
+ ".frc971.control_loops.drivetrain_queue.position",
+ ".frc971.control_loops.drivetrain_queue.output",
+ ".frc971.control_loops.drivetrain_queue.status");
+
{
::aos::controls::ControlLoopReplayer<
::frc971::control_loops::DrivetrainQueue>
- replayer(&::frc971::control_loops::drivetrain_queue, "drivetrain");
+ replayer(&drivetrain_queue, "drivetrain");
replayer.AddDirectQueueSender<::frc971::sensors::GyroReading>(
"wpilib_interface.Gyro", "sending", ".frc971.sensors.gyro_reading");
@@ -26,10 +33,6 @@
replayer.ProcessFile(argv[i]);
}
}
- ::frc971::control_loops::drivetrain_queue.goal.Clear();
- ::frc971::control_loops::drivetrain_queue.status.Clear();
- ::frc971::control_loops::drivetrain_queue.position.Clear();
- ::frc971::control_loops::drivetrain_queue.output.Clear();
::aos::Cleanup();
}
diff --git a/y2012/joystick_reader.cc b/y2012/joystick_reader.cc
index 97bc283..86c7a90 100644
--- a/y2012/joystick_reader.cc
+++ b/y2012/joystick_reader.cc
@@ -14,8 +14,6 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2012/control_loops/accessories/accessories.q.h"
-using ::frc971::control_loops::drivetrain_queue;
-
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::JoystickAxis;
using ::aos::input::driver_station::ControlBit;
@@ -82,6 +80,10 @@
public:
Reader(::aos::EventLoop *event_loop)
: ::aos::input::JoystickInput(event_loop),
+ drivetrain_goal_sender_(
+ event_loop
+ ->MakeSender<::frc971::control_loops::DrivetrainQueue::Goal>(
+ ".frc971.control_loops.drivetrain_queue.goal")),
accessories_goal_sender_(
event_loop
->MakeSender<::y2012::control_loops::AccessoriesQueue::Message>(
@@ -104,12 +106,13 @@
if (data.PosEdge(kShiftHigh)) {
is_high_gear_ = true;
}
- if (!drivetrain_queue.goal.MakeWithBuilder()
- .wheel(wheel)
- .throttle(throttle)
- .highgear(is_high_gear_)
- .quickturn(data.IsPressed(kQuickTurn))
- .Send()) {
+ auto drivetrain_message = drivetrain_goal_sender_.MakeMessage();
+ drivetrain_message->wheel = wheel;
+ drivetrain_message->throttle = throttle;
+ drivetrain_message->highgear = is_high_gear_;
+ drivetrain_message->quickturn = data.IsPressed(kQuickTurn);
+
+ if (!drivetrain_message.Send()) {
LOG(WARNING, "sending stick values failed\n");
}
}
@@ -127,6 +130,8 @@
}
private:
+ ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Goal>
+ drivetrain_goal_sender_;
::aos::Sender<::y2012::control_loops::AccessoriesQueue::Message>
accessories_goal_sender_;
diff --git a/y2012/wpilib_interface.cc b/y2012/wpilib_interface.cc
index 91a86ed..46f1ec2 100644
--- a/y2012/wpilib_interface.cc
+++ b/y2012/wpilib_interface.cc
@@ -48,7 +48,6 @@
#define M_PI 3.14159265358979323846
#endif
-using ::frc971::control_loops::drivetrain_queue;
using ::y2012::control_loops::AccessoriesQueue;
using namespace frc;
using aos::make_unique;
@@ -75,11 +74,15 @@
: ::frc971::wpilib::SensorReader(event_loop),
accessories_position_sender_(
event_loop->MakeSender<::aos::control_loops::Position>(
- ".y2012.control_loops.accessories_queue.position")) {}
+ ".y2012.control_loops.accessories_queue.position")),
+ drivetrain_position_sender_(
+ event_loop->MakeSender<
+ ::frc971::control_loops::DrivetrainQueue::Position>(
+ ".frc971.control_loops.drivetrain_queue.position")) {}
void RunIteration() {
{
- auto drivetrain_message = drivetrain_queue.position.MakeMessage();
+ auto drivetrain_message = drivetrain_position_sender_.MakeMessage();
drivetrain_message->right_encoder =
drivetrain_translate(drivetrain_right_encoder_->GetRaw());
drivetrain_message->left_encoder =
@@ -97,6 +100,8 @@
private:
::aos::Sender<::aos::control_loops::Position> accessories_position_sender_;
+ ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ drivetrain_position_sender_;
};
class SolenoidWriter {
diff --git a/y2014/joystick_reader.cc b/y2014/joystick_reader.cc
index 5510635..ecd55c9 100644
--- a/y2014/joystick_reader.cc
+++ b/y2014/joystick_reader.cc
@@ -20,8 +20,6 @@
#include "y2014/control_loops/drivetrain/drivetrain_base.h"
#include "y2014/control_loops/shooter/shooter.q.h"
-using ::frc971::control_loops::drivetrain_queue;
-
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::JoystickAxis;
using ::aos::input::driver_station::ControlBit;
@@ -168,6 +166,10 @@
shooter_goal_sender_(
event_loop->MakeSender<::y2014::control_loops::ShooterQueue::Goal>(
".y2014.control_loops.shooter_queue.goal")),
+ drivetrain_status_fetcher_(
+ event_loop
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
+ ".frc971.control_loops.drivetrain_queue.status")),
shot_power_(80.0),
goal_angle_(0.0),
separation_angle_(kGrabSeparation),
@@ -348,11 +350,11 @@
velocity_compensation_ = 0.0;
}
- ::frc971::control_loops::drivetrain_queue.status.FetchLatest();
+ drivetrain_status_fetcher_.Fetch();
double goal_angle = goal_angle_;
- if (::frc971::control_loops::drivetrain_queue.status.get()) {
- goal_angle += SpeedToAngleOffset(
- ::frc971::control_loops::drivetrain_queue.status->robot_speed);
+ if (drivetrain_status_fetcher_.get()) {
+ goal_angle +=
+ SpeedToAngleOffset(drivetrain_status_fetcher_->robot_speed);
} else {
LOG_INTERVAL(no_drivetrain_status_);
}
@@ -417,6 +419,8 @@
claw_status_fetcher_;
::aos::Sender<::y2014::control_loops::ClawQueue::Goal> claw_goal_sender_;
::aos::Sender<::y2014::control_loops::ShooterQueue::Goal> shooter_goal_sender_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ drivetrain_status_fetcher_;
double shot_power_;
double goal_angle_;
diff --git a/y2014/wpilib_interface.cc b/y2014/wpilib_interface.cc
index 5e398db..d4d7ad6 100644
--- a/y2014/wpilib_interface.cc
+++ b/y2014/wpilib_interface.cc
@@ -53,7 +53,6 @@
#define M_PI 3.14159265358979323846
#endif
-using ::frc971::control_loops::drivetrain_queue;
using ::y2014::control_loops::ClawQueue;
using ::y2014::control_loops::ShooterQueue;
using aos::make_unique;
@@ -124,7 +123,11 @@
shooter_position_sender_(event_loop->MakeSender<ShooterQueue::Position>(
".y2014.control_loops.shooter_queue.position")),
claw_position_sender_(event_loop->MakeSender<ClawQueue::Position>(
- ".y2014.control_loops.claw_queue.position")) {
+ ".y2014.control_loops.claw_queue.position")),
+ drivetrain_position_sender_(
+ event_loop->MakeSender<
+ ::frc971::control_loops::DrivetrainQueue::Position>(
+ ".frc971.control_loops.drivetrain_queue.position")) {
// Set it to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateMediumEncoderFilterHz(kMaximumEncoderPulsesPerSecond);
@@ -246,7 +249,7 @@
const auto &values = constants::GetValues();
{
- auto drivetrain_message = drivetrain_queue.position.MakeMessage();
+ auto drivetrain_message = drivetrain_position_sender_.MakeMessage();
drivetrain_message->right_encoder =
drivetrain_translate(drivetrain_right_encoder_->GetRaw());
drivetrain_message->left_encoder =
@@ -402,6 +405,8 @@
::aos::Sender<::y2014::sensors::AutoMode> auto_mode_sender_;
::aos::Sender<ShooterQueue::Position> shooter_position_sender_;
::aos::Sender<ClawQueue::Position> claw_position_sender_;
+ ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ drivetrain_position_sender_;
::std::unique_ptr<::frc::AnalogInput> auto_selector_analog_;
diff --git a/y2014_bot3/joystick_reader.cc b/y2014_bot3/joystick_reader.cc
index 7b6d950..989a1c1 100644
--- a/y2014_bot3/joystick_reader.cc
+++ b/y2014_bot3/joystick_reader.cc
@@ -18,8 +18,6 @@
#include "y2014_bot3/control_loops/drivetrain/drivetrain_base.h"
#include "y2014_bot3/control_loops/rollers/rollers.q.h"
-using ::frc971::control_loops::drivetrain_queue;
-
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::POVLocation;
using ::aos::input::driver_station::JoystickAxis;
@@ -56,7 +54,7 @@
::frc971::autonomous::BaseAutonomousActor::MakeFactory(
event_loop)) {
drivetrain_input_reader_ = DrivetrainInputReader::Make(
- DrivetrainInputReader::InputType::kSteeringWheel,
+ event_loop, DrivetrainInputReader::InputType::kSteeringWheel,
::y2014_bot3::control_loops::drivetrain::GetDrivetrainConfig());
}
diff --git a/y2014_bot3/wpilib_interface.cc b/y2014_bot3/wpilib_interface.cc
index f1413fb..c526742 100644
--- a/y2014_bot3/wpilib_interface.cc
+++ b/y2014_bot3/wpilib_interface.cc
@@ -49,7 +49,6 @@
#endif
using ::aos::util::SimpleLogInterval;
-using ::frc971::control_loops::drivetrain_queue;
using ::frc971::wpilib::BufferedPcm;
using ::frc971::wpilib::BufferedSolenoid;
using ::frc971::wpilib::LoopOutputHandler;
@@ -80,12 +79,16 @@
rollers_position_sender_(
event_loop->MakeSender<
::y2014_bot3::control_loops::RollersQueue::Position>(
- ".y2014_bot3.control_loops.rollers_queue.position")) {}
+ ".y2014_bot3.control_loops.rollers_queue.position")),
+ drivetrain_position_sender_(
+ event_loop->MakeSender<
+ ::frc971::control_loops::DrivetrainQueue::Position>(
+ ".frc971.control_loops.drivetrain_queue.position")) {}
void RunIteration() {
// Drivetrain
{
- auto drivetrain_message = drivetrain_queue.position.MakeMessage();
+ auto drivetrain_message = drivetrain_position_sender_.MakeMessage();
drivetrain_message->right_encoder =
-drivetrain_translate(drivetrain_right_encoder_->GetRaw());
drivetrain_message->left_encoder =
@@ -108,6 +111,8 @@
private:
::aos::Sender<::y2014_bot3::control_loops::RollersQueue::Position>
rollers_position_sender_;
+ ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ drivetrain_position_sender_;
};
// Writes out our pneumatic outputs.
diff --git a/y2016/actors/autonomous_actor.cc b/y2016/actors/autonomous_actor.cc
index e94cccd..01650c4 100644
--- a/y2016/actors/autonomous_actor.cc
+++ b/y2016/actors/autonomous_actor.cc
@@ -18,7 +18,6 @@
namespace y2016 {
namespace actors {
using ::aos::monotonic_clock;
-using ::frc971::control_loops::drivetrain_queue;
namespace chrono = ::std::chrono;
namespace this_thread = ::std::this_thread;
@@ -245,20 +244,20 @@
last_angle = vision_status_fetcher_->horizontal_angle;
}
- drivetrain_queue.status.FetchLatest();
- drivetrain_queue.goal.FetchLatest();
+ drivetrain_status_fetcher_.Fetch();
+ drivetrain_goal_fetcher_.Fetch();
- if (drivetrain_queue.status.get() && drivetrain_queue.goal.get()) {
- const double left_goal = drivetrain_queue.goal->left_goal;
- const double right_goal = drivetrain_queue.goal->right_goal;
+ if (drivetrain_status_fetcher_.get() && drivetrain_goal_fetcher_.get()) {
+ const double left_goal = drivetrain_goal_fetcher_->left_goal;
+ const double right_goal = drivetrain_goal_fetcher_->right_goal;
const double left_current =
- drivetrain_queue.status->estimated_left_position;
+ drivetrain_status_fetcher_->estimated_left_position;
const double right_current =
- drivetrain_queue.status->estimated_right_position;
+ drivetrain_status_fetcher_->estimated_right_position;
const double left_velocity =
- drivetrain_queue.status->estimated_left_velocity;
+ drivetrain_status_fetcher_->estimated_left_velocity;
const double right_velocity =
- drivetrain_queue.status->estimated_right_velocity;
+ drivetrain_status_fetcher_->estimated_right_velocity;
if (vision_valid && ::std::abs(last_angle) < 0.02 &&
::std::abs((left_goal - right_goal) -
@@ -496,14 +495,14 @@
if (!WaitForAboveAngle(above)) return;
// Ok, we are good now. Compensate by moving the goal by the error.
// Should be here at 2.7
- 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);
const double distance_to_go = (left_error + right_error) / 2.0;
const double distance_compensation =
goal_distance - tip_distance - distance_to_go;
@@ -558,7 +557,7 @@
return;
}
phased_loop.SleepUntilNext();
- drivetrain_queue.status.FetchLatest();
+ drivetrain_status_fetcher_.Fetch();
if (IsDriveDone()) {
return;
}
diff --git a/y2016/actors/vision_align_actor.cc b/y2016/actors/vision_align_actor.cc
index 1110f92..f42aad9 100644
--- a/y2016/actors/vision_align_actor.cc
+++ b/y2016/actors/vision_align_actor.cc
@@ -20,14 +20,17 @@
namespace y2016 {
namespace actors {
-using ::frc971::control_loops::drivetrain_queue;
VisionAlignActor::VisionAlignActor(::aos::EventLoop *event_loop)
: aos::common::actions::ActorBase<actors::VisionAlignActionQueueGroup>(
event_loop, ".y2016.actors.vision_align_action"),
vision_status_fetcher_(
event_loop->MakeFetcher<::y2016::vision::VisionStatus>(
- ".y2016.vision.vision_status")) {}
+ ".y2016.vision.vision_status")),
+ drivetrain_goal_sender_(
+ event_loop
+ ->MakeSender<::frc971::control_loops::DrivetrainQueue::Goal>(
+ ".frc971.control_loops.drivetrain_queue.goal")) {}
bool VisionAlignActor::RunAction(
const actors::VisionAlignActionParams & /*params*/) {
@@ -64,15 +67,16 @@
const double left_current = vision_status.drivetrain_left_position;
const double right_current = vision_status.drivetrain_right_position;
- if (!drivetrain_queue.goal.MakeWithBuilder()
- .wheel(0.0)
- .throttle(0.0)
- .highgear(false)
- .quickturn(false)
- .controller_type(1)
- .left_goal(left_current + side_distance_change)
- .right_goal(right_current - side_distance_change)
- .Send()) {
+ auto drivetrain_message = drivetrain_goal_sender_.MakeMessage();
+ drivetrain_message->wheel = 0.0;
+ drivetrain_message->throttle = 0.0;
+ drivetrain_message->highgear = false;
+ drivetrain_message->quickturn = false;
+ drivetrain_message->controller_type = 1;
+ drivetrain_message->left_goal = left_current + side_distance_change;
+ drivetrain_message->right_goal = right_current - side_distance_change;
+
+ if (!drivetrain_message.Send()) {
LOG(WARNING, "sending drivetrain goal failed\n");
}
}
diff --git a/y2016/actors/vision_align_actor.h b/y2016/actors/vision_align_actor.h
index f92f507..586f7d0 100644
--- a/y2016/actors/vision_align_actor.h
+++ b/y2016/actors/vision_align_actor.h
@@ -5,6 +5,7 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "y2016/actors/vision_align_action.q.h"
#include "y2016/vision/vision.q.h"
@@ -29,6 +30,8 @@
private:
::aos::Fetcher<::y2016::vision::VisionStatus> vision_status_fetcher_;
+ ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Goal>
+ drivetrain_goal_sender_;
};
} // namespace actors
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index 618b538..64c033a 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -25,8 +25,6 @@
#include "y2016/queues/ball_detector.q.h"
#include "y2016/vision/vision.q.h"
-using ::frc971::control_loops::drivetrain_queue;
-
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::ControlBit;
using ::aos::input::driver_station::JoystickAxis;
@@ -91,6 +89,14 @@
event_loop
->MakeSender<::y2016::control_loops::SuperstructureQueue::Goal>(
".y2016.control_loops.superstructure_queue.goal")),
+ drivetrain_goal_fetcher_(
+ event_loop
+ ->MakeFetcher<::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")),
intake_goal_(0.0),
shoulder_goal_(M_PI / 2.0),
wrist_goal_(0.0),
@@ -183,8 +189,9 @@
// Forwards shot
shoulder_goal_ = M_PI / 2.0 + 0.1;
wrist_goal_ = M_PI + 0.41 + 0.02 - 0.005;
- if (drivetrain_queue.status.get()) {
- wrist_goal_ += drivetrain_queue.status->ground_angle;
+ drivetrain_status_fetcher_.Fetch();
+ if (drivetrain_status_fetcher_.get()) {
+ wrist_goal_ += drivetrain_status_fetcher_->ground_angle;
}
shooter_velocity_ = 640.0;
intake_goal_ = intake_when_shooting;
@@ -192,8 +199,9 @@
// Backwards shot
shoulder_goal_ = M_PI / 2.0 - 0.4;
wrist_goal_ = -0.62 - 0.02;
- if (drivetrain_queue.status.get()) {
- wrist_goal_ += drivetrain_queue.status->ground_angle;
+ drivetrain_status_fetcher_.Fetch();
+ if (drivetrain_status_fetcher_.get()) {
+ wrist_goal_ += drivetrain_status_fetcher_->ground_angle;
}
shooter_velocity_ = 640.0;
intake_goal_ = intake_when_shooting;
@@ -259,19 +267,20 @@
if (data.IsPressed(kFire) && shooter_velocity_ != 0.0) {
if (data.IsPressed(kVisionAlign)) {
// Make sure that we are lined up.
- drivetrain_queue.status.FetchLatest();
- drivetrain_queue.goal.FetchLatest();
- if (drivetrain_queue.status.get() && drivetrain_queue.goal.get()) {
- const double left_goal = drivetrain_queue.goal->left_goal;
- const double right_goal = drivetrain_queue.goal->right_goal;
+ drivetrain_status_fetcher_.Fetch();
+ drivetrain_goal_fetcher_.Fetch();
+ if (drivetrain_status_fetcher_.get() &&
+ drivetrain_goal_fetcher_.get()) {
+ const double left_goal = drivetrain_goal_fetcher_->left_goal;
+ const double right_goal = drivetrain_goal_fetcher_->right_goal;
const double left_current =
- drivetrain_queue.status->estimated_left_position;
+ drivetrain_status_fetcher_->estimated_left_position;
const double right_current =
- drivetrain_queue.status->estimated_right_position;
+ drivetrain_status_fetcher_->estimated_right_position;
const double left_velocity =
- drivetrain_queue.status->estimated_left_velocity;
+ drivetrain_status_fetcher_->estimated_left_velocity;
const double right_velocity =
- drivetrain_queue.status->estimated_right_velocity;
+ drivetrain_status_fetcher_->estimated_right_velocity;
if (vision_action_running_ && ::std::abs(last_angle_) < 0.02 &&
::std::abs((left_goal - right_goal) -
(left_current - right_current)) /
@@ -399,6 +408,10 @@
superstructure_status_fetcher_;
::aos::Sender<::y2016::control_loops::SuperstructureQueue::Goal>
superstructure_goal_sender_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Goal>
+ drivetrain_goal_fetcher_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ drivetrain_status_fetcher_;
// Whatever these are set to are our default goals to send out after zeroing.
double intake_goal_;
diff --git a/y2016/vision/target_receiver.cc b/y2016/vision/target_receiver.cc
index 57fbd51..a496848 100644
--- a/y2016/vision/target_receiver.cc
+++ b/y2016/vision/target_receiver.cc
@@ -194,26 +194,25 @@
// Handles calculating drivetrain offsets.
class DrivetrainOffsetCalculator {
public:
- // To be called by ::std::thread.
- void operator()() {
- auto &status = ::frc971::control_loops::drivetrain_queue.status;
- while (run_) {
- status.FetchAnother();
-
- ::aos::MutexLocker locker(&lock_);
- data_[data_index_].time = status->sent_time;
- data_[data_index_].left = status->estimated_left_position;
- data_[data_index_].right = status->estimated_right_position;
- ++data_index_;
- if (data_index_ == data_.size()) data_index_ = 0;
- if (valid_data_ < data_.size()) ++valid_data_;
- }
- }
+ DrivetrainOffsetCalculator(::aos::EventLoop *event_loop)
+ : drivetrain_status_fetcher_(
+ event_loop
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
+ ".frc971.control_loops.drivetrain_queue.status")) {}
// Takes a vision status message with everything except
// drivetrain_{left,right}_position set and sets those.
// Returns false if it doesn't have enough data to fill them out.
bool CompleteVisionStatus(::y2016::vision::VisionStatus *status) {
+ while (drivetrain_status_fetcher_.FetchNext()) {
+ data_[data_index_].time = drivetrain_status_fetcher_->sent_time;
+ data_[data_index_].left = drivetrain_status_fetcher_->estimated_left_position;
+ data_[data_index_].right = drivetrain_status_fetcher_->estimated_right_position;
+ ++data_index_;
+ if (data_index_ == data_.size()) data_index_ = 0;
+ if (valid_data_ < data_.size()) ++valid_data_;
+ }
+
if (valid_data_ == 0) return false;
const monotonic_clock::time_point capture_time =
@@ -237,8 +236,6 @@
return true;
}
- void Quit() { run_ = false; }
-
private:
struct DrivetrainData {
monotonic_clock::time_point time;
@@ -250,7 +247,6 @@
// Do not call this if valid_data_ is 0.
void FindBeforeAfter(DrivetrainData *before, DrivetrainData *after,
monotonic_clock::time_point capture_time) {
- ::aos::MutexLocker locker(&lock_);
size_t location = 0;
while (true) {
// We hit the end of our data. Just fill them both out as the last data
@@ -290,15 +286,14 @@
}
}
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ drivetrain_status_fetcher_;
+
::std::array<DrivetrainData, 200> data_;
// The index into data_ the next data point is going at.
size_t data_index_ = 0;
// How many elemets of data_ are valid.
size_t valid_data_ = 0;
-
- ::aos::Mutex lock_;
-
- ::std::atomic<bool> run_{true};
};
void Main() {
@@ -312,8 +307,7 @@
LOG(INFO, "calibration: %s\n",
stereo.calibration().ShortDebugString().c_str());
- DrivetrainOffsetCalculator drivetrain_offset;
- ::std::thread drivetrain_offset_thread(::std::ref(drivetrain_offset));
+ DrivetrainOffsetCalculator drivetrain_offset(&event_loop);
CameraHandler left;
CameraHandler right;
@@ -426,9 +420,6 @@
right.target().ShortDebugString().c_str());
}
}
-
- drivetrain_offset.Quit();
- drivetrain_offset_thread.join();
}
} // namespace vision
diff --git a/y2016/wpilib_interface.cc b/y2016/wpilib_interface.cc
index c2a4271..5b5fecb 100644
--- a/y2016/wpilib_interface.cc
+++ b/y2016/wpilib_interface.cc
@@ -53,7 +53,6 @@
#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "y2016/queues/ball_detector.q.h"
-using ::frc971::control_loops::drivetrain_queue;
using aos::make_unique;
using ::frc971::wpilib::LoopOutputHandler;
using ::y2016::control_loops::shooter::ShooterQueue;
@@ -158,7 +157,11 @@
".y2016.control_loops.shooter.shooter_queue.position")),
superstructure_position_sender_(
event_loop->MakeSender<SuperstructureQueue::Position>(
- ".y2016.control_loops.superstructure_queue.position")) {
+ ".y2016.control_loops.superstructure_queue.position")),
+ drivetrain_position_sender_(
+ event_loop->MakeSender<
+ ::frc971::control_loops::DrivetrainQueue::Position>(
+ ".frc971.control_loops.drivetrain_queue.position")) {
// Set it to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxDrivetrainShooterEncoderPulsesPerSecond);
@@ -255,7 +258,7 @@
void RunIteration() {
{
- auto drivetrain_message = drivetrain_queue.position.MakeMessage();
+ auto drivetrain_message = drivetrain_position_sender_.MakeMessage();
drivetrain_message->right_encoder =
drivetrain_translate(-drivetrain_right_encoder_->GetRaw());
drivetrain_message->left_encoder =
@@ -326,6 +329,8 @@
::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
::aos::Sender<ShooterQueue::Position> shooter_position_sender_;
::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
+ ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ drivetrain_position_sender_;
::std::unique_ptr<::frc::AnalogInput> drivetrain_left_hall_,
drivetrain_right_hall_;
diff --git a/y2017/actors/autonomous_actor.cc b/y2017/actors/autonomous_actor.cc
index 9b7855c..33c29a8 100644
--- a/y2017/actors/autonomous_actor.cc
+++ b/y2017/actors/autonomous_actor.cc
@@ -14,7 +14,6 @@
namespace y2017 {
namespace actors {
using ::aos::monotonic_clock;
-using ::frc971::control_loops::drivetrain_queue;
namespace chrono = ::std::chrono;
namespace this_thread = ::std::this_thread;
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index 9b0297e..c76dd19 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -21,7 +21,6 @@
} // namespace
typedef ::y2017::constants::Values::ShotParams ShotParams;
-using ::frc971::control_loops::drivetrain_queue;
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
@@ -30,6 +29,10 @@
vision_status_fetcher_(
event_loop->MakeFetcher<::y2017::vision::VisionStatus>(
".y2017.vision.vision_status")),
+ drivetrain_status_fetcher_(
+ event_loop
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
+ ".frc971.control_loops.drivetrain_queue.status")),
column_(event_loop) {
shot_interpolation_table_ =
::frc971::shooter_interpolation::InterpolationTable<ShotParams>({
@@ -84,9 +87,9 @@
// If we are moving too fast, disable shooting and clear the accumulator.
double robot_velocity = 0.0;
- drivetrain_queue.status.FetchLatest();
- if (drivetrain_queue.status.get()) {
- robot_velocity = drivetrain_queue.status->robot_speed;
+ drivetrain_status_fetcher_.Fetch();
+ if (drivetrain_status_fetcher_.get()) {
+ robot_velocity = drivetrain_status_fetcher_->robot_speed;
}
if (::std::abs(robot_velocity) > 0.2) {
diff --git a/y2017/control_loops/superstructure/superstructure.h b/y2017/control_loops/superstructure/superstructure.h
index e387cf2..660b861 100644
--- a/y2017/control_loops/superstructure/superstructure.h
+++ b/y2017/control_loops/superstructure/superstructure.h
@@ -43,6 +43,8 @@
private:
::aos::Fetcher<::y2017::vision::VisionStatus> vision_status_fetcher_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ drivetrain_status_fetcher_;
hood::Hood hood_;
intake::Intake intake_;
diff --git a/y2017/joystick_reader.cc b/y2017/joystick_reader.cc
index 10bf20d..79d5c12 100644
--- a/y2017/joystick_reader.cc
+++ b/y2017/joystick_reader.cc
@@ -18,8 +18,6 @@
#include "y2017/control_loops/superstructure/superstructure.q.h"
#include "y2017/control_loops/drivetrain/drivetrain_base.h"
-using ::frc971::control_loops::drivetrain_queue;
-
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::ControlBit;
using ::aos::input::driver_station::JoystickAxis;
diff --git a/y2017/wpilib_interface.cc b/y2017/wpilib_interface.cc
index e8dca61..4fac858 100644
--- a/y2017/wpilib_interface.cc
+++ b/y2017/wpilib_interface.cc
@@ -58,7 +58,6 @@
#define M_PI 3.14159265358979323846
#endif
-using ::frc971::control_loops::drivetrain_queue;
using ::y2017::control_loops::SuperstructureQueue;
using ::y2017::constants::Values;
using ::aos::monotonic_clock;
@@ -132,7 +131,11 @@
".frc971.autonomous.auto_mode")),
superstructure_position_sender_(
event_loop->MakeSender<SuperstructureQueue::Position>(
- ".y2017.control_loops.superstructure_queue.position")) {
+ ".y2017.control_loops.superstructure_queue.position")),
+ drivetrain_position_sender_(
+ event_loop->MakeSender<
+ ::frc971::control_loops::DrivetrainQueue::Position>(
+ ".frc971.control_loops.drivetrain_queue.position")) {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -204,7 +207,7 @@
void RunIteration() {
{
- auto drivetrain_message = drivetrain_queue.position.MakeMessage();
+ auto drivetrain_message = drivetrain_position_sender_.MakeMessage();
drivetrain_message->right_encoder =
drivetrain_translate(drivetrain_right_encoder_->GetRaw());
drivetrain_message->right_speed =
@@ -265,6 +268,8 @@
private:
::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
+ ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ drivetrain_position_sender_;
DigitalGlitchFilter hall_filter_;
diff --git a/y2018/actors/autonomous_actor.cc b/y2018/actors/autonomous_actor.cc
index 5b4bda8..80cdbe9 100644
--- a/y2018/actors/autonomous_actor.cc
+++ b/y2018/actors/autonomous_actor.cc
@@ -16,12 +16,13 @@
using ::frc971::ProfileParameters;
using ::aos::monotonic_clock;
-using ::frc971::control_loops::drivetrain_queue;
namespace chrono = ::std::chrono;
namespace this_thread = ::std::this_thread;
namespace {
+namespace arm = ::y2018::control_loops::superstructure::arm;
+
const ProfileParameters kFinalSwitchDrive = {0.5, 1.5};
const ProfileParameters kDrive = {5.0, 2.5};
const ProfileParameters kThirdBoxDrive = {3.0, 2.5};
diff --git a/y2018/actors/autonomous_actor.h b/y2018/actors/autonomous_actor.h
index 1b20190..4cedff3 100644
--- a/y2018/actors/autonomous_actor.h
+++ b/y2018/actors/autonomous_actor.h
@@ -15,9 +15,6 @@
namespace y2018 {
namespace actors {
-using ::frc971::control_loops::drivetrain_queue;
-
-namespace arm = ::y2018::control_loops::superstructure::arm;
class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
public:
@@ -31,7 +28,8 @@
roller_voltage_ = 0.0;
left_intake_angle_ = -3.2;
right_intake_angle_ = -3.2;
- arm_goal_position_ = arm::NeutralIndex();
+ arm_goal_position_ =
+ ::y2018::control_loops::superstructure::arm::NeutralIndex();
grab_box_ = false;
open_claw_ = false;
close_claw_ = false;
@@ -50,7 +48,8 @@
double roller_voltage_ = 0.0;
double left_intake_angle_ = -3.2;
double right_intake_angle_ = -3.2;
- uint32_t arm_goal_position_ = arm::NeutralIndex();
+ uint32_t arm_goal_position_ =
+ ::y2018::control_loops::superstructure::arm::NeutralIndex();
bool grab_box_ = false;
bool open_claw_ = false;
bool close_claw_ = false;
@@ -125,22 +124,22 @@
}
superstructure_status_fetcher_.Fetch();
- drivetrain_queue.status.FetchLatest();
- if (drivetrain_queue.status.get() &&
+ drivetrain_status_fetcher_.Fetch();
+ if (drivetrain_status_fetcher_.get() &&
superstructure_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;
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index d0ff71d..24a8880 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -33,6 +33,10 @@
vision_status_fetcher_(
event_loop->MakeFetcher<::y2018::vision::VisionStatus>(
".y2018.vision.vision_status")),
+ drivetrain_output_fetcher_(
+ event_loop
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
+ ".frc971.control_loops.drivetrain_queue.output")),
intake_left_(constants::GetValues().left_intake.zeroing),
intake_right_(constants::GetValues().right_intake.zeroing) {}
@@ -256,7 +260,7 @@
}
status->rotation_state = static_cast<uint32_t>(rotation_state_);
- ::frc971::control_loops::drivetrain_queue.output.FetchLatest();
+ drivetrain_output_fetcher_.Fetch();
vision_status_fetcher_.Fetch();
monotonic_clock::time_point monotonic_now = event_loop()->monotonic_now();
@@ -275,11 +279,10 @@
SendColors(0.0, 0.0, 0.5);
} else if (position->box_distance < 0.2) {
SendColors(0.0, 0.5, 0.0);
- } else if (::frc971::control_loops::drivetrain_queue.output.get() &&
- ::std::max(::std::abs(::frc971::control_loops::drivetrain_queue
- .output->left_voltage),
- ::std::abs(::frc971::control_loops::drivetrain_queue
- .output->right_voltage)) > 11.5) {
+ } else if (drivetrain_output_fetcher_.get() &&
+ ::std::max(::std::abs(drivetrain_output_fetcher_->left_voltage),
+ ::std::abs(drivetrain_output_fetcher_->right_voltage)) >
+ 11.5) {
SendColors(0.5, 0.0, 0.5);
} else {
SendColors(0.0, 0.0, 0.0);
diff --git a/y2018/control_loops/superstructure/superstructure.h b/y2018/control_loops/superstructure/superstructure.h
index bceb643..81b6d9d 100644
--- a/y2018/control_loops/superstructure/superstructure.h
+++ b/y2018/control_loops/superstructure/superstructure.h
@@ -5,6 +5,7 @@
#include "aos/controls/control_loop.h"
#include "aos/events/event-loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "y2018/control_loops/superstructure/arm/arm.h"
#include "y2018/control_loops/superstructure/intake/intake.h"
@@ -40,6 +41,8 @@
::aos::Sender<::y2018::StatusLight> status_light_sender_;
::aos::Fetcher<::y2018::vision::VisionStatus> vision_status_fetcher_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
+ drivetrain_output_fetcher_;
intake::IntakeSide intake_left_;
intake::IntakeSide intake_right_;
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
index 8533dce..202c428 100644
--- a/y2018/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -274,7 +274,6 @@
".y2018.control_loops.superstructure.status",
".y2018.control_loops.superstructure.position"),
superstructure_(&event_loop_, ".y2018.control_loops.superstructure") {
- ::frc971::control_loops::drivetrain_queue.output.Clear();
set_team_id(::frc971::control_loops::testing::kTeamNumber);
}
diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index eb23196..d819c6f 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -25,7 +25,6 @@
#include "y2018/vision.pb.h"
using ::aos::monotonic_clock;
-using ::frc971::control_loops::drivetrain_queue;
using ::aos::events::ProtoTXUdpSocket;
using ::aos::events::RXUdpSocket;
using ::aos::input::driver_station::ButtonLocation;
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index 16c95c8..903301e 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -56,7 +56,6 @@
#define M_PI 3.14159265358979323846
#endif
-using ::frc971::control_loops::drivetrain_queue;
using ::y2018::control_loops::SuperstructureQueue;
using ::y2018::constants::Values;
using ::aos::monotonic_clock;
@@ -148,7 +147,11 @@
: ::frc971::wpilib::SensorReader(event_loop),
superstructure_position_sender_(
event_loop->MakeSender<SuperstructureQueue::Position>(
- ".y2018.control_loops.superstructure_queue.position")) {
+ ".y2018.control_loops.superstructure_queue.position")),
+ drivetrain_position_sender_(
+ event_loop->MakeSender<
+ ::frc971::control_loops::DrivetrainQueue::Position>(
+ ".frc971.control_loops.drivetrain_queue.position")) {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -269,7 +272,7 @@
void RunIteration() {
{
- auto drivetrain_message = drivetrain_queue.position.MakeMessage();
+ auto drivetrain_message = drivetrain_position_sender_.MakeMessage();
drivetrain_message->left_encoder =
drivetrain_translate(drivetrain_left_encoder_->GetRaw());
drivetrain_message->left_speed =
@@ -344,6 +347,8 @@
private:
::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
+ ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ drivetrain_position_sender_;
::std::unique_ptr<frc::AnalogInput> left_drivetrain_shifter_,
right_drivetrain_shifter_;
diff --git a/y2019/actors/autonomous_actor.cc b/y2019/actors/autonomous_actor.cc
index 0f2b04e..5138123 100644
--- a/y2019/actors/autonomous_actor.cc
+++ b/y2019/actors/autonomous_actor.cc
@@ -16,7 +16,6 @@
namespace y2019 {
namespace actors {
using ::aos::monotonic_clock;
-using ::frc971::control_loops::drivetrain_queue;
namespace chrono = ::std::chrono;
AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
@@ -46,9 +45,9 @@
return false;
}
phased_loop.SleepUntilNext();
- drivetrain_queue.status.FetchLatest();
- if (drivetrain_queue.status->x > x) {
- LOG(INFO, "X at %f\n", drivetrain_queue.status->x);
+ drivetrain_status_fetcher_.Fetch();
+ if (drivetrain_status_fetcher_->x > x) {
+ LOG(INFO, "X at %f\n", drivetrain_status_fetcher_->x);
return true;
}
}
@@ -65,9 +64,9 @@
return false;
}
phased_loop.SleepUntilNext();
- drivetrain_queue.status.FetchLatest();
- if (::std::abs(drivetrain_queue.status->y) < y) {
- LOG(INFO, "Y at %f\n", drivetrain_queue.status->y);
+ drivetrain_status_fetcher_.Fetch();
+ if (::std::abs(drivetrain_status_fetcher_->y) < y) {
+ LOG(INFO, "Y at %f\n", drivetrain_status_fetcher_->y);
return true;
}
}
@@ -104,12 +103,12 @@
// Wait for the drivetrain to run so it has time to reset the heading.
// Otherwise our drivetrain reset will do a 180 right at the start.
- drivetrain_queue.status.FetchAnother();
- LOG(INFO, "Heading is %f\n", drivetrain_queue.status->estimated_heading);
+ WaitUntil([this]() { return drivetrain_status_fetcher_.Fetch(); });
+ LOG(INFO, "Heading is %f\n", drivetrain_status_fetcher_->estimated_heading);
InitializeEncoders();
ResetDrivetrain();
- drivetrain_queue.status.FetchAnother();
- LOG(INFO, "Heading is %f\n", drivetrain_queue.status->estimated_heading);
+ WaitUntil([this]() { return drivetrain_status_fetcher_.Fetch(); });
+ LOG(INFO, "Heading is %f\n", drivetrain_status_fetcher_->estimated_heading);
ResetDrivetrain();
InitializeEncoders();
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index a4796a8..ec9e2e5 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -16,6 +16,10 @@
: aos::controls::ControlLoop<SuperstructureQueue>(event_loop, name),
status_light_sender_(
event_loop->MakeSender<::y2019::StatusLight>(".y2019.status_light")),
+ drivetrain_status_fetcher_(
+ event_loop
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
+ ".frc971.control_loops.drivetrain_queue.status")),
elevator_(constants::GetValues().elevator.subsystem_params),
wrist_(constants::GetValues().wrist.subsystem_params),
intake_(constants::GetValues().intake),
@@ -94,22 +98,20 @@
intake_.set_min_position(collision_avoidance_.min_intake_goal());
intake_.set_max_position(collision_avoidance_.max_intake_goal());
- ::frc971::control_loops::drivetrain_queue.status.FetchLatest();
+ drivetrain_status_fetcher_.Fetch();
if (status && unsafe_goal) {
// Light Logic
if (status->estopped) {
// Estop is red
SendColors(1.0, 0.0, 0.0);
- } else if (::frc971::control_loops::drivetrain_queue.status.get() &&
- ::frc971::control_loops::drivetrain_queue.status
- ->line_follow_logging.frozen) {
+ } else if (drivetrain_status_fetcher_.get() &&
+ drivetrain_status_fetcher_->line_follow_logging.frozen) {
// Vision align is flashing white for button pressed, purple for target
// acquired.
++line_blink_count_;
if (line_blink_count_ < 20) {
- if (::frc971::control_loops::drivetrain_queue.status
- ->line_follow_logging.have_target) {
+ if (drivetrain_status_fetcher_->line_follow_logging.have_target) {
SendColors(1.0, 0.0, 1.0);
} else {
SendColors(1.0, 1.0, 1.0);
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
index 2d51cff..aab8e7d 100644
--- a/y2019/control_loops/superstructure/superstructure.h
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -3,6 +3,7 @@
#include "aos/controls/control_loop.h"
#include "aos/events/event-loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2019/constants.h"
#include "y2019/control_loops/superstructure/collision_avoidance.h"
@@ -47,6 +48,8 @@
void SendColors(float red, float green, float blue);
::aos::Sender<::y2019::StatusLight> status_light_sender_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ drivetrain_status_fetcher_;
PotAndAbsoluteEncoderSubsystem elevator_;
PotAndAbsoluteEncoderSubsystem wrist_;
diff --git a/y2019/control_loops/superstructure/superstructure_lib_test.cc b/y2019/control_loops/superstructure/superstructure_lib_test.cc
index 036b210..7628ca8 100644
--- a/y2019/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2019/control_loops/superstructure/superstructure_lib_test.cc
@@ -6,7 +6,6 @@
#include "aos/controls/control_loop_test.h"
#include "aos/queue.h"
#include "frc971/control_loops/capped_test_plant.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
@@ -289,7 +288,6 @@
".y2019.control_loops.superstructure.superstructure_queue."
"position"),
superstructure_(&event_loop_) {
- ::frc971::control_loops::drivetrain_queue.status.Clear();
set_team_id(::frc971::control_loops::testing::kTeamNumber);
}
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 31b5158..9ca37d8 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -56,7 +56,6 @@
#define M_PI 3.14159265358979323846
#endif
-using ::frc971::control_loops::drivetrain_queue;
using ::y2019::control_loops::superstructure::SuperstructureQueue;
using ::y2019::constants::Values;
using ::aos::monotonic_clock;
@@ -138,7 +137,11 @@
superstructure_position_sender_(
event_loop->MakeSender<SuperstructureQueue::Position>(
".y2019.control_loops.superstructure.superstructure_queue."
- "position")) {
+ "position")),
+ drivetrain_position_sender_(
+ event_loop->MakeSender<
+ ::frc971::control_loops::DrivetrainQueue::Position>(
+ ".frc971.control_loops.drivetrain_queue.position")) {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -230,7 +233,7 @@
void RunIteration() override {
{
- auto drivetrain_message = drivetrain_queue.position.MakeMessage();
+ auto drivetrain_message = drivetrain_position_sender_.MakeMessage();
drivetrain_message->left_encoder =
drivetrain_translate(drivetrain_left_encoder_->GetRaw());
drivetrain_message->left_speed =
@@ -302,6 +305,8 @@
private:
::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
+ ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ drivetrain_position_sender_;
::frc971::wpilib::AbsoluteEncoderAndPotentiometer elevator_encoder_,
wrist_encoder_, stilts_encoder_;