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(