Move Position, robot_state, and joystick_state over
ControlLoop should now be completely using the event loop.
Change-Id: Ifb936c384d1031abbc9e61d834b1ef157ad0841c
diff --git a/aos/controls/control_loop-tmpl.h b/aos/controls/control_loop-tmpl.h
index 6953575..49bad45 100644
--- a/aos/controls/control_loop-tmpl.h
+++ b/aos/controls/control_loop-tmpl.h
@@ -3,7 +3,6 @@
#include "aos/logging/logging.h"
#include "aos/logging/queue_logging.h"
-#include "aos/robot_state/robot_state.q.h"
namespace aos {
namespace controls {
@@ -25,8 +24,16 @@
template <class T>
void ControlLoop<T>::Iterate() {
- control_loop_->position.FetchAnother();
- IteratePosition(*control_loop_->position.get());
+ if (!has_iterate_fetcher_) {
+ iterate_position_fetcher_ =
+ event_loop_->MakeFetcher<PositionType>(name_ + ".position");
+ has_iterate_fetcher_ = true;
+ }
+ const bool did_fetch = iterate_position_fetcher_.Fetch();
+ if (!did_fetch) {
+ LOG(FATAL, "Failed to fetch from position queue\n");
+ }
+ IteratePosition(*iterate_position_fetcher_);
}
template <class T>
@@ -52,35 +59,35 @@
LOG_INTERVAL(no_goal_);
}
- const bool new_robot_state = ::aos::robot_state.FetchLatest();
- if (!::aos::robot_state.get()) {
+ const bool new_robot_state = robot_state_fetcher_.Fetch();
+ if (!robot_state_fetcher_.get()) {
LOG_INTERVAL(no_sensor_state_);
return;
}
- if (sensor_reader_pid_ != ::aos::robot_state->reader_pid) {
+ if (sensor_reader_pid_ != robot_state_fetcher_->reader_pid) {
LOG(INFO, "new sensor reader PID %" PRId32 ", old was %" PRId32 "\n",
- ::aos::robot_state->reader_pid, sensor_reader_pid_);
+ robot_state_fetcher_->reader_pid, sensor_reader_pid_);
reset_ = true;
- sensor_reader_pid_ = ::aos::robot_state->reader_pid;
+ sensor_reader_pid_ = robot_state_fetcher_->reader_pid;
}
- bool outputs_enabled = ::aos::robot_state->outputs_enabled;
+ bool outputs_enabled = robot_state_fetcher_->outputs_enabled;
// Check to see if we got a driver station packet recently.
if (new_robot_state) {
- if (::aos::robot_state->outputs_enabled) {
+ if (robot_state_fetcher_->outputs_enabled) {
// If the driver's station reports being disabled, we're probably not
// actually going to send motor values regardless of what the FPGA
// reports.
- last_pwm_sent_ = ::aos::robot_state->sent_time;
+ last_pwm_sent_ = robot_state_fetcher_->sent_time;
}
}
const ::aos::monotonic_clock::time_point now = ::aos::monotonic_clock::now();
const bool motors_off = now >= kPwmDisableTime + last_pwm_sent_;
- ::aos::joystick_state.FetchLatest();
+ joystick_state_fetcher_.Fetch();
if (motors_off) {
- if (::aos::joystick_state.get() && ::aos::joystick_state->enabled) {
+ if (joystick_state_fetcher_.get() && joystick_state_fetcher_->enabled) {
LOG_INTERVAL(motors_off_log_);
}
outputs_enabled = false;
@@ -122,7 +129,7 @@
PCHECK(sigaction(SIGQUIT, &action, nullptr));
PCHECK(sigaction(SIGINT, &action, nullptr));
- event_loop_->MakeWatcher(::std::string(control_loop_->name()) + ".position",
+ event_loop_->MakeWatcher(name_ + ".position",
[this](const PositionType &position) {
this->IteratePosition(position);
});
diff --git a/aos/controls/control_loop.h b/aos/controls/control_loop.h
index 0b6e391..1f05c33 100644
--- a/aos/controls/control_loop.h
+++ b/aos/controls/control_loop.h
@@ -7,6 +7,7 @@
#include "aos/events/event-loop.h"
#include "aos/events/shm-event-loop.h"
#include "aos/queue.h"
+#include "aos/robot_state/robot_state.q.h"
#include "aos/time/time.h"
#include "aos/type_traits/type_traits.h"
#include "aos/util/log_interval.h"
@@ -52,13 +53,22 @@
OutputType;
ControlLoop(T *control_loop)
- : event_loop_(new ::aos::ShmEventLoop()), control_loop_(control_loop) {
- output_sender_ = event_loop_->MakeSender<OutputType>(
- ::std::string(control_loop_->name()) + ".output");
- status_sender_ = event_loop_->MakeSender<StatusType>(
- ::std::string(control_loop_->name()) + ".status");
- goal_fetcher_ = event_loop_->MakeFetcher<GoalType>(
- ::std::string(control_loop_->name()) + ".goal");
+ : shm_event_loop_(new ::aos::ShmEventLoop()),
+ event_loop_(shm_event_loop_.get()),
+ name_(control_loop->name()) {
+ output_sender_ = event_loop_->MakeSender<OutputType>(name_ + ".output");
+ status_sender_ = event_loop_->MakeSender<StatusType>(name_ + ".status");
+ goal_fetcher_ = event_loop_->MakeFetcher<GoalType>(name_ + ".goal");
+ robot_state_fetcher_ =
+ event_loop_->MakeFetcher<::aos::RobotState>(".aos.robot_state");
+ joystick_state_fetcher_ =
+ event_loop_->MakeFetcher<::aos::JoystickState>(".aos.joystick_state");
+ }
+
+ const ::aos::RobotState &robot_state() const { return *robot_state_fetcher_; }
+ bool has_joystick_state() const { return joystick_state_fetcher_.get(); }
+ const ::aos::JoystickState &joystick_state() const {
+ return *joystick_state_fetcher_;
}
// Returns true if all the counters etc in the sensor data have been reset.
@@ -120,12 +130,20 @@
::std::chrono::milliseconds(100);
// Pointer to the queue group
- ::std::unique_ptr<EventLoop> event_loop_;
- T *control_loop_;
+ ::std::unique_ptr<ShmEventLoop> shm_event_loop_;
+ EventLoop *event_loop_;
+ ::std::string name_;
::aos::Sender<OutputType> output_sender_;
::aos::Sender<StatusType> status_sender_;
::aos::Fetcher<GoalType> goal_fetcher_;
+ ::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
+ ::aos::Fetcher<::aos::JoystickState> joystick_state_fetcher_;
+
+ // Fetcher only to be used for the Iterate method. If Iterate is called, Run
+ // can't be called.
+ bool has_iterate_fetcher_ = false;
+ ::aos::Fetcher<PositionType> iterate_position_fetcher_;
bool reset_ = false;
int32_t sensor_reader_pid_ = 0;
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 36f7c82..27a0a76 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -331,7 +331,7 @@
dt_openloop_.SetGoal(*goal);
}
- dt_openloop_.Update();
+ dt_openloop_.Update(robot_state().voltage_battery);
dt_closedloop_.Update(output != NULL && controller_type == 1);
@@ -393,7 +393,7 @@
right_high_requested_ = output->right_high;
}
- const double scalar = ::aos::robot_state->voltage_battery / 12.0;
+ const double scalar = robot_state().voltage_battery / 12.0;
left_voltage *= scalar;
right_voltage *= scalar;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index a6711e4..bb5d5d9 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -164,6 +164,7 @@
last_U_ << my_drivetrain_queue_.output->left_voltage,
my_drivetrain_queue_.output->right_voltage;
{
+ ::aos::robot_state.FetchLatest();
const double scalar = ::aos::robot_state->voltage_battery / 12.0;
last_U_ *= scalar;
}
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index 64b1aff..cc3468a 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -45,7 +45,7 @@
Scalar MaxVelocity();
- void Update();
+ void Update(Scalar voltage_battery);
void SetOutput(::frc971::control_loops::DrivetrainQueue::Output *output);
@@ -292,7 +292,7 @@
}
template <typename Scalar>
-void PolyDrivetrain<Scalar>::Update() {
+void PolyDrivetrain<Scalar>::Update(Scalar voltage_battery) {
if (dt_config_.loop_type == LoopType::CLOSED_LOOP) {
loop_->mutable_X_hat()(0, 0) = kf_->X_hat()(1, 0);
loop_->mutable_X_hat()(1, 0) = kf_->X_hat()(3, 0);
@@ -406,7 +406,9 @@
(R_right / dt_config_.v)(0, 0) + (IsInGear(right_gear_) ? 0 : wiggle),
-kTwelve, kTwelve);
#ifdef __linux__
- loop_->mutable_U() *= kTwelve / ::aos::robot_state->voltage_battery;
+ loop_->mutable_U() *= kTwelve / voltage_battery;
+#else
+ (void)voltage_battery;
#endif // __linux__
}
}
diff --git a/motors/simple_receiver.cc b/motors/simple_receiver.cc
index a78f1e0..0470c31 100644
--- a/motors/simple_receiver.cc
+++ b/motors/simple_receiver.cc
@@ -561,7 +561,7 @@
DrivetrainQueue_Output output;
polydrivetrain->SetGoal(goal);
- polydrivetrain->Update();
+ polydrivetrain->Update(12.0f);
polydrivetrain->SetOutput(&output);
vesc_set_duty(0, -output.left_voltage / 12.0f);
diff --git a/y2014/control_loops/claw/claw.cc b/y2014/control_loops/claw/claw.cc
index 088472c..82ca3a5 100644
--- a/y2014/control_loops/claw/claw.cc
+++ b/y2014/control_loops/claw/claw.cc
@@ -604,9 +604,7 @@
return (
(top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
- (((::aos::joystick_state.get() == NULL)
- ? true
- : ::aos::joystick_state->autonomous) &&
+ ((has_joystick_state() ? joystick_state().autonomous : true) &&
((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
top_claw_.zeroing_state() ==
ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
@@ -674,12 +672,12 @@
}
bool autonomous, enabled;
- if (::aos::joystick_state.get() == nullptr) {
+ if (has_joystick_state()) {
+ autonomous = joystick_state().autonomous;
+ enabled = joystick_state().enabled;
+ } else {
autonomous = true;
enabled = false;
- } else {
- autonomous = ::aos::joystick_state->autonomous;
- enabled = ::aos::joystick_state->enabled;
}
double bottom_claw_velocity_ = 0.0;
diff --git a/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
index f61cc02..7f3a3b0 100644
--- a/y2014/control_loops/shooter/shooter.cc
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -249,8 +249,7 @@
// Don't even let the control loops run.
bool shooter_loop_disable = false;
- const bool disabled =
- !::aos::joystick_state.get() || !::aos::joystick_state->enabled;
+ const bool disabled = !has_joystick_state() || !joystick_state().enabled;
// If true, move the goal if we saturate.
bool cap_goal = false;
@@ -549,7 +548,7 @@
shooter_.absolute_position()) > 0.0005 &&
cycles_not_moved_ > 6) ||
monotonic_clock::now() > shot_end_time_) &&
- ::aos::robot_state->voltage_battery > 10.5) {
+ robot_state().voltage_battery > 10.5) {
state_ = STATE_REQUEST_LOAD;
++shot_count_;
}
diff --git a/y2016/control_loops/superstructure/superstructure_lib_test.cc b/y2016/control_loops/superstructure/superstructure_lib_test.cc
index e4d1301..57ca9fa 100644
--- a/y2016/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2016/control_loops/superstructure/superstructure_lib_test.cc
@@ -90,7 +90,7 @@
pot_encoder_wrist_(constants::Values::kWristEncoderIndexDifference),
superstructure_queue_(".y2016.control_loops.superstructure",
".y2016.control_loops.superstructure.goal",
- ".y2016.control_loops.superstructure.status",
+ ".y2016.control_loops.superstructure.position",
".y2016.control_loops.superstructure.output",
".y2016.control_loops.superstructure.status") {
InitializeIntakePosition(0.0);
@@ -248,7 +248,7 @@
SuperstructureTest()
: superstructure_queue_(".y2016.control_loops.superstructure",
".y2016.control_loops.superstructure.goal",
- ".y2016.control_loops.superstructure.status",
+ ".y2016.control_loops.superstructure.position",
".y2016.control_loops.superstructure.output",
".y2016.control_loops.superstructure.status"),
superstructure_(&superstructure_queue_),