Convert all year's robots to proper event loops
Each robot has a couple of event loops, one per thread. Each of these
threads corresponds to the threads from before the change. y2016 has
been tested on real hardware.
Change-Id: I99f726a8bc0498204c1a3b99f15508119eed9ad3
diff --git a/y2014_bot3/wpilib_interface.cc b/y2014_bot3/wpilib_interface.cc
index f0d0ba2..1c6bddf 100644
--- a/y2014_bot3/wpilib_interface.cc
+++ b/y2014_bot3/wpilib_interface.cc
@@ -56,7 +56,6 @@
using ::frc971::wpilib::LoopOutputHandler;
using ::frc971::wpilib::JoystickSender;
using ::frc971::wpilib::GyroSender;
-using namespace frc;
using aos::make_unique;
namespace y2014_bot3 {
@@ -115,16 +114,30 @@
// Writes out our pneumatic outputs.
class SolenoidWriter {
public:
- SolenoidWriter(const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
+ SolenoidWriter(::aos::EventLoop *event_loop,
+ const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
: pcm_(pcm),
- drivetrain_(".frc971.control_loops.drivetrain_queue.output"),
- rollers_(".y2014_bot3.control_loops.rollers_queue.output") {}
+ drivetrain_(
+ event_loop
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
+ ".frc971.control_loops.drivetrain_queue.output")),
+ rollers_(event_loop->MakeFetcher<
+ ::y2014_bot3::control_loops::RollersQueue::Output>(
+ ".y2014_bot3.control_loops.rollers_queue.output")) {
+ event_loop->set_name("Solenoids");
+ event_loop->SetRuntimeRealtimePriority(27);
- void set_pressure_switch(::std::unique_ptr<DigitalInput> pressure_switch) {
+ event_loop->AddPhasedLoop([this](int iterations) { Loop(iterations); },
+ ::std::chrono::milliseconds(20),
+ ::std::chrono::milliseconds(1));
+ }
+
+ void set_pressure_switch(
+ ::std::unique_ptr<::frc::DigitalInput> pressure_switch) {
pressure_switch_ = ::std::move(pressure_switch);
}
- void set_compressor_relay(::std::unique_ptr<Relay> compressor_relay) {
+ void set_compressor_relay(::std::unique_ptr<::frc::Relay> compressor_relay) {
compressor_relay_ = ::std::move(compressor_relay);
}
@@ -144,64 +157,50 @@
rollers_back_ = ::std::move(s);
}
- void operator()() {
- ::aos::SetCurrentThreadName("Solenoids");
- ::aos::SetCurrentThreadRealtimePriority(27);
+ void Loop(const int iterations) {
+ if (iterations != 1) {
+ LOG(DEBUG, "Solenoids skipped %d iterations\n", iterations - 1);
+ }
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(20),
- ::aos::monotonic_clock::now(),
- ::std::chrono::milliseconds(1));
-
- while (run_) {
- {
- const int iterations = phased_loop.SleepUntilNext();
- if (iterations != 1) {
- LOG(DEBUG, "Solenoids skipped %d iterations\n", iterations - 1);
- }
- }
-
- // Drivetrain
- {
- drivetrain_.FetchLatest();
- if (drivetrain_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
- drivetrain_left_->Set(drivetrain_->left_high);
- drivetrain_right_->Set(drivetrain_->right_high);
- }
- }
-
- // Intake
- {
- rollers_.FetchLatest();
- if (rollers_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *rollers_);
- rollers_front_->Set(rollers_->front_extended);
- rollers_back_->Set(rollers_->back_extended);
- }
- }
-
- // Compressor
- {
- ::frc971::wpilib::PneumaticsToLog to_log;
- {
- // Refill if pneumatic pressure goes too low.
- const bool compressor_on = !pressure_switch_->Get();
- to_log.compressor_on = compressor_on;
- if (compressor_on) {
- compressor_relay_->Set(Relay::kForward);
- } else {
- compressor_relay_->Set(Relay::kOff);
- }
- }
-
- pcm_->Flush();
- to_log.read_solenoids = pcm_->GetAll();
- LOG_STRUCT(DEBUG, "pneumatics info", to_log);
+ // Drivetrain
+ {
+ drivetrain_.Fetch();
+ if (drivetrain_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
+ drivetrain_left_->Set(drivetrain_->left_high);
+ drivetrain_right_->Set(drivetrain_->right_high);
}
}
- }
- void Quit() { run_ = false; }
+ // Intake
+ {
+ rollers_.Fetch();
+ if (rollers_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *rollers_);
+ rollers_front_->Set(rollers_->front_extended);
+ rollers_back_->Set(rollers_->back_extended);
+ }
+ }
+
+ // Compressor
+ {
+ ::frc971::wpilib::PneumaticsToLog to_log;
+ {
+ // Refill if pneumatic pressure goes too low.
+ const bool compressor_on = !pressure_switch_->Get();
+ to_log.compressor_on = compressor_on;
+ if (compressor_on) {
+ compressor_relay_->Set(::frc::Relay::kForward);
+ } else {
+ compressor_relay_->Set(::frc::Relay::kOff);
+ }
+ }
+
+ pcm_->Flush();
+ to_log.read_solenoids = pcm_->GetAll();
+ LOG_STRUCT(DEBUG, "pneumatics info", to_log);
+ }
+ }
private:
const ::std::unique_ptr<BufferedPcm> &pcm_;
@@ -209,54 +208,51 @@
::std::unique_ptr<BufferedSolenoid> drivetrain_left_, drivetrain_right_;
::std::unique_ptr<BufferedSolenoid> rollers_front_, rollers_back_;
- ::std::unique_ptr<DigitalInput> pressure_switch_;
- ::std::unique_ptr<Relay> compressor_relay_;
+ ::std::unique_ptr<::frc::DigitalInput> pressure_switch_;
+ ::std::unique_ptr<::frc::Relay> compressor_relay_;
- ::aos::Queue<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
- ::aos::Queue<::y2014_bot3::control_loops::RollersQueue::Output> rollers_;
-
- ::std::atomic<bool> run_{true};
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
+ ::aos::Fetcher<::y2014_bot3::control_loops::RollersQueue::Output> rollers_;
};
// Writes out rollers voltages.
-class RollersWriter : public LoopOutputHandler {
+class RollersWriter : public LoopOutputHandler<
+ ::y2014_bot3::control_loops::RollersQueue::Output> {
public:
RollersWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler(event_loop) {}
+ : ::frc971::wpilib::LoopOutputHandler<
+ ::y2014_bot3::control_loops::RollersQueue::Output>(
+ event_loop, ".y2014_bot3.control_loops.rollers_queue.output") {}
- void set_rollers_front_intake_talon(::std::unique_ptr<Talon> t_left,
- ::std::unique_ptr<Talon> t_right) {
+ void set_rollers_front_intake_talon(::std::unique_ptr<::frc::Talon> t_left,
+ ::std::unique_ptr<::frc::Talon> t_right) {
rollers_front_left_intake_talon_ = ::std::move(t_left);
rollers_front_right_intake_talon_ = ::std::move(t_right);
}
- void set_rollers_back_intake_talon(::std::unique_ptr<Talon> t_left,
- ::std::unique_ptr<Talon> t_right) {
+ void set_rollers_back_intake_talon(::std::unique_ptr<::frc::Talon> t_left,
+ ::std::unique_ptr<::frc::Talon> t_right) {
rollers_back_left_intake_talon_ = ::std::move(t_left);
rollers_back_right_intake_talon_ = ::std::move(t_right);
}
- void set_rollers_low_goal_talon(::std::unique_ptr<Talon> t) {
+ void set_rollers_low_goal_talon(::std::unique_ptr<::frc::Talon> t) {
rollers_low_goal_talon_ = ::std::move(t);
}
private:
- virtual void Read() override {
- ::y2014_bot3::control_loops::rollers_queue.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::y2014_bot3::control_loops::rollers_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- rollers_front_left_intake_talon_->SetSpeed(queue->front_intake_voltage /
+ virtual void Write(const ::y2014_bot3::control_loops::RollersQueue::Output
+ &output) override {
+ LOG_STRUCT(DEBUG, "will output", output);
+ rollers_front_left_intake_talon_->SetSpeed(output.front_intake_voltage /
12.0);
rollers_front_right_intake_talon_->SetSpeed(
- -(queue->front_intake_voltage / 12.0));
- rollers_back_left_intake_talon_->SetSpeed(queue->back_intake_voltage /
+ -(output.front_intake_voltage / 12.0));
+ rollers_back_left_intake_talon_->SetSpeed(output.back_intake_voltage /
12.0);
rollers_back_right_intake_talon_->SetSpeed(
- -(queue->back_intake_voltage / 12.0));
- rollers_low_goal_talon_->SetSpeed(queue->low_goal_voltage / 12.0);
+ -(output.back_intake_voltage / 12.0));
+ rollers_low_goal_talon_->SetSpeed(output.low_goal_voltage / 12.0);
}
virtual void Stop() override {
@@ -268,105 +264,79 @@
rollers_low_goal_talon_->SetDisabled();
}
- ::std::unique_ptr<Talon> rollers_front_left_intake_talon_,
+ ::std::unique_ptr<::frc::Talon> rollers_front_left_intake_talon_,
rollers_back_left_intake_talon_, rollers_front_right_intake_talon_,
rollers_back_right_intake_talon_, rollers_low_goal_talon_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
public:
- ::std::unique_ptr<Encoder> make_encoder(int index) {
- return make_unique<Encoder>(10 + index * 2, 11 + index * 2, false,
- Encoder::k4X);
+ ::std::unique_ptr<::frc::Encoder> make_encoder(int index) {
+ return make_unique<::frc::Encoder>(10 + index * 2, 11 + index * 2, false,
+ ::frc::Encoder::k4X);
}
void Run() override {
- ::aos::InitNRT();
- ::aos::SetCurrentThreadName("StartCompetition");
+ // Thread 1.
+ ::aos::ShmEventLoop joystick_sender_event_loop;
+ ::frc971::wpilib::JoystickSender joystick_sender(
+ &joystick_sender_event_loop);
+ AddLoop(&joystick_sender_event_loop);
- ::aos::ShmEventLoop event_loop;
+ // Thread 2.
+ ::aos::ShmEventLoop pdp_fetcher_event_loop;
+ ::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
+ AddLoop(&pdp_fetcher_event_loop);
- JoystickSender joystick_sender(&event_loop);
- ::std::thread joystick_thread(::std::ref(joystick_sender));
-
- ::frc971::wpilib::PDPFetcher pdp_fetcher(&event_loop);
- ::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
-
- // TODO(comran): IO ports are placeholders at the moment, so match them to
- // the robot before turning on.
-
+ // Thread 3.
// Sensors
- SensorReader reader(&event_loop);
- reader.set_drivetrain_left_encoder(make_encoder(4));
- reader.set_drivetrain_right_encoder(make_encoder(5));
+ ::aos::ShmEventLoop sensor_reader_event_loop;
+ SensorReader sensor_reader(&sensor_reader_event_loop);
+ sensor_reader.set_drivetrain_left_encoder(make_encoder(4));
+ sensor_reader.set_drivetrain_right_encoder(make_encoder(5));
+ AddLoop(&sensor_reader_event_loop);
- ::std::thread reader_thread(::std::ref(reader));
- GyroSender gyro_sender(&event_loop);
- ::std::thread gyro_thread(::std::ref(gyro_sender));
+ // Thread 4.
+ ::aos::ShmEventLoop gyro_event_loop;
+ GyroSender gyro_sender(&gyro_event_loop);
+ AddLoop(&gyro_event_loop);
+ // Thread 5.
// Outputs
- ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&event_loop);
+ ::aos::ShmEventLoop output_event_loop;
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
drivetrain_writer.set_left_controller0(
- ::std::unique_ptr<Talon>(new Talon(5)), true);
+ ::std::unique_ptr<::frc::Talon>(new ::frc::Talon(5)), true);
drivetrain_writer.set_right_controller0(
- ::std::unique_ptr<Talon>(new Talon(2)), false);
- ::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
+ ::std::unique_ptr<::frc::Talon>(new ::frc::Talon(2)), false);
- RollersWriter rollers_writer(&event_loop);
+ RollersWriter rollers_writer(&output_event_loop);
rollers_writer.set_rollers_front_intake_talon(
- ::std::unique_ptr<Talon>(new Talon(3)),
- ::std::unique_ptr<Talon>(new Talon(7)));
+ ::std::unique_ptr<::frc::Talon>(new ::frc::Talon(3)),
+ ::std::unique_ptr<::frc::Talon>(new ::frc::Talon(7)));
rollers_writer.set_rollers_back_intake_talon(
- ::std::unique_ptr<Talon>(new Talon(1)),
- ::std::unique_ptr<Talon>(new Talon(6)));
+ ::std::unique_ptr<::frc::Talon>(new ::frc::Talon(1)),
+ ::std::unique_ptr<::frc::Talon>(new ::frc::Talon(6)));
rollers_writer.set_rollers_low_goal_talon(
- ::std::unique_ptr<Talon>(new Talon(4)));
- ::std::thread rollers_writer_thread(::std::ref(rollers_writer));
+ ::std::unique_ptr<::frc::Talon>(new ::frc::Talon(4)));
+ AddLoop(&output_event_loop);
+ // Thread 6.
+ ::aos::ShmEventLoop solenoid_writer_event_loop;
::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
new ::frc971::wpilib::BufferedPcm());
- SolenoidWriter solenoid_writer(pcm);
+ SolenoidWriter solenoid_writer(&solenoid_writer_event_loop, pcm);
solenoid_writer.set_drivetrain_left(pcm->MakeSolenoid(6));
solenoid_writer.set_drivetrain_right(pcm->MakeSolenoid(5));
solenoid_writer.set_rollers_front(pcm->MakeSolenoid(2));
solenoid_writer.set_rollers_back(pcm->MakeSolenoid(4));
// Don't change the following IDs.
- solenoid_writer.set_pressure_switch(make_unique<DigitalInput>(9));
- solenoid_writer.set_compressor_relay(make_unique<Relay>(0));
- ::std::thread solenoid_thread(::std::ref(solenoid_writer));
+ solenoid_writer.set_pressure_switch(make_unique<::frc::DigitalInput>(9));
+ solenoid_writer.set_compressor_relay(make_unique<::frc::Relay>(0));
+ AddLoop(&solenoid_writer_event_loop);
- // Wait forever. Not much else to do...
- while (true) {
- const int r = select(0, nullptr, nullptr, nullptr, nullptr);
- if (r != 0) {
- PLOG(WARNING, "infinite select failed");
- } else {
- PLOG(WARNING, "infinite select succeeded??\n");
- }
- }
-
- LOG(ERROR, "Exiting WPILibRobot\n");
-
- joystick_sender.Quit();
- joystick_thread.join();
- pdp_fetcher.Quit();
- pdp_fetcher_thread.join();
- reader.Quit();
- reader_thread.join();
- gyro_sender.Quit();
- gyro_thread.join();
-
- drivetrain_writer.Quit();
- drivetrain_writer_thread.join();
-
- rollers_writer.Quit();
- rollers_writer_thread.join();
-
- solenoid_writer.Quit();
- solenoid_thread.join();
-
- ::aos::Cleanup();
+ RunLoops();
}
};