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/frc971/wpilib/ADIS16448.cc b/frc971/wpilib/ADIS16448.cc
index 75f082d..38d194d 100644
--- a/frc971/wpilib/ADIS16448.cc
+++ b/frc971/wpilib/ADIS16448.cc
@@ -139,6 +139,17 @@
dio1_->RequestInterrupts();
dio1_->SetUpSourceEdge(true, false);
+
+ // NI's SPI driver defaults to SCHED_OTHER. Find it's PID with ps, and change
+ // it to a RT priority of 33.
+ PCHECK(
+ system("ps -ef | grep '\\[spi0\\]' | awk '{print $1}' | xargs chrt -f -p "
+ "33") == 0);
+
+ event_loop_->set_name("IMU");
+ event_loop_->SetRuntimeRealtimePriority(33);
+
+ event_loop_->OnRun([this]() { DoRun(); });
}
void ADIS16448::SetDummySPI(frc::SPI::Port port) {
@@ -155,7 +166,7 @@
}
void ADIS16448::InitializeUntilSuccessful() {
- while (run_ && !Initialize()) {
+ while (event_loop_->is_running() && !Initialize()) {
if (reset_) {
reset_->Set(false);
// Datasheet says this needs to be at least 10 us long, so 10 ms is
@@ -170,19 +181,9 @@
}
}
LOG(INFO, "IMU initialized successfully\n");
-
- ::aos::SetCurrentThreadRealtimePriority(33);
}
-void ADIS16448::operator()() {
- // NI's SPI driver defaults to SCHED_OTHER. Find it's PID with ps, and change
- // it to a RT priority of 33.
- PCHECK(
- system("ps -ef | grep '\\[spi0\\]' | awk '{print $1}' | xargs chrt -f -p "
- "33") == 0);
-
- ::aos::SetCurrentThreadName("IMU");
-
+void ADIS16448::DoRun() {
InitializeUntilSuccessful();
// Rounded to approximate the 204.8 Hz.
@@ -193,8 +194,10 @@
zeroing::Averager<double, 6 * kImuSendRate> average_gyro_z;
bool got_an_interrupt = false;
- while (run_) {
+ while (event_loop_->is_running()) {
{
+ // Wait for an interrupt. (This prevents us from going to sleep in the
+ // event loop like we normally would)
const frc::InterruptableSensorBase::WaitResult result =
dio1_->WaitForInterrupt(0.1, !got_an_interrupt);
if (result == frc::InterruptableSensorBase::kTimeout) {
diff --git a/frc971/wpilib/ADIS16448.h b/frc971/wpilib/ADIS16448.h
index 5af6a4f..6aeac9d 100644
--- a/frc971/wpilib/ADIS16448.h
+++ b/frc971/wpilib/ADIS16448.h
@@ -43,25 +43,21 @@
// Sets the reset line for the IMU to use for error recovery.
void set_reset(frc::DigitalOutput *output) { reset_ = output; }
- // For ::std::thread to call.
- //
- // Initializes the sensor and then loops until Quit() is called taking
- // readings.
- void operator()();
-
// Sets a function to be called immediately after each time this class uses
// the SPI bus. This is a good place to do other things on the bus.
void set_spi_idle_callback(std::function<void()> spi_idle_callback) {
spi_idle_callback_ = std::move(spi_idle_callback);
}
- void Quit() { run_ = false; }
-
double gyro_x_zeroed_offset() const { return gyro_x_zeroed_offset_; }
double gyro_y_zeroed_offset() const { return gyro_y_zeroed_offset_; }
double gyro_z_zeroed_offset() const { return gyro_z_zeroed_offset_; }
private:
+ // Initializes the sensor and then loops until Quit() is called taking
+ // readings.
+ void DoRun();
+
// Try to initialize repeatedly as long as we're supposed to be running.
void InitializeUntilSuccessful();
@@ -104,7 +100,6 @@
frc::DigitalOutput *reset_ = nullptr;
std::function<void()> spi_idle_callback_ = []() {};
- ::std::atomic<bool> run_{true};
// The averaged values of the gyro over 6 seconds after power up.
bool gyros_are_zeroed_ = false;
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index c6e72a4..870e655 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -118,6 +118,13 @@
],
)
+queue_library(
+ name = "loop_output_handler_test_queue",
+ srcs = [
+ "loop_output_handler_test.q",
+ ],
+)
+
cc_library(
name = "loop_output_handler",
srcs = [
@@ -136,6 +143,21 @@
],
)
+cc_test(
+ name = "loop_output_handler_test",
+ srcs = [
+ "loop_output_handler_test.cc",
+ ],
+ deps = [
+ ":loop_output_handler",
+ ":loop_output_handler_test_queue",
+ "//aos/events:simulated_event_loop",
+ "//aos/logging:queue_logging",
+ "//aos/testing:googletest",
+ "//aos/testing:test_logging",
+ ],
+)
+
cc_library(
name = "joystick_sender",
srcs = [
diff --git a/frc971/wpilib/drivetrain_writer.cc b/frc971/wpilib/drivetrain_writer.cc
index 8d388dc..fdbc028 100644
--- a/frc971/wpilib/drivetrain_writer.cc
+++ b/frc971/wpilib/drivetrain_writer.cc
@@ -10,27 +10,23 @@
namespace frc971 {
namespace wpilib {
-void DrivetrainWriter::Write() {
- auto &queue = ::frc971::control_loops::drivetrain_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- left_controller0_->SetSpeed(SafeSpeed(reversed_left0_, queue->left_voltage));
+void DrivetrainWriter::Write(
+ const ::frc971::control_loops::DrivetrainQueue::Output &output) {
+ LOG_STRUCT(DEBUG, "will output", output);
+ left_controller0_->SetSpeed(SafeSpeed(reversed_left0_, output.left_voltage));
right_controller0_->SetSpeed(
- SafeSpeed(reversed_right0_, queue->right_voltage));
+ SafeSpeed(reversed_right0_, output.right_voltage));
if (left_controller1_) {
left_controller1_->SetSpeed(
- SafeSpeed(reversed_left1_, queue->left_voltage));
+ SafeSpeed(reversed_left1_, output.left_voltage));
}
if (right_controller1_) {
right_controller1_->SetSpeed(
- SafeSpeed(reversed_right1_, queue->right_voltage));
+ SafeSpeed(reversed_right1_, output.right_voltage));
}
}
-void DrivetrainWriter::Read() {
- ::frc971::control_loops::drivetrain_queue.output.FetchAnother();
-}
-
void DrivetrainWriter::Stop() {
LOG(WARNING, "drivetrain output too old\n");
left_controller0_->SetDisabled();
diff --git a/frc971/wpilib/drivetrain_writer.h b/frc971/wpilib/drivetrain_writer.h
index ea3df46..9397230 100644
--- a/frc971/wpilib/drivetrain_writer.h
+++ b/frc971/wpilib/drivetrain_writer.h
@@ -3,16 +3,20 @@
#include "aos/commonmath.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/wpilib/ahal/PWM.h"
#include "frc971/wpilib/loop_output_handler.h"
namespace frc971 {
namespace wpilib {
-class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
+class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler<
+ ::frc971::control_loops::DrivetrainQueue::Output> {
public:
DrivetrainWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler(event_loop) {}
+ : ::frc971::wpilib::LoopOutputHandler<
+ ::frc971::control_loops::DrivetrainQueue::Output>(
+ event_loop, ".frc971.control_loops.drivetrain_queue.output") {}
void set_left_controller0(::std::unique_ptr<::frc::PWM> t, bool reversed) {
left_controller0_ = ::std::move(t);
@@ -35,8 +39,8 @@
}
private:
- void Write() override;
- void Read() override;
+ void Write(
+ const ::frc971::control_loops::DrivetrainQueue::Output &output) override;
void Stop() override;
double SafeSpeed(bool reversed, double voltage) {
diff --git a/frc971/wpilib/gyro_sender.cc b/frc971/wpilib/gyro_sender.cc
index 99dc735..b1edbd2 100644
--- a/frc971/wpilib/gyro_sender.cc
+++ b/frc971/wpilib/gyro_sender.cc
@@ -13,7 +13,6 @@
#include "aos/logging/queue_logging.h"
#include "aos/robot_state/robot_state.q.h"
#include "aos/time/time.h"
-#include "aos/util/phased_loop.h"
#include "frc971/queues/gyro.q.h"
#include "frc971/zeroing/averager.h"
@@ -36,124 +35,117 @@
PCHECK(
system("ps -ef | grep '\\[spi0\\]' | awk '{print $1}' | xargs chrt -f -p "
"33") == 0);
+ event_loop_->set_name("Gyro");
+ event_loop_->SetRuntimeRealtimePriority(33);
+
+ // TODO(austin): This should be synchronized with SensorReader... Pull out
+ // the sync logic and re-use it here.
+ event_loop_->AddPhasedLoop([this](int iterations) { Loop(iterations); },
+ ::aos::time::FromRate(kReadingRate),
+ chrono::milliseconds(4));
}
-void GyroSender::operator()() {
- ::aos::SetCurrentThreadName("Gyro");
+void GyroSender::Loop(const int iterations) {
+ switch (state_) {
+ case State::INITIALIZING: {
+ const monotonic_clock::time_point monotonic_now =
+ event_loop_->monotonic_now();
+ if (last_initialize_time_ + chrono::milliseconds(50) < monotonic_now) {
+ if (gyro_.InitializeGyro()) {
+ state_ = State::RUNNING;
+ LOG(INFO, "gyro initialized successfully\n");
- // Try to initialize repeatedly as long as we're supposed to be running.
- while (run_ && !gyro_.InitializeGyro()) {
- ::std::this_thread::sleep_for(::std::chrono::milliseconds(50));
- }
- LOG(INFO, "gyro initialized successfully\n");
-
- auto message = uid_sender_.MakeMessage();
- message->uid = gyro_.ReadPartID();
- LOG_STRUCT(INFO, "gyro ID", *message);
- message.Send();
-
- // In radians, ready to send out.
- double angle = 0;
-
- int startup_cycles_left = 2 * kReadingRate;
-
- zeroing::Averager<double, 6 * kReadingRate> zeroing_data;
- bool zeroed = false;
- double zero_offset = 0;
-
- ::aos::SetCurrentThreadRealtimePriority(33);
-
- ::aos::time::PhasedLoop phased_loop(::aos::time::FromRate(kReadingRate),
- event_loop_->monotonic_now(),
- chrono::milliseconds(4));
- // How many timesteps the next reading represents.
- int number_readings = 0;
-
- ::aos::SetCurrentThreadRealtimePriority(33);
-
- while (run_) {
- number_readings += phased_loop.SleepUntilNext();
-
- const uint32_t result = gyro_.GetReading();
- if (result == 0) {
- LOG(WARNING, "normal gyro read failed\n");
- continue;
- }
- switch (gyro_.ExtractStatus(result)) {
- case 0:
- LOG(WARNING, "gyro says data is bad\n");
- continue;
- case 1:
- break;
- default:
- LOG(WARNING, "gyro gave weird status 0x%" PRIx8 "\n",
- gyro_.ExtractStatus(result));
- continue;
- }
- if (gyro_.ExtractErrors(result) != 0) {
- const uint8_t errors = gyro_.ExtractErrors(result);
- if (errors & (1 << 6)) {
- LOG(WARNING, "gyro gave PLL error\n");
+ {
+ auto message = uid_sender_.MakeMessage();
+ message->uid = gyro_.ReadPartID();
+ LOG_STRUCT(INFO, "gyro ID", *message);
+ message.Send();
+ }
+ }
+ last_initialize_time_ = monotonic_now;
}
- if (errors & (1 << 5)) {
- LOG(WARNING, "gyro gave quadrature error\n");
+ } break;
+ case State::RUNNING: {
+ const uint32_t result = gyro_.GetReading();
+ if (result == 0) {
+ LOG(WARNING, "normal gyro read failed\n");
+ return;
}
- if (errors & (1 << 4)) {
- LOG(WARNING, "gyro gave non-volatile memory error\n");
+ switch (gyro_.ExtractStatus(result)) {
+ case 0:
+ LOG(WARNING, "gyro says data is bad\n");
+ return;
+ case 1:
+ break;
+ default:
+ LOG(WARNING, "gyro gave weird status 0x%" PRIx8 "\n",
+ gyro_.ExtractStatus(result));
+ return;
}
- if (errors & (1 << 3)) {
- LOG(WARNING, "gyro gave volatile memory error\n");
+ if (gyro_.ExtractErrors(result) != 0) {
+ const uint8_t errors = gyro_.ExtractErrors(result);
+ if (errors & (1 << 6)) {
+ LOG(WARNING, "gyro gave PLL error\n");
+ }
+ if (errors & (1 << 5)) {
+ LOG(WARNING, "gyro gave quadrature error\n");
+ }
+ if (errors & (1 << 4)) {
+ LOG(WARNING, "gyro gave non-volatile memory error\n");
+ }
+ if (errors & (1 << 3)) {
+ LOG(WARNING, "gyro gave volatile memory error\n");
+ }
+ if (errors & (1 << 2)) {
+ LOG(WARNING, "gyro gave power error\n");
+ }
+ if (errors & (1 << 1)) {
+ LOG(WARNING, "gyro gave continuous self-test error\n");
+ }
+ if (errors & 1) {
+ LOG(WARNING, "gyro gave unexpected self-test mode\n");
+ }
+ return;
}
- if (errors & (1 << 2)) {
- LOG(WARNING, "gyro gave power error\n");
- }
- if (errors & (1 << 1)) {
- LOG(WARNING, "gyro gave continuous self-test error\n");
- }
- if (errors & 1) {
- LOG(WARNING, "gyro gave unexpected self-test mode\n");
- }
- continue;
- }
- if (startup_cycles_left > 0) {
- --startup_cycles_left;
- continue;
- }
+ if (startup_cycles_left_ > 0) {
+ --startup_cycles_left_;
+ return;
+ }
- const double angle_rate = gyro_.ExtractAngle(result);
- const double new_angle = angle_rate / static_cast<double>(kReadingRate);
- auto message = gyro_reading_sender_.MakeMessage();
- if (zeroed) {
- angle += (new_angle + zero_offset) * number_readings;
- message->angle = angle;
- message->velocity = angle_rate + zero_offset * kReadingRate;
- LOG_STRUCT(DEBUG, "sending", *message);
- message.Send();
- } else {
- // TODO(brian): Don't break without 6 seconds of standing still before
- // enabling. Ideas:
- // Don't allow driving until we have at least some data?
- // Some kind of indicator light?
- {
- message->angle = new_angle;
- message->velocity = angle_rate;
- LOG_STRUCT(DEBUG, "collected while zeroing", *message);
- message->angle = 0.0;
- message->velocity = 0.0;
+ const double angle_rate = gyro_.ExtractAngle(result);
+ const double new_angle = angle_rate / static_cast<double>(kReadingRate);
+ auto message = gyro_reading_sender_.MakeMessage();
+ if (zeroed_) {
+ angle_ += (new_angle + zero_offset_) * iterations;
+ message->angle = angle_;
+ message->velocity = angle_rate + zero_offset_ * kReadingRate;
+ LOG_STRUCT(DEBUG, "sending", *message);
message.Send();
- }
- zeroing_data.AddData(new_angle);
+ } else {
+ // TODO(brian): Don't break without 6 seconds of standing still before
+ // enabling. Ideas:
+ // Don't allow driving until we have at least some data?
+ // Some kind of indicator light?
+ {
+ message->angle = new_angle;
+ message->velocity = angle_rate;
+ LOG_STRUCT(DEBUG, "collected while zeroing", *message);
+ message->angle = 0.0;
+ message->velocity = 0.0;
+ message.Send();
+ }
+ zeroing_data_.AddData(new_angle);
- joystick_state_fetcher_.Fetch();
- if (joystick_state_fetcher_.get() && joystick_state_fetcher_->enabled &&
- zeroing_data.full()) {
- zero_offset = -zeroing_data.GetAverage();
- LOG(INFO, "total zero offset %f\n", zero_offset);
- zeroed = true;
+ joystick_state_fetcher_.Fetch();
+ if (joystick_state_fetcher_.get() && joystick_state_fetcher_->enabled &&
+ zeroing_data_.full()) {
+ zero_offset_ = -zeroing_data_.GetAverage();
+ LOG(INFO, "total zero offset %f\n", zero_offset_);
+ zeroed_ = true;
+ }
}
- }
- number_readings = 0;
+ } break;
}
}
diff --git a/frc971/wpilib/gyro_sender.h b/frc971/wpilib/gyro_sender.h
index 338ad32..3a43391 100644
--- a/frc971/wpilib/gyro_sender.h
+++ b/frc971/wpilib/gyro_sender.h
@@ -9,6 +9,7 @@
#include "aos/robot_state/robot_state.q.h"
#include "frc971/queues/gyro.q.h"
#include "frc971/wpilib/gyro_interface.h"
+#include "frc971/zeroing/averager.h"
namespace frc971 {
namespace wpilib {
@@ -21,25 +22,37 @@
public:
GyroSender(::aos::EventLoop *event_loop);
- // For ::std::thread to call.
- //
- // Initializes the gyro and then loops until Quit() is called taking readings.
- void operator()();
-
- void Quit() { run_ = false; }
+ enum class State { INITIALIZING, RUNNING };
private:
+ // Initializes the gyro and then loops until Exit() is called on the event
+ // loop, taking readings.
+ void Loop(const int iterations);
+
::aos::EventLoop *event_loop_;
::aos::Fetcher<::aos::JoystickState> joystick_state_fetcher_;
::aos::Sender<::frc971::sensors::Uid> uid_sender_;
::aos::Sender<::frc971::sensors::GyroReading> gyro_reading_sender_;
// Readings per second.
- static const int kReadingRate = 200;
+ static constexpr int kReadingRate = 200;
GyroInterface gyro_;
- ::std::atomic<bool> run_{true};
+ State state_ = State::INITIALIZING;
+
+ // In radians, ready to send out.
+ double angle_ = 0;
+ // Calibrated offset.
+ double zero_offset_ = 0;
+
+ ::aos::monotonic_clock::time_point last_initialize_time_ =
+ ::aos::monotonic_clock::min_time;
+ int startup_cycles_left_ = 2 * kReadingRate;
+
+ zeroing::Averager<double, 6 * kReadingRate> zeroing_data_;
+
+ bool zeroed_ = false;
};
} // namespace wpilib
diff --git a/frc971/wpilib/joystick_sender.cc b/frc971/wpilib/joystick_sender.cc
index a71ab9c..a073066 100644
--- a/frc971/wpilib/joystick_sender.cc
+++ b/frc971/wpilib/joystick_sender.cc
@@ -11,51 +11,57 @@
namespace frc971 {
namespace wpilib {
-void JoystickSender::operator()() {
- frc::DriverStation *const ds = &frc::DriverStation::GetInstance();
- ::aos::SetCurrentThreadName("DSReader");
- uint16_t team_id = ::aos::network::GetTeamNumber();
+JoystickSender::JoystickSender(::aos::EventLoop *event_loop)
+ : event_loop_(event_loop),
+ joystick_state_sender_(
+ event_loop_->MakeSender<::aos::JoystickState>(".aos.joystick_state")),
+ team_id_(::aos::network::GetTeamNumber()) {
- ::aos::SetCurrentThreadRealtimePriority(29);
+ event_loop_->SetRuntimeRealtimePriority(29);
- // TODO(Brian): Fix the potential deadlock when stopping here (condition
- // variable / mutex needs to get exposed all the way out or something).
- while (run_) {
- ds->RunIteration([&]() {
- auto new_state = joystick_state_sender_.MakeMessage();
+ event_loop_->OnRun([this]() {
+ frc::DriverStation *const ds = &frc::DriverStation::GetInstance();
+ ::aos::SetCurrentThreadName("DSReader");
- HAL_MatchInfo match_info;
- auto status = HAL_GetMatchInfo(&match_info);
- if (status == 0) {
- new_state->switch_left = match_info.gameSpecificMessage[0] == 'L' ||
- match_info.gameSpecificMessage[0] == 'l';
- new_state->scale_left = match_info.gameSpecificMessage[1] == 'L' ||
- match_info.gameSpecificMessage[1] == 'l';
- }
+ // TODO(Brian): Fix the potential deadlock when stopping here (condition
+ // variable / mutex needs to get exposed all the way out or something).
+ while (event_loop_->is_running()) {
+ ds->RunIteration([&]() {
+ auto new_state = joystick_state_sender_.MakeMessage();
- new_state->test_mode = ds->IsTestMode();
- new_state->fms_attached = ds->IsFmsAttached();
- new_state->enabled = ds->IsEnabled();
- new_state->autonomous = ds->IsAutonomous();
- new_state->team_id = team_id;
- new_state->fake = false;
-
- for (uint8_t i = 0;
- i < sizeof(new_state->joysticks) / sizeof(::aos::Joystick); ++i) {
- new_state->joysticks[i].buttons = ds->GetStickButtons(i);
- for (int j = 0; j < 6; ++j) {
- new_state->joysticks[i].axis[j] = ds->GetStickAxis(i, j);
+ HAL_MatchInfo match_info;
+ auto status = HAL_GetMatchInfo(&match_info);
+ if (status == 0) {
+ new_state->switch_left = match_info.gameSpecificMessage[0] == 'L' ||
+ match_info.gameSpecificMessage[0] == 'l';
+ new_state->scale_left = match_info.gameSpecificMessage[1] == 'L' ||
+ match_info.gameSpecificMessage[1] == 'l';
}
- if (ds->GetStickPOVCount(i) > 0) {
- new_state->joysticks[i].pov = ds->GetStickPOV(i, 0);
+
+ new_state->test_mode = ds->IsTestMode();
+ new_state->fms_attached = ds->IsFmsAttached();
+ new_state->enabled = ds->IsEnabled();
+ new_state->autonomous = ds->IsAutonomous();
+ new_state->team_id = team_id_;
+ new_state->fake = false;
+
+ for (uint8_t i = 0;
+ i < sizeof(new_state->joysticks) / sizeof(::aos::Joystick); ++i) {
+ new_state->joysticks[i].buttons = ds->GetStickButtons(i);
+ for (int j = 0; j < 6; ++j) {
+ new_state->joysticks[i].axis[j] = ds->GetStickAxis(i, j);
+ }
+ if (ds->GetStickPOVCount(i) > 0) {
+ new_state->joysticks[i].pov = ds->GetStickPOV(i, 0);
+ }
+ LOG_STRUCT(DEBUG, "joystick_state", *new_state);
}
- LOG_STRUCT(DEBUG, "joystick_state", *new_state);
- }
- if (!new_state.Send()) {
- LOG(WARNING, "sending joystick_state failed\n");
- }
- });
- }
+ if (!new_state.Send()) {
+ LOG(WARNING, "sending joystick_state failed\n");
+ }
+ });
+ }
+ });
}
} // namespace wpilib
diff --git a/frc971/wpilib/joystick_sender.h b/frc971/wpilib/joystick_sender.h
index a758173..34c6bf4 100644
--- a/frc971/wpilib/joystick_sender.h
+++ b/frc971/wpilib/joystick_sender.h
@@ -11,18 +11,12 @@
class JoystickSender {
public:
- JoystickSender(::aos::EventLoop *event_loop)
- : event_loop_(event_loop),
- joystick_state_sender_(event_loop_->MakeSender<::aos::JoystickState>(
- ".aos.joystick_state")) {}
- void operator()();
-
- void Quit() { run_ = false; }
+ JoystickSender(::aos::EventLoop *event_loop);
private:
::aos::EventLoop *event_loop_;
::aos::Sender<::aos::JoystickState> joystick_state_sender_;
- ::std::atomic<bool> run_{true};
+ const uint16_t team_id_;
};
} // namespace wpilib
diff --git a/frc971/wpilib/loop_output_handler.cc b/frc971/wpilib/loop_output_handler.cc
index 6be855e..5039fc3 100644
--- a/frc971/wpilib/loop_output_handler.cc
+++ b/frc971/wpilib/loop_output_handler.cc
@@ -1,85 +1 @@
#include "frc971/wpilib/loop_output_handler.h"
-
-#include <sys/timerfd.h>
-
-#include <chrono>
-#include <functional>
-#include <thread>
-
-#include "aos/events/event-loop.h"
-#include "aos/init.h"
-#include "aos/robot_state/robot_state.q.h"
-
-namespace frc971 {
-namespace wpilib {
-
-namespace chrono = ::std::chrono;
-
-LoopOutputHandler::LoopOutputHandler(::aos::EventLoop *event_loop,
- ::std::chrono::nanoseconds timeout)
- : event_loop_(event_loop),
- joystick_state_fetcher_(event_loop_->MakeFetcher<::aos::JoystickState>(
- ".aos.joystick_state")),
- watchdog_(this, timeout) {}
-
-void LoopOutputHandler::operator()() {
- ::std::thread watchdog_thread(::std::ref(watchdog_));
- ::aos::SetCurrentThreadName("OutputHandler");
-
- ::aos::SetCurrentThreadRealtimePriority(30);
- while (run_) {
- no_joystick_state_.Print();
- fake_joystick_state_.Print();
- Read();
- joystick_state_fetcher_.Fetch();
- if (!joystick_state_fetcher_.get()) {
- LOG_INTERVAL(no_joystick_state_);
- continue;
- }
- if (joystick_state_fetcher_->fake) {
- LOG_INTERVAL(fake_joystick_state_);
- continue;
- }
-
- watchdog_.Reset();
- Write();
- }
-
- Stop();
-
- watchdog_.Quit();
- watchdog_thread.join();
-}
-
-LoopOutputHandler::Watchdog::Watchdog(LoopOutputHandler *handler,
- ::std::chrono::nanoseconds timeout)
- : handler_(handler),
- timeout_(timeout),
- timerfd_(timerfd_create(CLOCK_MONOTONIC, 0)) {
- if (timerfd_.get() == -1) {
- PLOG(FATAL, "timerfd_create(CLOCK_MONOTONIC, 0)");
- }
-}
-
-void LoopOutputHandler::Watchdog::operator()() {
- ::aos::SetCurrentThreadRealtimePriority(35);
- ::aos::SetCurrentThreadName("OutputWatchdog");
- uint8_t buf[8];
- while (run_) {
- PCHECK(read(timerfd_.get(), buf, sizeof(buf)));
- handler_->Stop();
- }
-}
-
-void LoopOutputHandler::Watchdog::Reset() {
- itimerspec value = itimerspec();
- value.it_value.tv_sec = chrono::duration_cast<chrono::seconds>(timeout_).count();
- value.it_value.tv_nsec =
- chrono::duration_cast<chrono::nanoseconds>(
- timeout_ - chrono::seconds(value.it_value.tv_sec))
- .count();
- PCHECK(timerfd_settime(timerfd_.get(), 0, &value, nullptr));
-}
-
-} // namespace wpilib
-} // namespace frc971
diff --git a/frc971/wpilib/loop_output_handler.h b/frc971/wpilib/loop_output_handler.h
index ff8415f..3c95408 100644
--- a/frc971/wpilib/loop_output_handler.h
+++ b/frc971/wpilib/loop_output_handler.h
@@ -21,24 +21,37 @@
// The intended use is to have a subclass for each loop which implements the
// pure virtual methods and is then run in a separate thread. The operator()
// loops writing values until Quit() is called.
+template <typename T>
class LoopOutputHandler {
public:
LoopOutputHandler(
- ::aos::EventLoop *event_loop,
- ::std::chrono::nanoseconds timeout = ::std::chrono::milliseconds(100));
+ ::aos::EventLoop *event_loop, const ::std::string &name,
+ ::std::chrono::nanoseconds timeout = ::std::chrono::milliseconds(100))
+ : event_loop_(event_loop), timeout_(timeout) {
+ event_loop_->SetRuntimeRealtimePriority(30);
+ // TODO(austin): Name thread.
+ event_loop_->MakeWatcher(name, [this](const T &t) {
+ // Push the watchdog out a bit further.
+ timer_handler_->Setup(event_loop_->monotonic_now() + timeout_);
+ Write(t);
+ });
- void Quit() { run_ = false; }
+ // TODO(austin): Set name.
+ timer_handler_ = event_loop_->AddTimer([this]() { Stop(); });
- void operator()();
+ event_loop_->OnRun([this, timeout]() {
+ timer_handler_->Setup(event_loop_->monotonic_now() + timeout_);
+ });
+ }
+
+ void Quit() { event_loop_->Exit(); }
+
+ // Note, all subclasses should call Stop.
+ virtual ~LoopOutputHandler() {}
protected:
- // Read a message from the appropriate queue.
- // This must block.
- virtual void Read() = 0;
-
// Send the output from the appropriate queue to the hardware.
- // Read() will always be called at least once before per invocation of this.
- virtual void Write() = 0;
+ virtual void Write(const T &t) = 0;
// Stop all outputs. This will be called in a separate thread (if at all).
// The subclass implementation should handle any appropriate logging.
@@ -50,46 +63,9 @@
::aos::EventLoop *event_loop() { return event_loop_; }
private:
- // The thread that actually contains a timerfd to implement timeouts. The
- // idea is to have a timerfd that is repeatedly set to the timeout expiration
- // in the future so it will never actually expire unless it is not reset for
- // too long.
- //
- // This class nicely encapsulates the raw fd and manipulating it. Its
- // operator() loops until Quit() is called, calling Stop() on its associated
- // LoopOutputHandler whenever the timerfd expires.
- class Watchdog {
- public:
- Watchdog(LoopOutputHandler *handler, ::std::chrono::nanoseconds timeout);
-
- void operator()();
-
- void Reset();
-
- void Quit() { run_ = false; }
-
- private:
- LoopOutputHandler *const handler_;
-
- const ::std::chrono::nanoseconds timeout_;
-
- ::aos::ScopedFD timerfd_;
-
- ::std::atomic<bool> run_{true};
- };
-
::aos::EventLoop *event_loop_;
- ::aos::Fetcher<::aos::JoystickState> joystick_state_fetcher_;
- Watchdog watchdog_;
-
- ::std::atomic<bool> run_{true};
-
- ::aos::util::SimpleLogInterval no_joystick_state_ =
- ::aos::util::SimpleLogInterval(::std::chrono::milliseconds(500), INFO,
- "no joystick state -> not outputting");
- ::aos::util::SimpleLogInterval fake_joystick_state_ =
- ::aos::util::SimpleLogInterval(::std::chrono::milliseconds(500), DEBUG,
- "fake joystick state -> not outputting");
+ const ::std::chrono::nanoseconds timeout_;
+ ::aos::TimerHandler *timer_handler_;
};
} // namespace wpilib
diff --git a/frc971/wpilib/loop_output_handler_test.cc b/frc971/wpilib/loop_output_handler_test.cc
new file mode 100644
index 0000000..8a7e903
--- /dev/null
+++ b/frc971/wpilib/loop_output_handler_test.cc
@@ -0,0 +1,113 @@
+#include "frc971/wpilib/loop_output_handler.h"
+
+#include <chrono>
+
+#include "gtest/gtest.h"
+
+#include "aos/events/simulated-event-loop.h"
+#include "aos/logging/logging.h"
+#include "aos/logging/queue_logging.h"
+#include "aos/testing/test_logging.h"
+#include "aos/time/time.h"
+#include "frc971/wpilib/loop_output_handler_test.q.h"
+
+namespace frc971 {
+namespace wpilib {
+namespace testing {
+namespace {
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+} // namespace
+
+class LoopOutputHandlerTest : public ::testing::Test {
+ public:
+ LoopOutputHandlerTest()
+ : ::testing::Test(),
+ loop_output_hander_event_loop_(event_loop_factory_.MakeEventLoop()),
+ test_event_loop_(event_loop_factory_.MakeEventLoop()) {
+ ::aos::testing::EnableTestLogging();
+ }
+
+ ::aos::SimulatedEventLoopFactory event_loop_factory_;
+ ::std::unique_ptr<::aos::EventLoop> loop_output_hander_event_loop_;
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+};
+
+// Test loop output handler which logs and counts.
+class TestLoopOutputHandler
+ : public LoopOutputHandler<LoopOutputHandlerTestOutput> {
+ public:
+ TestLoopOutputHandler(::aos::EventLoop *event_loop, const ::std::string &name)
+ : LoopOutputHandler(event_loop, name) {}
+
+ ~TestLoopOutputHandler() { Stop(); }
+
+ int count() const { return count_; }
+
+ ::aos::monotonic_clock::time_point last_time() const { return last_time_; }
+ ::aos::monotonic_clock::time_point stop_time() const { return stop_time_; }
+
+ protected:
+ void Write(const LoopOutputHandlerTestOutput &output) override {
+ LOG_STRUCT(DEBUG, "output", output);
+ ++count_;
+ last_time_ = event_loop()->monotonic_now();
+ }
+
+ void Stop() override {
+ stop_time_ = event_loop()->monotonic_now();
+ LOG(DEBUG, "Stopping\n");
+ }
+
+ private:
+ int count_ = 0;
+
+ ::aos::monotonic_clock::time_point last_time_ =
+ ::aos::monotonic_clock::min_time;
+ ::aos::monotonic_clock::time_point stop_time_ =
+ ::aos::monotonic_clock::min_time;
+};
+
+// Test that the watchdog calls Stop at the right time.
+TEST_F(LoopOutputHandlerTest, WatchdogTest) {
+ TestLoopOutputHandler loop_output(loop_output_hander_event_loop_.get(),
+ ".test");
+
+ ::aos::Sender<LoopOutputHandlerTestOutput> output_sender =
+ test_event_loop_->MakeSender<LoopOutputHandlerTestOutput>(".test");
+
+ const monotonic_clock::time_point start_time =
+ test_event_loop_->monotonic_now();
+
+ int count = 0;
+ // Send outputs for 1 second.
+ ::aos::TimerHandler *timer_handle = test_event_loop_->AddTimer(
+ [this, &start_time, &output_sender, &loop_output, &count]() {
+ EXPECT_EQ(count, loop_output.count());
+ if (test_event_loop_->monotonic_now() <
+ start_time + chrono::seconds(1)) {
+ auto output = output_sender.MakeMessage();
+ output->voltage = 5.0;
+ EXPECT_TRUE(output.Send());
+
+ ++count;
+ }
+ LOG(INFO, "Ping\n");
+ });
+
+ // Kick off the ping timer handler.
+ test_event_loop_->OnRun([this, &timer_handle]() {
+ timer_handle->Setup(test_event_loop_->monotonic_now(),
+ chrono::milliseconds(5));
+ });
+
+ event_loop_factory_.RunFor(chrono::seconds(2));
+
+ // Confirm the watchdog
+ EXPECT_EQ(loop_output.stop_time(),
+ loop_output.last_time() + chrono::milliseconds(100));
+}
+
+} // namespace testing
+} // namespace wpilib
+} // namespace frc971
diff --git a/frc971/wpilib/loop_output_handler_test.q b/frc971/wpilib/loop_output_handler_test.q
new file mode 100644
index 0000000..81336ef
--- /dev/null
+++ b/frc971/wpilib/loop_output_handler_test.q
@@ -0,0 +1,6 @@
+package frc971.wpilib;
+
+// Test output message.
+message LoopOutputHandlerTestOutput {
+ double voltage;
+};
diff --git a/frc971/wpilib/pdp_fetcher.cc b/frc971/wpilib/pdp_fetcher.cc
index 01c9613..98da761 100644
--- a/frc971/wpilib/pdp_fetcher.cc
+++ b/frc971/wpilib/pdp_fetcher.cc
@@ -5,41 +5,42 @@
#include "aos/events/event-loop.h"
#include "aos/init.h"
#include "aos/logging/queue_logging.h"
-#include "aos/util/phased_loop.h"
#include "frc971/wpilib/ahal/PowerDistributionPanel.h"
#include "frc971/wpilib/pdp_values.q.h"
namespace frc971 {
namespace wpilib {
-void PDPFetcher::operator()() {
- ::aos::SetCurrentThreadName("PDPFetcher");
- ::std::unique_ptr<frc::PowerDistributionPanel> pdp(
- new frc::PowerDistributionPanel());
+namespace chrono = ::std::chrono;
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(20),
- ::aos::monotonic_clock::now(),
- ::std::chrono::milliseconds(4));
+PDPFetcher::PDPFetcher(::aos::EventLoop *event_loop)
+ : event_loop_(event_loop),
+ pdp_values_sender_(
+ event_loop_->MakeSender<::frc971::PDPValues>(".frc971.pdp_values")),
+ pdp_(new frc::PowerDistributionPanel()) {
+ event_loop_->set_name("PDPFetcher");
- // TODO(austin): Event loop instead of while loop.
- while (true) {
- {
- const int iterations = phased_loop.SleepUntilNext();
- if (iterations != 1) {
- LOG(DEBUG, "PDPFetcher skipped %d iterations\n", iterations - 1);
- }
- }
- auto message = pdp_values_sender_.MakeMessage();
- message->voltage = pdp->GetVoltage();
- message->temperature = pdp->GetTemperature();
- message->power = pdp->GetTotalPower();
- for (int i = 0; i < 16; ++i) {
- message->currents[i] = pdp->GetCurrent(i);
- }
- LOG_STRUCT(DEBUG, "got", *message);
- if (!message.Send()) {
- LOG(WARNING, "sending pdp values failed\n");
- }
+ // SCHED_OTHER on purpose.
+ event_loop_->AddPhasedLoop([this](int iterations) { Loop(iterations); },
+ chrono::milliseconds(20), chrono::milliseconds(4));
+}
+
+PDPFetcher::~PDPFetcher() {}
+
+void PDPFetcher::Loop(int iterations) {
+ if (iterations != 1) {
+ LOG(DEBUG, "PDPFetcher skipped %d iterations\n", iterations - 1);
+ }
+ auto message = pdp_values_sender_.MakeMessage();
+ message->voltage = pdp_->GetVoltage();
+ message->temperature = pdp_->GetTemperature();
+ message->power = pdp_->GetTotalPower();
+ for (int i = 0; i < 16; ++i) {
+ message->currents[i] = pdp_->GetCurrent(i);
+ }
+ LOG_STRUCT(DEBUG, "got", *message);
+ if (!message.Send()) {
+ LOG(WARNING, "sending pdp values failed\n");
}
}
diff --git a/frc971/wpilib/pdp_fetcher.h b/frc971/wpilib/pdp_fetcher.h
index db783f6..fd05d67 100644
--- a/frc971/wpilib/pdp_fetcher.h
+++ b/frc971/wpilib/pdp_fetcher.h
@@ -7,29 +7,28 @@
#include "aos/events/event-loop.h"
#include "frc971/wpilib/pdp_values.q.h"
+namespace frc {
+class PowerDistributionPanel;
+} // namespace frc
+
namespace frc971 {
namespace wpilib {
-// Handles fetching values from the PDP. This is slow, so it has to happen in a
-// separate thread.
+// Handles fetching values from the PDP.
class PDPFetcher {
public:
- PDPFetcher(::aos::EventLoop *event_loop)
- : event_loop_(event_loop),
- pdp_values_sender_(event_loop_->MakeSender<::frc971::PDPValues>(
- ".frc971.pdp_values")) {}
+ PDPFetcher(::aos::EventLoop *event_loop);
- void Quit() { run_ = false; }
-
- // To be called by a ::std::thread.
- void operator()();
+ ~PDPFetcher();
private:
+ void Loop(int iterations);
+
::aos::EventLoop *event_loop_;
::aos::Sender<::frc971::PDPValues> pdp_values_sender_;
- ::std::atomic<bool> run_{true};
+ ::std::unique_ptr<::frc::PowerDistributionPanel> pdp_;
};
} // namespace wpilib
diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
index 2e0cd0a..209054b 100644
--- a/frc971/wpilib/sensor_reader.cc
+++ b/frc971/wpilib/sensor_reader.cc
@@ -19,12 +19,23 @@
SensorReader::SensorReader(::aos::EventLoop *event_loop)
: event_loop_(event_loop),
robot_state_sender_(
- event_loop_->MakeSender<::aos::RobotState>(".aos.robot_state")) {
+ event_loop_->MakeSender<::aos::RobotState>(".aos.robot_state")),
+ my_pid_(getpid()) {
// Set some defaults. We don't tend to exceed these, so old robots should
// just work with them.
UpdateFastEncoderFilterHz(500000);
UpdateMediumEncoderFilterHz(100000);
ds_ = &::frc::DriverStation::GetInstance();
+
+ event_loop->SetRuntimeRealtimePriority(40);
+
+ // Fill in the no pwm trigger defaults.
+ phased_loop_handler_ =
+ event_loop_->AddPhasedLoop([this](int iterations) { Loop(iterations); },
+ period_, chrono::milliseconds(4));
+
+ event_loop->set_name("SensorReader");
+ event_loop->OnRun([this]() { DoStart(); });
}
void SensorReader::UpdateFastEncoderFilterHz(int hz) {
@@ -82,77 +93,75 @@
}
}
-void SensorReader::operator()() {
- ::aos::SetCurrentThreadName("SensorReader");
-
- int32_t my_pid = getpid();
-
+void SensorReader::DoStart() {
Start();
if (dma_synchronizer_) {
dma_synchronizer_->Start();
}
- const chrono::microseconds period =
+ period_ =
pwm_trigger_ ? chrono::microseconds(5050) : chrono::microseconds(5000);
if (pwm_trigger_) {
LOG(INFO, "Using PWM trigger and a 5.05 ms period\n");
} else {
LOG(INFO, "Defaulting to open loop pwm synchronization\n");
}
- ::aos::time::PhasedLoop phased_loop(
- period, ::aos::monotonic_clock::now(),
+
+ // Now that we are configured, actually fill in the defaults.
+ phased_loop_handler_->set_interval_and_offset(
+ period_,
pwm_trigger_ ? ::std::chrono::milliseconds(3) : chrono::milliseconds(4));
- ::aos::SetCurrentThreadRealtimePriority(40);
- monotonic_clock::time_point last_monotonic_now = monotonic_clock::now();
- while (run_) {
- {
- const int iterations = phased_loop.SleepUntilNext();
- if (iterations != 1) {
- LOG(WARNING, "SensorReader skipped %d iterations\n", iterations - 1);
- }
- }
- const monotonic_clock::time_point monotonic_now = monotonic_clock::now();
+ last_monotonic_now_ = monotonic_clock::now();
+}
- {
- auto new_state = robot_state_sender_.MakeMessage();
- ::frc971::wpilib::PopulateRobotState(new_state.get(), my_pid);
- LOG_STRUCT(DEBUG, "robot_state", *new_state);
- new_state.Send();
- }
- RunIteration();
- if (dma_synchronizer_) {
- dma_synchronizer_->RunIteration();
- RunDmaIteration();
+void SensorReader::Loop(const int iterations) {
+ if (iterations != 1) {
+ LOG(WARNING, "SensorReader skipped %d iterations\n", iterations - 1);
+ }
+
+ const monotonic_clock::time_point monotonic_now =
+ event_loop_->monotonic_now();
+
+ {
+ auto new_state = robot_state_sender_.MakeMessage();
+ ::frc971::wpilib::PopulateRobotState(new_state.get(), my_pid_);
+ LOG_STRUCT(DEBUG, "robot_state", *new_state);
+ new_state.Send();
+ }
+ RunIteration();
+ if (dma_synchronizer_) {
+ dma_synchronizer_->RunIteration();
+ RunDmaIteration();
+ }
+
+ if (pwm_trigger_) {
+ LOG(DEBUG, "PWM wakeup delta: %lld\n",
+ (monotonic_now - last_monotonic_now_).count());
+ last_monotonic_now_ = monotonic_now;
+
+ monotonic_clock::time_point last_tick_timepoint = GetPWMStartTime();
+ if (last_tick_timepoint == monotonic_clock::min_time) {
+ return;
}
- if (pwm_trigger_) {
- LOG(DEBUG, "PWM wakeup delta: %lld\n",
- (monotonic_now - last_monotonic_now).count());
- last_monotonic_now = monotonic_now;
-
- monotonic_clock::time_point last_tick_timepoint = GetPWMStartTime();
- if (last_tick_timepoint == monotonic_clock::min_time) {
- continue;
- }
-
- last_tick_timepoint +=
- (monotonic_now - last_tick_timepoint) / period * period;
- // If it's over 1/2 of a period back in time, that's wrong. Move it
- // forwards to now.
- if (last_tick_timepoint - monotonic_now < -period / 2) {
- last_tick_timepoint += period;
- }
-
- // We should be sampling our sensors to kick off the control cycle 50 uS
- // after the falling edge. This gives us a little bit of buffer for
- // errors in waking up. The PWM cycle starts at the falling edge of the
- // PWM pulse.
- chrono::nanoseconds new_offset = phased_loop.OffsetFromIntervalAndTime(
- period, last_tick_timepoint + chrono::microseconds(50));
-
- phased_loop.set_interval_and_offset(period, new_offset);
+ last_tick_timepoint +=
+ ((monotonic_now - last_tick_timepoint) / period_) * period_;
+ // If it's over 1/2 of a period back in time, that's wrong. Move it
+ // forwards to now.
+ if (last_tick_timepoint - monotonic_now < -period_ / 2) {
+ last_tick_timepoint += period_;
}
+
+ // We should be sampling our sensors to kick off the control cycle 50 uS
+ // after the falling edge. This gives us a little bit of buffer for
+ // errors in waking up. The PWM cycle starts at the falling edge of the
+ // PWM pulse.
+ chrono::nanoseconds new_offset =
+ ::aos::time::PhasedLoop::OffsetFromIntervalAndTime(
+ period_, last_tick_timepoint + chrono::microseconds(50));
+
+ phased_loop_handler_->set_interval_and_offset(period_, new_offset);
}
}
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index d14f704..7a63444 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -59,8 +59,6 @@
// sensor has been added to DMA.
virtual void RunDmaIteration() {}
- void operator()();
-
protected:
// Copies a DMAEncoder to a IndexPosition with the correct unit and direction
// changes.
@@ -185,16 +183,33 @@
// Gets called right before the DMA synchronizer is up and running.
virtual void Start() {}
+ // Sets up everything during startup.
+ void DoStart();
+
+ // Runs a single iteration.
+ void Loop(int iterations);
+
// Returns the monotonic time of the start of the first PWM cycle.
// Returns min_time if no start time could be calculated.
monotonic_clock::time_point GetPWMStartTime();
- bool pwm_trigger_;
+ bool pwm_trigger_ = false;
::std::unique_ptr<::frc971::wpilib::DMASynchronizer> dma_synchronizer_;
::std::atomic<bool> run_{true};
::frc::DriverStation *ds_;
+
+ const int32_t my_pid_;
+
+ // Pointer to the phased loop handler used to modify the wakeup.
+ ::aos::PhasedLoopHandler *phased_loop_handler_;
+
+ // Last time we got called.
+ ::aos::monotonic_clock::time_point last_monotonic_now_ =
+ ::aos::monotonic_clock::min_time;
+ // The current period.
+ chrono::microseconds period_ = chrono::microseconds(5000);
};
} // namespace wpilib
diff --git a/frc971/wpilib/wpilib_robot_base.h b/frc971/wpilib/wpilib_robot_base.h
index 3deb0e4..2cccf65 100644
--- a/frc971/wpilib/wpilib_robot_base.h
+++ b/frc971/wpilib/wpilib_robot_base.h
@@ -1,14 +1,45 @@
#ifndef FRC971_WPILIB_NEWROBOTBASE_H_
#define FRC971_WPILIB_NEWROBOTBASE_H_
+#include "aos/events/shm-event-loop.h"
+#include "aos/init.h"
#include "frc971/wpilib/ahal/RobotBase.h"
namespace frc971 {
namespace wpilib {
class WPILibRobotBase {
-public:
+ public:
virtual void Run() = 0;
+
+ // Runs all the loops.
+ void RunLoops() {
+ // TODO(austin): SIGINT handler calling Exit on all the loops.
+ // TODO(austin): RegisterSignalHandler in ShmEventLoop for others.
+
+ ::std::vector<::std::thread> threads;
+ for (size_t i = 1; i < loops_.size(); ++i) {
+ threads.emplace_back([this, i]() { loops_[i]->Run(); });
+ }
+ // Save some memory and run the last one in the main thread.
+ loops_[0]->Run();
+
+ for (::std::thread &thread : threads) {
+ thread.join();
+ }
+
+ LOG(ERROR, "Exiting WPILibRobot\n");
+
+ ::aos::Cleanup();
+ }
+
+ protected:
+ // Adds a loop to the list of loops to run.
+ void AddLoop(::aos::ShmEventLoop *loop) { loops_.push_back(loop); }
+
+ private:
+ // List of the event loops to run in RunLoops.
+ ::std::vector<::aos::ShmEventLoop *> loops_;
};
#define AOS_ROBOT_CLASS(_ClassName_) \
@@ -17,7 +48,11 @@
template <typename T>
class WPILibAdapterRobot : public frc::RobotBase {
public:
- void StartCompetition() override { robot_.Run(); }
+ void StartCompetition() override {
+ ::aos::InitNRT();
+
+ robot_.Run();
+ }
private:
T robot_;
diff --git a/y2012/wpilib_interface.cc b/y2012/wpilib_interface.cc
index 59ceac5..91a86ed 100644
--- a/y2012/wpilib_interface.cc
+++ b/y2012/wpilib_interface.cc
@@ -49,7 +49,7 @@
#endif
using ::frc971::control_loops::drivetrain_queue;
-using ::y2012::control_loops::accessories_queue;
+using ::y2012::control_loops::AccessoriesQueue;
using namespace frc;
using aos::make_unique;
@@ -72,7 +72,10 @@
class SensorReader : public ::frc971::wpilib::SensorReader {
public:
SensorReader(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::SensorReader(event_loop) {}
+ : ::frc971::wpilib::SensorReader(event_loop),
+ accessories_position_sender_(
+ event_loop->MakeSender<::aos::control_loops::Position>(
+ ".y2012.control_loops.accessories_queue.position")) {}
void RunIteration() {
{
@@ -89,16 +92,33 @@
drivetrain_message.Send();
}
- accessories_queue.position.MakeMessage().Send();
+ accessories_position_sender_.MakeMessage().Send();
}
+
+ private:
+ ::aos::Sender<::aos::control_loops::Position> accessories_position_sender_;
};
class SolenoidWriter {
public:
- SolenoidWriter(const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
- : pcm_(pcm),
- drivetrain_(".y2012.control_loops.drivetrain_queue.output"),
- accessories_(".y2012.control_loops.accessories_queue.output") {}
+ SolenoidWriter(::aos::EventLoop *event_loop,
+ const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
+ : event_loop_(event_loop),
+ pcm_(pcm),
+ drivetrain_fetcher_(
+ event_loop_
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
+ ".y2012.control_loops.drivetrain_queue.output")),
+ accessories_fetcher_(event_loop_->MakeFetcher<
+ ::y2012::control_loops::AccessoriesQueue::Message>(
+ ".y2012.control_loops.accessories_queue.output")) {
+ event_loop_->set_name("Solenoids");
+ event_loop_->SetRuntimeRealtimePriority(27);
+
+ event_loop_->AddPhasedLoop([this](int iterations) { Loop(iterations); },
+ ::std::chrono::milliseconds(20),
+ ::std::chrono::milliseconds(1));
+ }
void set_drivetrain_high(
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
@@ -122,54 +142,43 @@
s3_ = ::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);
- }
+ {
+ accessories_fetcher_.Fetch();
+ if (accessories_fetcher_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *accessories_fetcher_);
+ s1_->Set(accessories_fetcher_->solenoids[0]);
+ s2_->Set(accessories_fetcher_->solenoids[1]);
+ s3_->Set(accessories_fetcher_->solenoids[2]);
}
+ }
- {
- accessories_.FetchLatest();
- if (accessories_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *accessories_);
- s1_->Set(accessories_->solenoids[0]);
- s2_->Set(accessories_->solenoids[1]);
- s3_->Set(accessories_->solenoids[2]);
- }
+ {
+ drivetrain_fetcher_.Fetch();
+ if (drivetrain_fetcher_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *drivetrain_fetcher_);
+ const bool high =
+ drivetrain_fetcher_->left_high || drivetrain_fetcher_->right_high;
+ drivetrain_high_->Set(high);
+ drivetrain_low_->Set(!high);
}
+ }
- {
- drivetrain_.FetchLatest();
- if (drivetrain_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
- const bool high = drivetrain_->left_high || drivetrain_->right_high;
- drivetrain_high_->Set(high);
- drivetrain_low_->Set(!high);
- }
- }
-
- {
- ::frc971::wpilib::PneumaticsToLog to_log;
- pcm_->Flush();
- to_log.read_solenoids = pcm_->GetAll();
- LOG_STRUCT(DEBUG, "pneumatics info", to_log);
- }
+ {
+ ::frc971::wpilib::PneumaticsToLog to_log;
+ pcm_->Flush();
+ to_log.read_solenoids = pcm_->GetAll();
+ LOG_STRUCT(DEBUG, "pneumatics info", to_log);
}
}
- void Quit() { run_ = false; }
-
private:
+ ::aos::EventLoop *event_loop_;
+
const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm_;
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_high_;
@@ -178,35 +187,28 @@
::std::unique_ptr<Compressor> compressor_;
- ::aos::Queue<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
- ::aos::Queue<::y2012::control_loops::AccessoriesQueue::Message> accessories_;
-
- ::std::atomic<bool> run_{true};
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
+ drivetrain_fetcher_;
+ ::aos::Fetcher<::y2012::control_loops::AccessoriesQueue::Message>
+ accessories_fetcher_;
};
-class AccessoriesWriter : public ::frc971::wpilib::LoopOutputHandler {
+class AccessoriesWriter
+ : public ::frc971::wpilib::LoopOutputHandler<AccessoriesQueue::Message> {
public:
AccessoriesWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler(event_loop) {}
+ : ::frc971::wpilib::LoopOutputHandler<AccessoriesQueue::Message>(
+ event_loop, ".y2012.control_loops.accessories_queue.output") {}
- void set_talon1(::std::unique_ptr<Talon> t) {
- talon1_ = ::std::move(t);
- }
+ void set_talon1(::std::unique_ptr<Talon> t) { talon1_ = ::std::move(t); }
- void set_talon2(::std::unique_ptr<Talon> t) {
- talon2_ = ::std::move(t);
- }
+ void set_talon2(::std::unique_ptr<Talon> t) { talon2_ = ::std::move(t); }
private:
- virtual void Read() override {
- ::y2012::control_loops::accessories_queue.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::y2012::control_loops::accessories_queue.output;
- talon1_->SetSpeed(queue->sticks[0]);
- talon2_->SetSpeed(queue->sticks[1]);
- LOG_STRUCT(DEBUG, "will output", *queue);
+ virtual void Write(const AccessoriesQueue::Message &output) override {
+ talon1_->SetSpeed(output.sticks[0]);
+ talon2_->SetSpeed(output.sticks[1]);
+ LOG_STRUCT(DEBUG, "will output", output);
}
virtual void Stop() override {
@@ -226,67 +228,45 @@
}
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 sensor_reader_event_loop;
+ SensorReader sensor_reader(&sensor_reader_event_loop);
+ sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
+ sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
+ AddLoop(&sensor_reader_event_loop);
- ::frc971::wpilib::JoystickSender joystick_sender(&event_loop);
- ::std::thread joystick_thread(::std::ref(joystick_sender));
-
- SensorReader reader(&event_loop);
-
- reader.set_drivetrain_left_encoder(make_encoder(0));
- reader.set_drivetrain_right_encoder(make_encoder(1));
-
- ::std::thread reader_thread(::std::ref(reader));
-
- ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&event_loop);
+ // Thread 3.
+ ::aos::ShmEventLoop output_event_loop;
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
drivetrain_writer.set_left_controller0(
::std::unique_ptr<Talon>(new Talon(3)), true);
drivetrain_writer.set_right_controller0(
::std::unique_ptr<Talon>(new Talon(4)), false);
- ::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
- ::y2012::wpilib::AccessoriesWriter accessories_writer(&event_loop);
+ ::y2012::wpilib::AccessoriesWriter accessories_writer(&output_event_loop);
accessories_writer.set_talon1(::std::unique_ptr<Talon>(new Talon(5)));
accessories_writer.set_talon2(::std::unique_ptr<Talon>(new Talon(6)));
- ::std::thread accessories_writer_thread(::std::ref(accessories_writer));
+ AddLoop(&output_event_loop);
+ // Thread 4.
+ ::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_high(pcm->MakeSolenoid(0));
solenoid_writer.set_drivetrain_low(pcm->MakeSolenoid(2));
solenoid_writer.set_s1(pcm->MakeSolenoid(1));
solenoid_writer.set_s2(pcm->MakeSolenoid(3));
solenoid_writer.set_s3(pcm->MakeSolenoid(4));
+ AddLoop(&solenoid_writer_event_loop);
- ::std::thread solenoid_thread(::std::ref(solenoid_writer));
-
- // 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();
- reader.Quit();
- reader_thread.join();
-
- drivetrain_writer.Quit();
- drivetrain_writer_thread.join();
- accessories_writer.Quit();
- accessories_writer_thread.join();
-
- ::aos::Cleanup();
+ RunLoops();
}
};
diff --git a/y2014/wpilib_interface.cc b/y2014/wpilib_interface.cc
index 09edf96..5e398db 100644
--- a/y2014/wpilib_interface.cc
+++ b/y2014/wpilib_interface.cc
@@ -54,9 +54,8 @@
#endif
using ::frc971::control_loops::drivetrain_queue;
-using ::y2014::control_loops::claw_queue;
-using ::y2014::control_loops::shooter_queue;
-using namespace frc;
+using ::y2014::control_loops::ClawQueue;
+using ::y2014::control_loops::ShooterQueue;
using aos::make_unique;
namespace y2014 {
@@ -121,7 +120,11 @@
SensorReader(::aos::EventLoop *event_loop)
: ::frc971::wpilib::SensorReader(event_loop),
auto_mode_sender_(event_loop->MakeSender<::y2014::sensors::AutoMode>(
- ".y2014.sensors.auto_mode")) {
+ ".y2014.sensors.auto_mode")),
+ 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")) {
// Set it to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateMediumEncoderFilterHz(kMaximumEncoderPulsesPerSecond);
@@ -133,89 +136,91 @@
bottom_reader_.Quit();
}
- void set_auto_selector_analog(::std::unique_ptr<AnalogInput> analog) {
+ void set_auto_selector_analog(::std::unique_ptr<::frc::AnalogInput> analog) {
auto_selector_analog_ = ::std::move(analog);
}
- void set_high_left_drive_hall(::std::unique_ptr<AnalogInput> analog) {
+ void set_high_left_drive_hall(::std::unique_ptr<::frc::AnalogInput> analog) {
high_left_drive_hall_ = ::std::move(analog);
}
- void set_low_right_drive_hall(::std::unique_ptr<AnalogInput> analog) {
+ void set_low_right_drive_hall(::std::unique_ptr<::frc::AnalogInput> analog) {
low_right_drive_hall_ = ::std::move(analog);
}
- void set_high_right_drive_hall(::std::unique_ptr<AnalogInput> analog) {
+ void set_high_right_drive_hall(::std::unique_ptr<::frc::AnalogInput> analog) {
high_right_drive_hall_ = ::std::move(analog);
}
- void set_low_left_drive_hall(::std::unique_ptr<AnalogInput> analog) {
+ void set_low_left_drive_hall(::std::unique_ptr<::frc::AnalogInput> analog) {
low_left_drive_hall_ = ::std::move(analog);
}
- void set_top_claw_encoder(::std::unique_ptr<Encoder> encoder) {
+ void set_top_claw_encoder(::std::unique_ptr<::frc::Encoder> encoder) {
medium_encoder_filter_.Add(encoder.get());
top_reader_.set_encoder(::std::move(encoder));
}
- void set_top_claw_front_hall(::std::unique_ptr<DigitalInput> hall) {
+ void set_top_claw_front_hall(::std::unique_ptr<::frc::DigitalInput> hall) {
hall_filter_.Add(hall.get());
top_reader_.set_front_hall(::std::move(hall));
}
- void set_top_claw_calibration_hall(::std::unique_ptr<DigitalInput> hall) {
+ void set_top_claw_calibration_hall(
+ ::std::unique_ptr<::frc::DigitalInput> hall) {
hall_filter_.Add(hall.get());
top_reader_.set_calibration_hall(::std::move(hall));
}
- void set_top_claw_back_hall(::std::unique_ptr<DigitalInput> hall) {
+ void set_top_claw_back_hall(::std::unique_ptr<::frc::DigitalInput> hall) {
hall_filter_.Add(hall.get());
top_reader_.set_back_hall(::std::move(hall));
}
- void set_bottom_claw_encoder(::std::unique_ptr<Encoder> encoder) {
+ void set_bottom_claw_encoder(::std::unique_ptr<::frc::Encoder> encoder) {
medium_encoder_filter_.Add(encoder.get());
bottom_reader_.set_encoder(::std::move(encoder));
}
- void set_bottom_claw_front_hall(::std::unique_ptr<DigitalInput> hall) {
+ void set_bottom_claw_front_hall(::std::unique_ptr<::frc::DigitalInput> hall) {
hall_filter_.Add(hall.get());
bottom_reader_.set_front_hall(::std::move(hall));
}
- void set_bottom_claw_calibration_hall(::std::unique_ptr<DigitalInput> hall) {
+ void set_bottom_claw_calibration_hall(
+ ::std::unique_ptr<::frc::DigitalInput> hall) {
hall_filter_.Add(hall.get());
bottom_reader_.set_calibration_hall(::std::move(hall));
}
- void set_bottom_claw_back_hall(::std::unique_ptr<DigitalInput> hall) {
+ void set_bottom_claw_back_hall(::std::unique_ptr<::frc::DigitalInput> hall) {
hall_filter_.Add(hall.get());
bottom_reader_.set_back_hall(::std::move(hall));
}
- void set_shooter_encoder(::std::unique_ptr<Encoder> encoder) {
+ void set_shooter_encoder(::std::unique_ptr<::frc::Encoder> encoder) {
medium_encoder_filter_.Add(encoder.get());
shooter_encoder_ = ::std::move(encoder);
}
- void set_shooter_proximal(::std::unique_ptr<DigitalInput> hall) {
+ void set_shooter_proximal(::std::unique_ptr<::frc::DigitalInput> hall) {
hall_filter_.Add(hall.get());
shooter_proximal_ = ::std::move(hall);
}
- void set_shooter_distal(::std::unique_ptr<DigitalInput> hall) {
+ void set_shooter_distal(::std::unique_ptr<::frc::DigitalInput> hall) {
hall_filter_.Add(hall.get());
shooter_distal_ = ::std::move(hall);
}
- void set_shooter_plunger(::std::unique_ptr<DigitalInput> hall) {
+ void set_shooter_plunger(::std::unique_ptr<::frc::DigitalInput> hall) {
hall_filter_.Add(hall.get());
shooter_plunger_ = ::std::move(hall);
shooter_plunger_reader_ =
make_unique<::frc971::wpilib::DMADigitalReader>(shooter_plunger_.get());
}
- void set_shooter_latch(::std::unique_ptr<DigitalInput> hall) {
+ void set_shooter_latch(::std::unique_ptr<::frc::DigitalInput> hall) {
hall_filter_.Add(hall.get());
shooter_latch_ = ::std::move(hall);
shooter_latch_reader_ =
@@ -276,7 +281,7 @@
void RunDmaIteration() {
{
- auto shooter_message = shooter_queue.position.MakeMessage();
+ auto shooter_message = shooter_position_sender_.MakeMessage();
shooter_message->position = shooter_translate(shooter_encoder_->GetRaw());
shooter_message->plunger = !shooter_plunger_reader_->value();
shooter_message->latch = !shooter_latch_reader_->value();
@@ -289,7 +294,7 @@
}
{
- auto claw_message = claw_queue.position.MakeMessage();
+ auto claw_message = claw_position_sender_.MakeMessage();
top_reader_.RunIteration(&claw_message->top);
bottom_reader_.RunIteration(&claw_message->bottom);
@@ -302,20 +307,20 @@
public:
HalfClawReader(bool reversed) : reversed_(reversed) {}
- void set_encoder(::std::unique_ptr<Encoder> encoder) {
+ void set_encoder(::std::unique_ptr<::frc::Encoder> encoder) {
encoder_ = ::std::move(encoder);
}
- void set_front_hall(::std::unique_ptr<DigitalInput> front_hall) {
+ void set_front_hall(::std::unique_ptr<::frc::DigitalInput> front_hall) {
front_hall_ = ::std::move(front_hall);
}
void set_calibration_hall(
- ::std::unique_ptr<DigitalInput> calibration_hall) {
+ ::std::unique_ptr<::frc::DigitalInput> calibration_hall) {
calibration_hall_ = ::std::move(calibration_hall);
}
- void set_back_hall(::std::unique_ptr<DigitalInput> back_hall) {
+ void set_back_hall(::std::unique_ptr<::frc::DigitalInput> back_hall) {
back_hall_ = ::std::move(back_hall);
}
@@ -374,10 +379,10 @@
::std::unique_ptr<::frc971::wpilib::InterruptSynchronizedEncoder>
synchronized_encoder_;
- ::std::unique_ptr<Encoder> encoder_;
- ::std::unique_ptr<DigitalInput> front_hall_;
- ::std::unique_ptr<DigitalInput> calibration_hall_;
- ::std::unique_ptr<DigitalInput> back_hall_;
+ ::std::unique_ptr<::frc::Encoder> encoder_;
+ ::std::unique_ptr<::frc::DigitalInput> front_hall_;
+ ::std::unique_ptr<::frc::DigitalInput> calibration_hall_;
+ ::std::unique_ptr<::frc::DigitalInput> back_hall_;
const bool reversed_;
};
@@ -395,39 +400,54 @@
}
::aos::Sender<::y2014::sensors::AutoMode> auto_mode_sender_;
+ ::aos::Sender<ShooterQueue::Position> shooter_position_sender_;
+ ::aos::Sender<ClawQueue::Position> claw_position_sender_;
- ::std::unique_ptr<AnalogInput> auto_selector_analog_;
+ ::std::unique_ptr<::frc::AnalogInput> auto_selector_analog_;
- ::std::unique_ptr<AnalogInput> low_left_drive_hall_;
- ::std::unique_ptr<AnalogInput> high_left_drive_hall_;
- ::std::unique_ptr<AnalogInput> low_right_drive_hall_;
- ::std::unique_ptr<AnalogInput> high_right_drive_hall_;
+ ::std::unique_ptr<::frc::AnalogInput> low_left_drive_hall_;
+ ::std::unique_ptr<::frc::AnalogInput> high_left_drive_hall_;
+ ::std::unique_ptr<::frc::AnalogInput> low_right_drive_hall_;
+ ::std::unique_ptr<::frc::AnalogInput> high_right_drive_hall_;
HalfClawReader top_reader_{false}, bottom_reader_{true};
- ::std::unique_ptr<Encoder> shooter_encoder_;
- ::std::unique_ptr<DigitalInput> shooter_proximal_, shooter_distal_;
- ::std::unique_ptr<DigitalInput> shooter_plunger_, shooter_latch_;
+ ::std::unique_ptr<::frc::Encoder> shooter_encoder_;
+ ::std::unique_ptr<::frc::DigitalInput> shooter_proximal_, shooter_distal_;
+ ::std::unique_ptr<::frc::DigitalInput> shooter_plunger_, shooter_latch_;
::std::unique_ptr<::frc971::wpilib::DMAEdgeCounter> shooter_proximal_counter_,
shooter_distal_counter_;
::std::unique_ptr<::frc971::wpilib::DMADigitalReader> shooter_plunger_reader_,
shooter_latch_reader_;
- DigitalGlitchFilter hall_filter_;
+ ::frc::DigitalGlitchFilter hall_filter_;
};
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),
- shooter_(".y2014.control_loops.shooter_queue.output"),
- drivetrain_(".frc971.control_loops.drivetrain_queue.output") {}
+ shooter_(event_loop->MakeFetcher<ShooterQueue::Output>(
+ ".y2014.control_loops.shooter_queue.output")),
+ drivetrain_(
+ event_loop
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
+ ".frc971.control_loops.drivetrain_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);
}
@@ -451,60 +471,46 @@
shooter_brake_ = ::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);
- }
- }
-
- {
- shooter_.FetchLatest();
- if (shooter_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *shooter_);
- shooter_latch_->Set(!shooter_->latch_piston);
- shooter_brake_->Set(!shooter_->brake_piston);
- }
- }
-
- {
- drivetrain_.FetchLatest();
- if (drivetrain_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
- drivetrain_left_->Set(!drivetrain_->left_high);
- drivetrain_right_->Set(!drivetrain_->right_high);
- }
- }
-
- {
- ::frc971::wpilib::PneumaticsToLog to_log;
- {
- 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);
+ {
+ shooter_.Fetch();
+ if (shooter_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *shooter_);
+ shooter_latch_->Set(!shooter_->latch_piston);
+ shooter_brake_->Set(!shooter_->brake_piston);
}
}
- }
- void Quit() { run_ = false; }
+ {
+ drivetrain_.Fetch();
+ if (drivetrain_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
+ drivetrain_left_->Set(!drivetrain_->left_high);
+ drivetrain_right_->Set(!drivetrain_->right_high);
+ }
+ }
+
+ {
+ ::frc971::wpilib::PneumaticsToLog to_log;
+ {
+ 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<::frc971::wpilib::BufferedPcm> &pcm_;
@@ -514,33 +520,28 @@
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> shooter_latch_;
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> shooter_brake_;
- ::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<::y2014::control_loops::ShooterQueue::Output> shooter_;
- ::aos::Queue<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
-
- ::std::atomic<bool> run_{true};
+ ::aos::Fetcher<ShooterQueue::Output> shooter_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
};
-class ShooterWriter : public ::frc971::wpilib::LoopOutputHandler {
+class ShooterWriter
+ : public ::frc971::wpilib::LoopOutputHandler<ShooterQueue::Output> {
public:
ShooterWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler(event_loop) {}
+ : ::frc971::wpilib::LoopOutputHandler<ShooterQueue::Output>(
+ event_loop, ".y2014.control_loops.shooter_queue.output") {}
- void set_shooter_talon(::std::unique_ptr<Talon> t) {
+ void set_shooter_talon(::std::unique_ptr<::frc::Talon> t) {
shooter_talon_ = ::std::move(t);
}
private:
- virtual void Read() override {
- ::y2014::control_loops::shooter_queue.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::y2014::control_loops::shooter_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- shooter_talon_->SetSpeed(queue->voltage / 12.0);
+ virtual void Write(const ShooterQueue::Output &output) override {
+ LOG_STRUCT(DEBUG, "will output", output);
+ shooter_talon_->SetSpeed(output.voltage / 12.0);
}
virtual void Stop() override {
@@ -548,52 +549,49 @@
shooter_talon_->SetDisabled();
}
- ::std::unique_ptr<Talon> shooter_talon_;
+ ::std::unique_ptr<::frc::Talon> shooter_talon_;
};
-class ClawWriter : public ::frc971::wpilib::LoopOutputHandler {
+class ClawWriter
+ : public ::frc971::wpilib::LoopOutputHandler<ClawQueue::Output> {
public:
ClawWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler(event_loop) {}
+ : ::frc971::wpilib::LoopOutputHandler<ClawQueue::Output>(
+ event_loop, ".y2014.control_loops.claw_queue.output") {}
- void set_top_claw_talon(::std::unique_ptr<Talon> t) {
+ void set_top_claw_talon(::std::unique_ptr<::frc::Talon> t) {
top_claw_talon_ = ::std::move(t);
}
- void set_bottom_claw_talon(::std::unique_ptr<Talon> t) {
+ void set_bottom_claw_talon(::std::unique_ptr<::frc::Talon> t) {
bottom_claw_talon_ = ::std::move(t);
}
- void set_left_tusk_talon(::std::unique_ptr<Talon> t) {
+ void set_left_tusk_talon(::std::unique_ptr<::frc::Talon> t) {
left_tusk_talon_ = ::std::move(t);
}
- void set_right_tusk_talon(::std::unique_ptr<Talon> t) {
+ void set_right_tusk_talon(::std::unique_ptr<::frc::Talon> t) {
right_tusk_talon_ = ::std::move(t);
}
- void set_intake1_talon(::std::unique_ptr<Talon> t) {
+ void set_intake1_talon(::std::unique_ptr<::frc::Talon> t) {
intake1_talon_ = ::std::move(t);
}
- void set_intake2_talon(::std::unique_ptr<Talon> t) {
+ void set_intake2_talon(::std::unique_ptr<::frc::Talon> t) {
intake2_talon_ = ::std::move(t);
}
private:
- virtual void Read() override {
- ::y2014::control_loops::claw_queue.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::y2014::control_loops::claw_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- intake1_talon_->SetSpeed(queue->intake_voltage / 12.0);
- intake2_talon_->SetSpeed(queue->intake_voltage / 12.0);
- bottom_claw_talon_->SetSpeed(-queue->bottom_claw_voltage / 12.0);
- top_claw_talon_->SetSpeed(queue->top_claw_voltage / 12.0);
- left_tusk_talon_->SetSpeed(queue->tusk_voltage / 12.0);
- right_tusk_talon_->SetSpeed(-queue->tusk_voltage / 12.0);
+ virtual void Write(const ClawQueue::Output &output) override {
+ LOG_STRUCT(DEBUG, "will output", output);
+ intake1_talon_->SetSpeed(output.intake_voltage / 12.0);
+ intake2_talon_->SetSpeed(output.intake_voltage / 12.0);
+ bottom_claw_talon_->SetSpeed(-output.bottom_claw_voltage / 12.0);
+ top_claw_talon_->SetSpeed(output.top_claw_voltage / 12.0);
+ left_tusk_talon_->SetSpeed(output.tusk_voltage / 12.0);
+ right_tusk_talon_->SetSpeed(-output.tusk_voltage / 12.0);
}
virtual void Stop() override {
@@ -606,136 +604,120 @@
right_tusk_talon_->SetDisabled();
}
- ::std::unique_ptr<Talon> top_claw_talon_;
- ::std::unique_ptr<Talon> bottom_claw_talon_;
- ::std::unique_ptr<Talon> left_tusk_talon_;
- ::std::unique_ptr<Talon> right_tusk_talon_;
- ::std::unique_ptr<Talon> intake1_talon_;
- ::std::unique_ptr<Talon> intake2_talon_;
+ ::std::unique_ptr<::frc::Talon> top_claw_talon_;
+ ::std::unique_ptr<::frc::Talon> bottom_claw_talon_;
+ ::std::unique_ptr<::frc::Talon> left_tusk_talon_;
+ ::std::unique_ptr<::frc::Talon> right_tusk_talon_;
+ ::std::unique_ptr<::frc::Talon> intake1_talon_;
+ ::std::unique_ptr<::frc::Talon> intake2_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);
- ::frc971::wpilib::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));
- SensorReader reader(&event_loop);
+ // Thread 3.
+ ::aos::ShmEventLoop sensor_reader_event_loop;
+ SensorReader sensor_reader(&sensor_reader_event_loop);
// Create this first to make sure it ends up in one of the lower-numbered
// FPGA slots so we can use it with DMA.
auto shooter_encoder_temp = make_encoder(2);
- reader.set_auto_selector_analog(make_unique<AnalogInput>(4));
+ sensor_reader.set_auto_selector_analog(make_unique<::frc::AnalogInput>(4));
- reader.set_drivetrain_left_encoder(make_encoder(0));
- reader.set_drivetrain_right_encoder(make_encoder(1));
- reader.set_high_left_drive_hall(make_unique<AnalogInput>(1));
- reader.set_low_left_drive_hall(make_unique<AnalogInput>(0));
- reader.set_high_right_drive_hall(make_unique<AnalogInput>(2));
- reader.set_low_right_drive_hall(make_unique<AnalogInput>(3));
+ sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
+ sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
+ sensor_reader.set_high_left_drive_hall(make_unique<::frc::AnalogInput>(1));
+ sensor_reader.set_low_left_drive_hall(make_unique<::frc::AnalogInput>(0));
+ sensor_reader.set_high_right_drive_hall(make_unique<::frc::AnalogInput>(2));
+ sensor_reader.set_low_right_drive_hall(make_unique<::frc::AnalogInput>(3));
- reader.set_top_claw_encoder(make_encoder(3));
- reader.set_top_claw_front_hall(make_unique<DigitalInput>(4)); // R2
- reader.set_top_claw_calibration_hall(make_unique<DigitalInput>(3)); // R3
- reader.set_top_claw_back_hall(make_unique<DigitalInput>(5)); // R1
+ sensor_reader.set_top_claw_encoder(make_encoder(3));
+ sensor_reader.set_top_claw_front_hall(
+ make_unique<::frc::DigitalInput>(4)); // R2
+ sensor_reader.set_top_claw_calibration_hall(
+ make_unique<::frc::DigitalInput>(3)); // R3
+ sensor_reader.set_top_claw_back_hall(
+ make_unique<::frc::DigitalInput>(5)); // R1
- reader.set_bottom_claw_encoder(make_encoder(4));
- reader.set_bottom_claw_front_hall(make_unique<DigitalInput>(1)); // L2
- reader.set_bottom_claw_calibration_hall(make_unique<DigitalInput>(0)); // L3
- reader.set_bottom_claw_back_hall(make_unique<DigitalInput>(2)); // L1
+ sensor_reader.set_bottom_claw_encoder(make_encoder(4));
+ sensor_reader.set_bottom_claw_front_hall(
+ make_unique<::frc::DigitalInput>(1)); // L2
+ sensor_reader.set_bottom_claw_calibration_hall(
+ make_unique<::frc::DigitalInput>(0)); // L3
+ sensor_reader.set_bottom_claw_back_hall(
+ make_unique<::frc::DigitalInput>(2)); // L1
- reader.set_shooter_encoder(::std::move(shooter_encoder_temp));
- reader.set_shooter_proximal(make_unique<DigitalInput>(6)); // S1
- reader.set_shooter_distal(make_unique<DigitalInput>(7)); // S2
- reader.set_shooter_plunger(make_unique<DigitalInput>(8)); // S3
- reader.set_shooter_latch(make_unique<DigitalInput>(9)); // S4
+ sensor_reader.set_shooter_encoder(::std::move(shooter_encoder_temp));
+ sensor_reader.set_shooter_proximal(
+ make_unique<::frc::DigitalInput>(6)); // S1
+ sensor_reader.set_shooter_distal(
+ make_unique<::frc::DigitalInput>(7)); // S2
+ sensor_reader.set_shooter_plunger(
+ make_unique<::frc::DigitalInput>(8)); // S3
+ sensor_reader.set_shooter_latch(make_unique<::frc::DigitalInput>(9)); // S4
+ AddLoop(&sensor_reader_event_loop);
- ::std::thread reader_thread(::std::ref(reader));
+ // Thread 4.
+ ::aos::ShmEventLoop gyro_event_loop;
+ ::frc971::wpilib::GyroSender gyro_sender(&gyro_event_loop);
+ AddLoop(&gyro_event_loop);
- ::frc971::wpilib::GyroSender gyro_sender(&event_loop);
- ::std::thread gyro_thread(::std::ref(gyro_sender));
+ // Thread 5.
+ ::aos::ShmEventLoop output_event_loop;
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
+ drivetrain_writer.set_left_controller0(make_unique<::frc::Talon>(5), true);
+ drivetrain_writer.set_right_controller0(make_unique<::frc::Talon>(2),
+ false);
- ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&event_loop);
- drivetrain_writer.set_left_controller0(
- ::std::unique_ptr<Talon>(new 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));
+ ::y2014::wpilib::ClawWriter claw_writer(&output_event_loop);
+ claw_writer.set_top_claw_talon(make_unique<::frc::Talon>(1));
+ claw_writer.set_bottom_claw_talon(make_unique<::frc::Talon>(0));
+ claw_writer.set_left_tusk_talon(make_unique<::frc::Talon>(4));
+ claw_writer.set_right_tusk_talon(make_unique<::frc::Talon>(3));
+ claw_writer.set_intake1_talon(make_unique<::frc::Talon>(7));
+ claw_writer.set_intake2_talon(make_unique<::frc::Talon>(8));
- ::y2014::wpilib::ClawWriter claw_writer(&event_loop);
- claw_writer.set_top_claw_talon(::std::unique_ptr<Talon>(new Talon(1)));
- claw_writer.set_bottom_claw_talon(::std::unique_ptr<Talon>(new Talon(0)));
- claw_writer.set_left_tusk_talon(::std::unique_ptr<Talon>(new Talon(4)));
- claw_writer.set_right_tusk_talon(::std::unique_ptr<Talon>(new Talon(3)));
- claw_writer.set_intake1_talon(::std::unique_ptr<Talon>(new Talon(7)));
- claw_writer.set_intake2_talon(::std::unique_ptr<Talon>(new Talon(8)));
- ::std::thread claw_writer_thread(::std::ref(claw_writer));
+ ::y2014::wpilib::ShooterWriter shooter_writer(&output_event_loop);
+ shooter_writer.set_shooter_talon(make_unique<::frc::Talon>(6));
- ::y2014::wpilib::ShooterWriter shooter_writer(&event_loop);
- shooter_writer.set_shooter_talon(::std::unique_ptr<Talon>(new Talon(6)));
- ::std::thread shooter_writer_thread(::std::ref(shooter_writer));
+ 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(7));
solenoid_writer.set_shooter_latch(pcm->MakeSolenoid(5));
solenoid_writer.set_shooter_brake(pcm->MakeSolenoid(4));
- solenoid_writer.set_pressure_switch(make_unique<DigitalInput>(25));
- 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>(25));
+ 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();
- shooter_writer.Quit();
- shooter_writer_thread.join();
- claw_writer.Quit();
- claw_writer_thread.join();
- solenoid_writer.Quit();
- solenoid_thread.join();
-
- ::aos::Cleanup();
+ RunLoops();
}
};
} // namespace wpilib
} // namespace y2014
-
AOS_ROBOT_CLASS(::y2014::wpilib::WPILibRobot);
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();
}
};
diff --git a/y2016/wpilib_interface.cc b/y2016/wpilib_interface.cc
index 86ac349..3167587 100644
--- a/y2016/wpilib_interface.cc
+++ b/y2016/wpilib_interface.cc
@@ -21,7 +21,6 @@
#include "aos/commonmath.h"
#include "aos/events/shm-event-loop.h"
-#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/logging/queue_logging.h"
#include "aos/make_unique.h"
@@ -29,7 +28,6 @@
#include "aos/stl_mutex/stl_mutex.h"
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
-#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
#include "frc971/autonomous/auto.q.h"
#include "frc971/control_loops/control_loops.q.h"
@@ -58,8 +56,9 @@
using ::frc971::control_loops::drivetrain_queue;
using ::y2016::control_loops::shooter::shooter_queue;
using ::y2016::control_loops::superstructure_queue;
-using namespace frc;
using aos::make_unique;
+using ::frc971::wpilib::LoopOutputHandler;
+using ::y2016::control_loops::shooter::ShooterQueue;
namespace y2016 {
namespace wpilib {
@@ -163,78 +162,81 @@
hall_filter_.SetPeriodNanoSeconds(100000);
}
- void set_drivetrain_left_hall(::std::unique_ptr<AnalogInput> analog) {
+ void set_drivetrain_left_hall(::std::unique_ptr<::frc::AnalogInput> analog) {
drivetrain_left_hall_ = ::std::move(analog);
}
- void set_drivetrain_right_hall(::std::unique_ptr<AnalogInput> analog) {
+ void set_drivetrain_right_hall(::std::unique_ptr<::frc::AnalogInput> analog) {
drivetrain_right_hall_ = ::std::move(analog);
}
// Shooter setters.
- void set_shooter_left_encoder(::std::unique_ptr<Encoder> encoder) {
+ void set_shooter_left_encoder(::std::unique_ptr<::frc::Encoder> encoder) {
fast_encoder_filter_.Add(encoder.get());
shooter_left_encoder_ = ::std::move(encoder);
}
- void set_shooter_right_encoder(::std::unique_ptr<Encoder> encoder) {
+ void set_shooter_right_encoder(::std::unique_ptr<::frc::Encoder> encoder) {
fast_encoder_filter_.Add(encoder.get());
shooter_right_encoder_ = ::std::move(encoder);
}
// Intake setters.
- void set_intake_encoder(::std::unique_ptr<Encoder> encoder) {
+ void set_intake_encoder(::std::unique_ptr<::frc::Encoder> encoder) {
medium_encoder_filter_.Add(encoder.get());
intake_encoder_.set_encoder(::std::move(encoder));
}
- void set_intake_potentiometer(::std::unique_ptr<AnalogInput> potentiometer) {
+ void set_intake_potentiometer(
+ ::std::unique_ptr<::frc::AnalogInput> potentiometer) {
intake_encoder_.set_potentiometer(::std::move(potentiometer));
}
- void set_intake_index(::std::unique_ptr<DigitalInput> index) {
+ void set_intake_index(::std::unique_ptr<::frc::DigitalInput> index) {
medium_encoder_filter_.Add(index.get());
intake_encoder_.set_index(::std::move(index));
}
// Shoulder setters.
- void set_shoulder_encoder(::std::unique_ptr<Encoder> encoder) {
+ void set_shoulder_encoder(::std::unique_ptr<::frc::Encoder> encoder) {
medium_encoder_filter_.Add(encoder.get());
shoulder_encoder_.set_encoder(::std::move(encoder));
}
void set_shoulder_potentiometer(
- ::std::unique_ptr<AnalogInput> potentiometer) {
+ ::std::unique_ptr<::frc::AnalogInput> potentiometer) {
shoulder_encoder_.set_potentiometer(::std::move(potentiometer));
}
- void set_shoulder_index(::std::unique_ptr<DigitalInput> index) {
+ void set_shoulder_index(::std::unique_ptr<::frc::DigitalInput> index) {
medium_encoder_filter_.Add(index.get());
shoulder_encoder_.set_index(::std::move(index));
}
// Wrist setters.
- void set_wrist_encoder(::std::unique_ptr<Encoder> encoder) {
+ void set_wrist_encoder(::std::unique_ptr<::frc::Encoder> encoder) {
medium_encoder_filter_.Add(encoder.get());
wrist_encoder_.set_encoder(::std::move(encoder));
}
- void set_wrist_potentiometer(::std::unique_ptr<AnalogInput> potentiometer) {
+ void set_wrist_potentiometer(
+ ::std::unique_ptr<::frc::AnalogInput> potentiometer) {
wrist_encoder_.set_potentiometer(::std::move(potentiometer));
}
- void set_wrist_index(::std::unique_ptr<DigitalInput> index) {
+ void set_wrist_index(::std::unique_ptr<::frc::DigitalInput> index) {
medium_encoder_filter_.Add(index.get());
wrist_encoder_.set_index(::std::move(index));
}
// Ball detector setter.
- void set_ball_detector(::std::unique_ptr<AnalogInput> analog) {
+ void set_ball_detector(::std::unique_ptr<::frc::AnalogInput> analog) {
ball_detector_ = ::std::move(analog);
}
// Autonomous mode switch setter.
- void set_autonomous_mode(int i, ::std::unique_ptr<DigitalInput> sensor) {
+ void set_autonomous_mode(int i,
+ ::std::unique_ptr<::frc::DigitalInput> sensor) {
autonomous_modes_.at(i) = ::std::move(sensor);
}
@@ -318,27 +320,45 @@
::aos::Sender<::y2016::sensors::BallDetector> ball_detector_sender_;
::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
- ::std::unique_ptr<AnalogInput> drivetrain_left_hall_, drivetrain_right_hall_;
+ ::std::unique_ptr<::frc::AnalogInput> drivetrain_left_hall_,
+ drivetrain_right_hall_;
- ::std::unique_ptr<Encoder> shooter_left_encoder_, shooter_right_encoder_;
+ ::std::unique_ptr<::frc::Encoder> shooter_left_encoder_,
+ shooter_right_encoder_;
::frc971::wpilib::DMAEncoderAndPotentiometer intake_encoder_,
shoulder_encoder_, wrist_encoder_;
- ::std::unique_ptr<AnalogInput> ball_detector_;
+ ::std::unique_ptr<::frc::AnalogInput> ball_detector_;
- ::std::array<::std::unique_ptr<DigitalInput>, 4> autonomous_modes_;
+ ::std::array<::std::unique_ptr<::frc::DigitalInput>, 4> autonomous_modes_;
- DigitalGlitchFilter hall_filter_;
+ ::frc::DigitalGlitchFilter hall_filter_;
};
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"),
- shooter_(".y2016.control_loops.shooter.shooter_queue.output"),
- superstructure_(".y2016.control_loops.superstructure_queue.output") {}
+ drivetrain_(
+ event_loop
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
+ ".frc971.control_loops.drivetrain_queue.output")),
+ shooter_(event_loop->MakeFetcher<ShooterQueue::Output>(
+ ".y2016.control_loops.shooter.shooter_queue.output")),
+ superstructure_(event_loop->MakeFetcher<
+ ::y2016::control_loops::SuperstructureQueue::Output>(
+ ".y2016.control_loops.superstructure_queue.output")) {
+ event_loop->set_name("Solenoids");
+ event_loop->SetRuntimeRealtimePriority(27);
- void set_compressor(::std::unique_ptr<Compressor> compressor) {
+ event_loop->OnRun([this]() { compressor_->Start(); });
+
+ event_loop->AddPhasedLoop([this](int iterations) { Loop(iterations); },
+ ::std::chrono::milliseconds(20),
+ ::std::chrono::milliseconds(1));
+ }
+
+ void set_compressor(::std::unique_ptr<::frc::Compressor> compressor) {
compressor_ = ::std::move(compressor);
}
@@ -375,185 +395,163 @@
lights_ = ::std::move(s);
}
- void set_flashlight(::std::unique_ptr<Relay> relay) {
+ void set_flashlight(::std::unique_ptr<::frc::Relay> relay) {
flashlight_ = ::std::move(relay);
}
- void operator()() {
- compressor_->Start();
- ::aos::SetCurrentThreadName("Solenoids");
- ::aos::SetCurrentThreadRealtimePriority(27);
+ private:
+ 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_.Fetch();
+ if (drivetrain_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
+ drivetrain_shifter_->Set(
+ !(drivetrain_->left_high || drivetrain_->right_high));
}
+ }
- {
- drivetrain_.FetchLatest();
- if (drivetrain_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
- drivetrain_shifter_->Set(
- !(drivetrain_->left_high || drivetrain_->right_high));
- }
- }
-
- {
- shooter_.FetchLatest();
- if (shooter_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *shooter_);
- shooter_clamp_->Set(shooter_->clamp_open);
- shooter_pusher_->Set(shooter_->push_to_shooter);
- lights_->Set(shooter_->lights_on);
- if (shooter_->forwards_flashlight) {
- if (shooter_->backwards_flashlight) {
- flashlight_->Set(Relay::kOn);
- } else {
- flashlight_->Set(Relay::kReverse);
- }
+ {
+ shooter_.Fetch();
+ if (shooter_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *shooter_);
+ shooter_clamp_->Set(shooter_->clamp_open);
+ shooter_pusher_->Set(shooter_->push_to_shooter);
+ lights_->Set(shooter_->lights_on);
+ if (shooter_->forwards_flashlight) {
+ if (shooter_->backwards_flashlight) {
+ flashlight_->Set(::frc::Relay::kOn);
} else {
- if (shooter_->backwards_flashlight) {
- flashlight_->Set(Relay::kForward);
- } else {
- flashlight_->Set(Relay::kOff);
- }
+ flashlight_->Set(::frc::Relay::kReverse);
+ }
+ } else {
+ if (shooter_->backwards_flashlight) {
+ flashlight_->Set(::frc::Relay::kForward);
+ } else {
+ flashlight_->Set(::frc::Relay::kOff);
}
}
}
+ }
- {
- superstructure_.FetchLatest();
- if (superstructure_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *superstructure_);
+ {
+ superstructure_.Fetch();
+ if (superstructure_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *superstructure_);
- climber_trigger_->Set(superstructure_->unfold_climber);
+ climber_trigger_->Set(superstructure_->unfold_climber);
- traverse_->Set(superstructure_->traverse_down);
- traverse_latch_->Set(superstructure_->traverse_unlatched);
- }
+ traverse_->Set(superstructure_->traverse_down);
+ traverse_latch_->Set(superstructure_->traverse_unlatched);
}
+ }
- {
- ::frc971::wpilib::PneumaticsToLog to_log;
- { to_log.compressor_on = compressor_->Enabled(); }
+ {
+ ::frc971::wpilib::PneumaticsToLog to_log;
+ { to_log.compressor_on = compressor_->Enabled(); }
- pcm_->Flush();
- to_log.read_solenoids = pcm_->GetAll();
- LOG_STRUCT(DEBUG, "pneumatics info", to_log);
- }
+ pcm_->Flush();
+ to_log.read_solenoids = pcm_->GetAll();
+ LOG_STRUCT(DEBUG, "pneumatics info", to_log);
}
}
- void Quit() { run_ = false; }
-
- private:
const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm_;
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_shifter_,
shooter_clamp_, shooter_pusher_, lights_, traverse_, traverse_latch_,
climber_trigger_;
- ::std::unique_ptr<Relay> flashlight_;
- ::std::unique_ptr<Compressor> compressor_;
+ ::std::unique_ptr<::frc::Relay> flashlight_;
+ ::std::unique_ptr<::frc::Compressor> compressor_;
- ::aos::Queue<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
- ::aos::Queue<::y2016::control_loops::shooter::ShooterQueue::Output> shooter_;
- ::aos::Queue<::y2016::control_loops::SuperstructureQueue::Output>
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
+ ::aos::Fetcher<ShooterQueue::Output> shooter_;
+ ::aos::Fetcher<::y2016::control_loops::SuperstructureQueue::Output>
superstructure_;
-
- ::std::atomic<bool> run_{true};
};
-class ShooterWriter : public ::frc971::wpilib::LoopOutputHandler {
+class ShooterWriter : public LoopOutputHandler<ShooterQueue::Output> {
public:
ShooterWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler(event_loop) {}
+ : LoopOutputHandler<ShooterQueue::Output>(
+ event_loop, ".y2016.control_loops.shooter.shooter_queue.output") {}
- void set_shooter_left_talon(::std::unique_ptr<Talon> t) {
+ void set_shooter_left_talon(::std::unique_ptr<::frc::Talon> t) {
shooter_left_talon_ = ::std::move(t);
}
- void set_shooter_right_talon(::std::unique_ptr<Talon> t) {
+ void set_shooter_right_talon(::std::unique_ptr<::frc::Talon> t) {
shooter_right_talon_ = ::std::move(t);
}
private:
- virtual void Read() override {
- ::y2016::control_loops::shooter::shooter_queue.output.FetchAnother();
+ void Write(const ShooterQueue::Output &output) override {
+ LOG_STRUCT(DEBUG, "will output", output);
+
+ shooter_left_talon_->SetSpeed(output.voltage_left / 12.0);
+ shooter_right_talon_->SetSpeed(-output.voltage_right / 12.0);
}
- virtual void Write() override {
- auto &queue = ::y2016::control_loops::shooter::shooter_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
-
- shooter_left_talon_->SetSpeed(queue->voltage_left / 12.0);
- shooter_right_talon_->SetSpeed(-queue->voltage_right / 12.0);
- }
-
- virtual void Stop() override {
+ void Stop() override {
LOG(WARNING, "Shooter output too old.\n");
shooter_left_talon_->SetDisabled();
shooter_right_talon_->SetDisabled();
}
- ::std::unique_ptr<Talon> shooter_left_talon_, shooter_right_talon_;
+ ::std::unique_ptr<::frc::Talon> shooter_left_talon_, shooter_right_talon_;
};
-class SuperstructureWriter : public ::frc971::wpilib::LoopOutputHandler {
+class SuperstructureWriter
+ : public LoopOutputHandler<
+ ::y2016::control_loops::SuperstructureQueue::Output> {
public:
SuperstructureWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler(event_loop) {}
+ : LoopOutputHandler<::y2016::control_loops::SuperstructureQueue::Output>(
+ event_loop, ".y2016.control_loops.superstructure_queue.output") {}
- void set_intake_talon(::std::unique_ptr<Talon> t) {
+ void set_intake_talon(::std::unique_ptr<::frc::Talon> t) {
intake_talon_ = ::std::move(t);
}
- void set_shoulder_talon(::std::unique_ptr<Talon> t) {
+ void set_shoulder_talon(::std::unique_ptr<::frc::Talon> t) {
shoulder_talon_ = ::std::move(t);
}
- void set_wrist_talon(::std::unique_ptr<Talon> t) {
+ void set_wrist_talon(::std::unique_ptr<::frc::Talon> t) {
wrist_talon_ = ::std::move(t);
}
- void set_top_rollers_talon(::std::unique_ptr<Talon> t) {
+ void set_top_rollers_talon(::std::unique_ptr<::frc::Talon> t) {
top_rollers_talon_ = ::std::move(t);
}
- void set_bottom_rollers_talon(::std::unique_ptr<Talon> t) {
+ void set_bottom_rollers_talon(::std::unique_ptr<::frc::Talon> t) {
bottom_rollers_talon_ = ::std::move(t);
}
- void set_climber_talon(::std::unique_ptr<Talon> t) {
+ void set_climber_talon(::std::unique_ptr<::frc::Talon> t) {
climber_talon_ = ::std::move(t);
}
private:
- virtual void Read() override {
- ::y2016::control_loops::superstructure_queue.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::y2016::control_loops::superstructure_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- intake_talon_->SetSpeed(::aos::Clip(queue->voltage_intake,
+ virtual void Write(const ::y2016::control_loops::SuperstructureQueue::Output
+ &output) override {
+ LOG_STRUCT(DEBUG, "will output", output);
+ intake_talon_->SetSpeed(::aos::Clip(output.voltage_intake,
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- shoulder_talon_->SetSpeed(::aos::Clip(-queue->voltage_shoulder,
+ shoulder_talon_->SetSpeed(::aos::Clip(-output.voltage_shoulder,
-kMaxBringupPower, kMaxBringupPower) /
12.0);
wrist_talon_->SetSpeed(
- ::aos::Clip(queue->voltage_wrist, -kMaxBringupPower, kMaxBringupPower) /
+ ::aos::Clip(output.voltage_wrist, -kMaxBringupPower, kMaxBringupPower) /
12.0);
- top_rollers_talon_->SetSpeed(-queue->voltage_top_rollers / 12.0);
- bottom_rollers_talon_->SetSpeed(-queue->voltage_bottom_rollers / 12.0);
- climber_talon_->SetSpeed(-queue->voltage_climber / 12.0);
+ top_rollers_talon_->SetSpeed(-output.voltage_top_rollers / 12.0);
+ bottom_rollers_talon_->SetSpeed(-output.voltage_bottom_rollers / 12.0);
+ climber_talon_->SetSpeed(-output.voltage_climber / 12.0);
}
virtual void Stop() override {
@@ -563,101 +561,110 @@
wrist_talon_->SetDisabled();
}
- ::std::unique_ptr<Talon> intake_talon_, shoulder_talon_, wrist_talon_,
+ ::std::unique_ptr<::frc::Talon> intake_talon_, shoulder_talon_, wrist_talon_,
top_rollers_talon_, bottom_rollers_talon_, climber_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);
- ::frc971::wpilib::JoystickSender joystick_sender(&event_loop);
- ::std::thread joystick_thread(::std::ref(joystick_sender));
+ // Thread 3.
+ ::aos::ShmEventLoop sensor_reader_event_loop;
+ SensorReader sensor_reader(&sensor_reader_event_loop);
- ::frc971::wpilib::PDPFetcher pdp_fetcher(&event_loop);
- ::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
- SensorReader reader(&event_loop);
+ sensor_reader.set_drivetrain_left_encoder(make_encoder(5));
+ sensor_reader.set_drivetrain_right_encoder(make_encoder(6));
+ sensor_reader.set_drivetrain_left_hall(make_unique<::frc::AnalogInput>(5));
+ sensor_reader.set_drivetrain_right_hall(make_unique<::frc::AnalogInput>(6));
- reader.set_drivetrain_left_encoder(make_encoder(5));
- reader.set_drivetrain_right_encoder(make_encoder(6));
- reader.set_drivetrain_left_hall(make_unique<AnalogInput>(5));
- reader.set_drivetrain_right_hall(make_unique<AnalogInput>(6));
+ sensor_reader.set_shooter_left_encoder(make_encoder(7));
+ sensor_reader.set_shooter_right_encoder(make_encoder(-3));
- reader.set_shooter_left_encoder(make_encoder(7));
- reader.set_shooter_right_encoder(make_encoder(-3));
+ sensor_reader.set_intake_encoder(make_encoder(0));
+ sensor_reader.set_intake_index(make_unique<::frc::DigitalInput>(0));
+ sensor_reader.set_intake_potentiometer(make_unique<::frc::AnalogInput>(0));
- reader.set_intake_encoder(make_encoder(0));
- reader.set_intake_index(make_unique<DigitalInput>(0));
- reader.set_intake_potentiometer(make_unique<AnalogInput>(0));
+ sensor_reader.set_shoulder_encoder(make_encoder(4));
+ sensor_reader.set_shoulder_index(make_unique<::frc::DigitalInput>(2));
+ sensor_reader.set_shoulder_potentiometer(
+ make_unique<::frc::AnalogInput>(2));
- reader.set_shoulder_encoder(make_encoder(4));
- reader.set_shoulder_index(make_unique<DigitalInput>(2));
- reader.set_shoulder_potentiometer(make_unique<AnalogInput>(2));
+ sensor_reader.set_wrist_encoder(make_encoder(1));
+ sensor_reader.set_wrist_index(make_unique<::frc::DigitalInput>(1));
+ sensor_reader.set_wrist_potentiometer(make_unique<::frc::AnalogInput>(1));
- reader.set_wrist_encoder(make_encoder(1));
- reader.set_wrist_index(make_unique<DigitalInput>(1));
- reader.set_wrist_potentiometer(make_unique<AnalogInput>(1));
+ sensor_reader.set_ball_detector(make_unique<::frc::AnalogInput>(7));
- reader.set_ball_detector(make_unique<AnalogInput>(7));
-
- reader.set_autonomous_mode(0, make_unique<DigitalInput>(9));
- reader.set_autonomous_mode(1, make_unique<DigitalInput>(8));
- reader.set_autonomous_mode(2, make_unique<DigitalInput>(7));
- reader.set_autonomous_mode(3, make_unique<DigitalInput>(6));
-
- ::std::thread reader_thread(::std::ref(reader));
+ sensor_reader.set_autonomous_mode(0, make_unique<::frc::DigitalInput>(9));
+ sensor_reader.set_autonomous_mode(1, make_unique<::frc::DigitalInput>(8));
+ sensor_reader.set_autonomous_mode(2, make_unique<::frc::DigitalInput>(7));
+ sensor_reader.set_autonomous_mode(3, make_unique<::frc::DigitalInput>(6));
+ AddLoop(&sensor_reader_event_loop);
// TODO(Brian): This interacts poorly with the SpiRxClearer in ADIS16448.
- ::frc971::wpilib::GyroSender gyro_sender(&event_loop);
- ::std::thread gyro_thread(::std::ref(gyro_sender));
+ // Thread 4.
+ ::aos::ShmEventLoop gyro_event_loop;
+ ::frc971::wpilib::GyroSender gyro_sender(&gyro_event_loop);
+ AddLoop(&gyro_event_loop);
- auto imu_trigger = make_unique<DigitalInput>(3);
- ::frc971::wpilib::ADIS16448 imu(&event_loop, SPI::Port::kMXP,
+ // Thread 5.
+ ::aos::ShmEventLoop imu_event_loop;
+ auto imu_trigger = make_unique<::frc::DigitalInput>(3);
+ ::frc971::wpilib::ADIS16448 imu(&imu_event_loop, ::frc::SPI::Port::kMXP,
imu_trigger.get());
- ::std::thread imu_thread(::std::ref(imu));
+ AddLoop(&imu_event_loop);
- ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&event_loop);
+ // Thread 5.
+ ::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)), false);
+ ::std::unique_ptr<::frc::Talon>(new ::frc::Talon(5)), false);
drivetrain_writer.set_right_controller0(
- ::std::unique_ptr<Talon>(new Talon(4)), true);
- ::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
+ ::std::unique_ptr<::frc::Talon>(new ::frc::Talon(4)), true);
- ShooterWriter shooter_writer(&event_loop);
+ ShooterWriter shooter_writer(&output_event_loop);
shooter_writer.set_shooter_left_talon(
- ::std::unique_ptr<Talon>(new Talon(9)));
+ ::std::unique_ptr<::frc::Talon>(new ::frc::Talon(9)));
shooter_writer.set_shooter_right_talon(
- ::std::unique_ptr<Talon>(new Talon(8)));
- ::std::thread shooter_writer_thread(::std::ref(shooter_writer));
+ ::std::unique_ptr<::frc::Talon>(new ::frc::Talon(8)));
- SuperstructureWriter superstructure_writer(&event_loop);
+ SuperstructureWriter superstructure_writer(&output_event_loop);
superstructure_writer.set_intake_talon(
- ::std::unique_ptr<Talon>(new Talon(3)));
+ ::std::unique_ptr<::frc::Talon>(new ::frc::Talon(3)));
superstructure_writer.set_shoulder_talon(
- ::std::unique_ptr<Talon>(new Talon(6)));
+ ::std::unique_ptr<::frc::Talon>(new ::frc::Talon(6)));
superstructure_writer.set_wrist_talon(
- ::std::unique_ptr<Talon>(new Talon(2)));
+ ::std::unique_ptr<::frc::Talon>(new ::frc::Talon(2)));
superstructure_writer.set_top_rollers_talon(
- ::std::unique_ptr<Talon>(new Talon(1)));
+ ::std::unique_ptr<::frc::Talon>(new ::frc::Talon(1)));
superstructure_writer.set_bottom_rollers_talon(
- ::std::unique_ptr<Talon>(new Talon(0)));
+ ::std::unique_ptr<::frc::Talon>(new ::frc::Talon(0)));
superstructure_writer.set_climber_talon(
- ::std::unique_ptr<Talon>(new Talon(7)));
- ::std::thread superstructure_writer_thread(
- ::std::ref(superstructure_writer));
+ ::std::unique_ptr<::frc::Talon>(new ::frc::Talon(7)));
+ 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_shifter(pcm->MakeSolenoid(0));
solenoid_writer.set_traverse_latch(pcm->MakeSolenoid(2));
solenoid_writer.set_traverse(pcm->MakeSolenoid(3));
@@ -665,45 +672,12 @@
solenoid_writer.set_shooter_pusher(pcm->MakeSolenoid(5));
solenoid_writer.set_lights(pcm->MakeSolenoid(6));
solenoid_writer.set_climber_trigger(pcm->MakeSolenoid(1));
- solenoid_writer.set_flashlight(make_unique<Relay>(0));
+ solenoid_writer.set_flashlight(make_unique<::frc::Relay>(0));
- solenoid_writer.set_compressor(make_unique<Compressor>());
+ solenoid_writer.set_compressor(make_unique<::frc::Compressor>());
+ AddLoop(&solenoid_writer_event_loop);
- ::std::thread solenoid_thread(::std::ref(solenoid_writer));
-
- // 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();
- imu.Quit();
- imu_thread.join();
-
- drivetrain_writer.Quit();
- drivetrain_writer_thread.join();
- shooter_writer.Quit();
- shooter_writer_thread.join();
- superstructure_writer.Quit();
- superstructure_writer_thread.join();
- solenoid_writer.Quit();
- solenoid_thread.join();
-
- ::aos::Cleanup();
+ RunLoops();
}
};
diff --git a/y2017/wpilib_interface.cc b/y2017/wpilib_interface.cc
index b72d6e9..e8dca61 100644
--- a/y2017/wpilib_interface.cc
+++ b/y2017/wpilib_interface.cc
@@ -59,7 +59,7 @@
#endif
using ::frc971::control_loops::drivetrain_queue;
-using ::y2017::control_loops::superstructure_queue;
+using ::y2017::control_loops::SuperstructureQueue;
using ::y2017::constants::Values;
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
@@ -129,7 +129,10 @@
: ::frc971::wpilib::SensorReader(event_loop),
auto_mode_sender_(
event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
- ".frc971.autonomous.auto_mode")) {
+ ".frc971.autonomous.auto_mode")),
+ superstructure_position_sender_(
+ event_loop->MakeSender<SuperstructureQueue::Position>(
+ ".y2017.control_loops.superstructure_queue.position")) {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -220,7 +223,7 @@
const auto values = constants::GetValues();
{
- auto superstructure_message = superstructure_queue.position.MakeMessage();
+ auto superstructure_message = superstructure_position_sender_.MakeMessage();
CopyPosition(intake_encoder_, &superstructure_message->intake,
Values::kIntakeEncoderCountsPerRevolution,
Values::kIntakeEncoderRatio, intake_pot_translate, true,
@@ -261,6 +264,7 @@
private:
::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
+ ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
DigitalGlitchFilter hall_filter_;
@@ -282,8 +286,17 @@
class SolenoidWriter {
public:
- SolenoidWriter()
- : superstructure_(".y2017.control_loops.superstructure_queue.output") {}
+ SolenoidWriter(::aos::EventLoop *event_loop)
+ : superstructure_output_fetcher_(
+ event_loop->MakeFetcher<SuperstructureQueue::Output>(
+ ".y2017.control_loops.superstructure_queue.output")) {
+ event_loop->set_name("Solenoids");
+ event_loop->SetRuntimeRealtimePriority(27);
+
+ event_loop->AddPhasedLoop([this](int iterations) { Loop(iterations); },
+ ::std::chrono::milliseconds(20),
+ ::std::chrono::milliseconds(1));
+ }
::frc971::wpilib::BufferedPcm *pcm() { return &pcm_; }
@@ -295,54 +308,40 @@
rgb_lights_ = ::std::move(s);
}
- void operator()() {
- ::aos::SetCurrentThreadName("Solenoids");
- ::aos::SetCurrentThreadRealtimePriority(27);
-
- ::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);
- }
- }
-
- {
- superstructure_.FetchLatest();
- if (superstructure_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *superstructure_);
- lights_->Set(superstructure_->lights_on);
- rgb_lights_->Set(superstructure_->red_light_on |
- superstructure_->green_light_on |
- superstructure_->blue_light_on);
- }
- }
-
- pcm_.Flush();
+ void Loop(const int iterations) {
+ if (iterations != 1) {
+ LOG(DEBUG, "Solenoids skipped %d iterations\n", iterations - 1);
}
- }
- void Quit() { run_ = false; }
+ {
+ superstructure_output_fetcher_.Fetch();
+ if (superstructure_output_fetcher_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *superstructure_output_fetcher_);
+ lights_->Set(superstructure_output_fetcher_->lights_on);
+ rgb_lights_->Set(superstructure_output_fetcher_->red_light_on |
+ superstructure_output_fetcher_->green_light_on |
+ superstructure_output_fetcher_->blue_light_on);
+ }
+ }
+
+ pcm_.Flush();
+ }
private:
::frc971::wpilib::BufferedPcm pcm_;
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> lights_, rgb_lights_;
- ::aos::Queue<::y2017::control_loops::SuperstructureQueue::Output>
- superstructure_;
-
- ::std::atomic<bool> run_{true};
+ ::aos::Fetcher<::y2017::control_loops::SuperstructureQueue::Output>
+ superstructure_output_fetcher_;
};
-class SuperstructureWriter : public ::frc971::wpilib::LoopOutputHandler {
+class SuperstructureWriter
+ : public ::frc971::wpilib::LoopOutputHandler<SuperstructureQueue::Output> {
public:
SuperstructureWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler(event_loop) {}
+ : ::frc971::wpilib::LoopOutputHandler<SuperstructureQueue::Output>(
+ event_loop, ".y2017.control_loops.superstructure_queue.output") {}
void set_intake_victor(::std::unique_ptr<::frc::VictorSP> t) {
intake_victor_ = ::std::move(t);
@@ -382,32 +381,27 @@
}
private:
- virtual void Read() override {
- ::y2017::control_loops::superstructure_queue.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::y2017::control_loops::superstructure_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- intake_victor_->SetSpeed(::aos::Clip(queue->voltage_intake,
+ virtual void Write(const SuperstructureQueue::Output &output) override {
+ LOG_STRUCT(DEBUG, "will output", output);
+ intake_victor_->SetSpeed(::aos::Clip(output.voltage_intake,
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- intake_rollers_victor_->SetSpeed(queue->voltage_intake_rollers / 12.0);
- indexer_victor_->SetSpeed(-queue->voltage_indexer / 12.0);
- indexer_roller_victor_->SetSpeed(queue->voltage_indexer_rollers / 12.0);
- turret_victor_->SetSpeed(::aos::Clip(-queue->voltage_turret,
+ intake_rollers_victor_->SetSpeed(output.voltage_intake_rollers / 12.0);
+ indexer_victor_->SetSpeed(-output.voltage_indexer / 12.0);
+ indexer_roller_victor_->SetSpeed(output.voltage_indexer_rollers / 12.0);
+ turret_victor_->SetSpeed(::aos::Clip(-output.voltage_turret,
-kMaxBringupPower, kMaxBringupPower) /
12.0);
hood_victor_->SetSpeed(
- ::aos::Clip(queue->voltage_hood, -kMaxBringupPower, kMaxBringupPower) /
+ ::aos::Clip(output.voltage_hood, -kMaxBringupPower, kMaxBringupPower) /
12.0);
- shooter_victor_->SetSpeed(queue->voltage_shooter / 12.0);
+ shooter_victor_->SetSpeed(output.voltage_shooter / 12.0);
- red_light_->Set(queue->red_light_on);
- green_light_->Set(queue->green_light_on);
- blue_light_->Set(queue->blue_light_on);
+ red_light_->Set(output.red_light_on);
+ green_light_->Set(output.green_light_on);
+ blue_light_->Set(output.blue_light_on);
- gear_servo_->SetPosition(queue->gear_servo);
+ gear_servo_->SetPosition(output.gear_servo);
}
virtual void Stop() override {
@@ -444,60 +438,64 @@
}
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);
- ::frc971::wpilib::JoystickSender joystick_sender(&event_loop);
- ::std::thread joystick_thread(::std::ref(joystick_sender));
+ // Thread 3.
+ ::aos::ShmEventLoop sensor_reader_event_loop;
+ SensorReader sensor_reader(&sensor_reader_event_loop);
- ::frc971::wpilib::PDPFetcher pdp_fetcher(&event_loop);
- ::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
- SensorReader reader(&event_loop);
+ sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
+ sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
- // TODO(campbell): Update port numbers
- reader.set_drivetrain_left_encoder(make_encoder(0));
- reader.set_drivetrain_right_encoder(make_encoder(1));
+ sensor_reader.set_intake_encoder(make_encoder(3));
+ sensor_reader.set_intake_absolute(make_unique<DigitalInput>(0));
+ sensor_reader.set_intake_potentiometer(make_unique<AnalogInput>(4));
- reader.set_intake_encoder(make_encoder(3));
- reader.set_intake_absolute(make_unique<DigitalInput>(0));
- reader.set_intake_potentiometer(make_unique<AnalogInput>(4));
+ sensor_reader.set_indexer_encoder(make_encoder(5));
+ sensor_reader.set_indexer_hall(make_unique<DigitalInput>(4));
- reader.set_indexer_encoder(make_encoder(5));
- reader.set_indexer_hall(make_unique<DigitalInput>(4));
+ sensor_reader.set_turret_encoder(make_encoder(6));
+ sensor_reader.set_turret_hall(make_unique<DigitalInput>(2));
- reader.set_turret_encoder(make_encoder(6));
- reader.set_turret_hall(make_unique<DigitalInput>(2));
+ sensor_reader.set_hood_encoder(make_encoder(4));
+ sensor_reader.set_hood_index(make_unique<DigitalInput>(1));
- reader.set_hood_encoder(make_encoder(4));
- reader.set_hood_index(make_unique<DigitalInput>(1));
+ sensor_reader.set_shooter_encoder(make_encoder(2));
- reader.set_shooter_encoder(make_encoder(2));
+ sensor_reader.set_autonomous_mode(0, make_unique<DigitalInput>(9));
+ sensor_reader.set_autonomous_mode(1, make_unique<DigitalInput>(8));
- reader.set_autonomous_mode(0, make_unique<DigitalInput>(9));
- reader.set_autonomous_mode(1, make_unique<DigitalInput>(8));
+ sensor_reader.set_pwm_trigger(true);
- reader.set_pwm_trigger(true);
+ AddLoop(&sensor_reader_event_loop);
- ::std::thread reader_thread(::std::ref(reader));
-
+ // Thread 5.
+ ::aos::ShmEventLoop imu_event_loop;
auto imu_trigger = make_unique<DigitalInput>(3);
- ::frc971::wpilib::ADIS16448 imu(&event_loop, SPI::Port::kOnboardCS1,
+ ::frc971::wpilib::ADIS16448 imu(&imu_event_loop, SPI::Port::kOnboardCS1,
imu_trigger.get());
imu.SetDummySPI(SPI::Port::kOnboardCS2);
auto imu_reset = make_unique<DigitalOutput>(6);
imu.set_reset(imu_reset.get());
- ::std::thread imu_thread(::std::ref(imu));
+ AddLoop(&imu_event_loop);
- ::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<::frc::VictorSP>(new ::frc::VictorSP(7)), true);
drivetrain_writer.set_right_controller0(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)), false);
- ::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
- SuperstructureWriter superstructure_writer(&event_loop);
+ SuperstructureWriter superstructure_writer(&output_event_loop);
superstructure_writer.set_intake_victor(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)));
superstructure_writer.set_intake_rollers_victor(
@@ -523,42 +521,16 @@
superstructure_writer.set_blue_light(
::std::unique_ptr<DigitalOutput>(new DigitalOutput(25)));
- ::std::thread superstructure_writer_thread(
- ::std::ref(superstructure_writer));
+ AddLoop(&output_event_loop);
- SolenoidWriter solenoid_writer;
+ // Thread 6.
+ ::aos::ShmEventLoop solenoid_writer_event_loop;
+ SolenoidWriter solenoid_writer(&solenoid_writer_event_loop);
solenoid_writer.set_lights(solenoid_writer.pcm()->MakeSolenoid(0));
solenoid_writer.set_rgb_light(solenoid_writer.pcm()->MakeSolenoid(1));
+ AddLoop(&solenoid_writer_event_loop);
- ::std::thread solenoid_thread(::std::ref(solenoid_writer));
-
- // 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();
- imu.Quit();
- imu_thread.join();
-
- drivetrain_writer.Quit();
- drivetrain_writer_thread.join();
- superstructure_writer.Quit();
- superstructure_writer_thread.join();
-
- ::aos::Cleanup();
+ RunLoops();
}
};
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index dc327a7..16c95c8 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -57,7 +57,7 @@
#endif
using ::frc971::control_loops::drivetrain_queue;
-using ::y2018::control_loops::superstructure_queue;
+using ::y2018::control_loops::SuperstructureQueue;
using ::y2018::constants::Values;
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
@@ -145,7 +145,10 @@
class SensorReader : public ::frc971::wpilib::SensorReader {
public:
SensorReader(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::SensorReader(event_loop) {
+ : ::frc971::wpilib::SensorReader(event_loop),
+ superstructure_position_sender_(
+ event_loop->MakeSender<SuperstructureQueue::Position>(
+ ".y2018.control_loops.superstructure_queue.position")) {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -291,7 +294,8 @@
const auto values = constants::GetValues();
{
- auto superstructure_message = superstructure_queue.position.MakeMessage();
+ auto superstructure_message =
+ superstructure_position_sender_.MakeMessage();
CopyPosition(proximal_encoder_, &superstructure_message->arm.proximal,
Values::kProximalEncoderCountsPerRevolution(),
@@ -339,6 +343,8 @@
}
private:
+ ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
+
::std::unique_ptr<frc::AnalogInput> left_drivetrain_shifter_,
right_drivetrain_shifter_;
@@ -370,15 +376,33 @@
->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
".frc971.control_loops.drivetrain_queue.output")),
superstructure_fetcher_(
- event_loop->MakeFetcher<
- ::y2018::control_loops::SuperstructureQueue::Output>(
+ event_loop->MakeFetcher<SuperstructureQueue::Output>(
".y2018.control_loops.superstructure_queue.output")),
status_light_fetcher_(event_loop->MakeFetcher<::y2018::StatusLight>(
".y2018.status_light")),
vision_status_fetcher_(
event_loop->MakeFetcher<::y2018::vision::VisionStatus>(
".y2018.vision.vision_status")),
- pcm_(pcm) {}
+ pcm_(pcm) {
+ event_loop_->set_name("Solenoids");
+ event_loop_->SetRuntimeRealtimePriority(27);
+
+ int32_t status = 0;
+ HAL_CompressorHandle compressor_ = HAL_InitializeCompressor(0, &status);
+ if (status != 0) {
+ LOG(ERROR, "Compressor status is nonzero, %d\n",
+ static_cast<int>(status));
+ }
+ HAL_SetCompressorClosedLoopControl(compressor_, true, &status);
+ if (status != 0) {
+ LOG(ERROR, "Compressor status is nonzero, %d\n",
+ static_cast<int>(status));
+ }
+
+ event_loop_->AddPhasedLoop([this](int iterations) { Loop(iterations); },
+ ::std::chrono::milliseconds(20),
+ ::std::chrono::milliseconds(1));
+ }
// left drive
// right drive
@@ -412,84 +436,72 @@
forks_ = ::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));
+ {
+ drivetrain_fetcher_.Fetch();
+ if (drivetrain_fetcher_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *drivetrain_fetcher_);
+ left_drivetrain_shifter_->Set(!drivetrain_fetcher_->left_high);
+ right_drivetrain_shifter_->Set(!drivetrain_fetcher_->right_high);
+ }
+ }
- while (run_) {
- {
- const int iterations = phased_loop.SleepUntilNext();
- if (iterations != 1) {
- LOG(DEBUG, "Solenoids skipped %d iterations\n", iterations - 1);
- }
+ {
+ superstructure_fetcher_.Fetch();
+ if (superstructure_fetcher_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *superstructure_fetcher_);
+
+ claw_->Set(!superstructure_fetcher_->claw_grabbed);
+ arm_brakes_->Set(superstructure_fetcher_->release_arm_brake);
+ hook_->Set(superstructure_fetcher_->hook_release);
+ forks_->Set(superstructure_fetcher_->forks_release);
+ }
+ }
+
+ {
+ ::frc971::wpilib::PneumaticsToLog to_log;
+
+ pcm_->Flush();
+ to_log.read_solenoids = pcm_->GetAll();
+ LOG_STRUCT(DEBUG, "pneumatics info", to_log);
+ }
+
+ monotonic_clock::time_point monotonic_now = event_loop_->monotonic_now();
+ status_light_fetcher_.Fetch();
+ // If we don't have a light request (or it's an old one), we are borked.
+ // Flash the red light slowly.
+ if (!status_light_fetcher_.get() ||
+ monotonic_now >
+ status_light_fetcher_->sent_time + chrono::milliseconds(100)) {
+ StatusLight color;
+ color.red = 0.0;
+ color.green = 0.0;
+ color.blue = 0.0;
+
+ vision_status_fetcher_.Fetch();
+ ++light_flash_;
+ if (light_flash_ > 10) {
+ color.red = 0.5;
+ } else if (!vision_status_fetcher_.get() ||
+ monotonic_now >
+ vision_status_fetcher_->sent_time + chrono::seconds(1)) {
+ color.red = 0.5;
+ color.green = 0.5;
}
- {
- drivetrain_fetcher_.Fetch();
- if (drivetrain_fetcher_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *drivetrain_fetcher_);
- left_drivetrain_shifter_->Set(!drivetrain_fetcher_->left_high);
- right_drivetrain_shifter_->Set(!drivetrain_fetcher_->right_high);
- }
+ if (light_flash_ > 20) {
+ light_flash_ = 0;
}
- {
- superstructure_fetcher_.Fetch();
- if (superstructure_fetcher_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *superstructure_fetcher_);
-
- claw_->Set(!superstructure_fetcher_->claw_grabbed);
- arm_brakes_->Set(superstructure_fetcher_->release_arm_brake);
- hook_->Set(superstructure_fetcher_->hook_release);
- forks_->Set(superstructure_fetcher_->forks_release);
- }
- }
-
- {
- ::frc971::wpilib::PneumaticsToLog to_log;
-
- pcm_->Flush();
- to_log.read_solenoids = pcm_->GetAll();
- LOG_STRUCT(DEBUG, "pneumatics info", to_log);
- }
-
- monotonic_clock::time_point monotonic_now = event_loop_->monotonic_now();
- status_light_fetcher_.Fetch();
- // If we don't have a light request (or it's an old one), we are borked.
- // Flash the red light slowly.
- if (!status_light_fetcher_.get() ||
- monotonic_now >
- status_light_fetcher_->sent_time + chrono::milliseconds(100)) {
- StatusLight color;
- color.red = 0.0;
- color.green = 0.0;
- color.blue = 0.0;
-
- vision_status_fetcher_.Fetch();
- ++light_flash_;
- if (light_flash_ > 10) {
- color.red = 0.5;
- } else if (!vision_status_fetcher_.get() ||
- monotonic_now >
- vision_status_fetcher_->sent_time + chrono::seconds(1)) {
- color.red = 0.5;
- color.green = 0.5;
- }
-
- if (light_flash_ > 20) {
- light_flash_ = 0;
- }
-
- LOG_STRUCT(DEBUG, "color", color);
- SetColor(color);
- } else {
- LOG_STRUCT(DEBUG, "color", *status_light_fetcher_);
- SetColor(*status_light_fetcher_);
- }
+ LOG_STRUCT(DEBUG, "color", color);
+ SetColor(color);
+ } else {
+ LOG_STRUCT(DEBUG, "color", *status_light_fetcher_);
+ SetColor(*status_light_fetcher_);
}
}
@@ -526,8 +538,7 @@
::aos::EventLoop *event_loop_;
::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
drivetrain_fetcher_;
- ::aos::Fetcher<::y2018::control_loops::SuperstructureQueue::Output>
- superstructure_fetcher_;
+ ::aos::Fetcher<SuperstructureQueue::Output> superstructure_fetcher_;
::aos::Fetcher<::y2018::StatusLight> status_light_fetcher_;
::aos::Fetcher<::y2018::vision::VisionStatus> vision_status_fetcher_;
@@ -537,6 +548,8 @@
left_drivetrain_shifter_, right_drivetrain_shifter_, claw_, arm_brakes_,
hook_, forks_;
+ HAL_CompressorHandle compressor_;
+
::ctre::phoenix::CANifier canifier_{0};
::std::atomic<bool> run_{true};
@@ -548,10 +561,12 @@
int light_flash_ = 0;
};
-class SuperstructureWriter : public ::frc971::wpilib::LoopOutputHandler {
+class SuperstructureWriter
+ : public ::frc971::wpilib::LoopOutputHandler<SuperstructureQueue::Output> {
public:
SuperstructureWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler(event_loop) {}
+ : ::frc971::wpilib::LoopOutputHandler<SuperstructureQueue::Output>(
+ event_loop, ".y2018.control_loops.superstructure_queue.output") {}
void set_proximal_victor(::std::unique_ptr<::frc::VictorSP> t) {
proximal_victor_ = ::std::move(t);
@@ -580,44 +595,39 @@
}
private:
- virtual void Read() override {
- ::y2018::control_loops::superstructure_queue.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::y2018::control_loops::superstructure_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
+ virtual void Write(const SuperstructureQueue::Output &output) override {
+ LOG_STRUCT(DEBUG, "will output", output);
left_intake_elastic_victor_->SetSpeed(
- ::aos::Clip(-queue->left_intake.voltage_elastic, -kMaxBringupPower,
+ ::aos::Clip(-output.left_intake.voltage_elastic, -kMaxBringupPower,
kMaxBringupPower) /
12.0);
right_intake_elastic_victor_->SetSpeed(
- ::aos::Clip(queue->right_intake.voltage_elastic, -kMaxBringupPower,
+ ::aos::Clip(output.right_intake.voltage_elastic, -kMaxBringupPower,
kMaxBringupPower) /
12.0);
left_intake_rollers_victor_->SetSpeed(
- ::aos::Clip(-queue->left_intake.voltage_rollers, -kMaxBringupPower,
+ ::aos::Clip(-output.left_intake.voltage_rollers, -kMaxBringupPower,
kMaxBringupPower) /
12.0);
right_intake_rollers_victor_->SetSpeed(
- ::aos::Clip(queue->right_intake.voltage_rollers, -kMaxBringupPower,
+ ::aos::Clip(output.right_intake.voltage_rollers, -kMaxBringupPower,
kMaxBringupPower) /
12.0);
- proximal_victor_->SetSpeed(::aos::Clip(-queue->voltage_proximal,
+ proximal_victor_->SetSpeed(::aos::Clip(-output.voltage_proximal,
-kMaxBringupPower,
kMaxBringupPower) /
12.0);
- distal_victor_->SetSpeed(::aos::Clip(queue->voltage_distal,
+ distal_victor_->SetSpeed(::aos::Clip(output.voltage_distal,
-kMaxBringupPower, kMaxBringupPower) /
12.0);
hanger_victor_->SetSpeed(
- ::aos::Clip(-queue->voltage_winch, -kMaxBringupPower, kMaxBringupPower) /
+ ::aos::Clip(-output.voltage_winch, -kMaxBringupPower, kMaxBringupPower) /
12.0);
}
@@ -648,76 +658,87 @@
}
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);
- ::frc971::wpilib::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));
-
- SensorReader reader(&event_loop);
-
- // TODO(Sabina): Update port numbers(Sensors and Victors)
- reader.set_drivetrain_left_encoder(make_encoder(0));
- reader.set_left_drivetrain_shifter_potentiometer(
+ // Thread 3.
+ ::aos::ShmEventLoop sensor_reader_event_loop;
+ SensorReader sensor_reader(&sensor_reader_event_loop);
+ sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
+ sensor_reader.set_left_drivetrain_shifter_potentiometer(
make_unique<frc::AnalogInput>(6));
- reader.set_drivetrain_right_encoder(make_encoder(1));
- reader.set_right_drivetrain_shifter_potentiometer(
+ sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
+ sensor_reader.set_right_drivetrain_shifter_potentiometer(
make_unique<frc::AnalogInput>(7));
- reader.set_proximal_encoder(make_encoder(4));
- reader.set_proximal_absolute_pwm(make_unique<frc::DigitalInput>(2));
- reader.set_proximal_potentiometer(make_unique<frc::AnalogInput>(2));
+ sensor_reader.set_proximal_encoder(make_encoder(4));
+ sensor_reader.set_proximal_absolute_pwm(make_unique<frc::DigitalInput>(2));
+ sensor_reader.set_proximal_potentiometer(make_unique<frc::AnalogInput>(2));
- reader.set_distal_encoder(make_encoder(2));
- reader.set_distal_absolute_pwm(make_unique<frc::DigitalInput>(3));
- reader.set_distal_potentiometer(make_unique<frc::AnalogInput>(3));
+ sensor_reader.set_distal_encoder(make_encoder(2));
+ sensor_reader.set_distal_absolute_pwm(make_unique<frc::DigitalInput>(3));
+ sensor_reader.set_distal_potentiometer(make_unique<frc::AnalogInput>(3));
- reader.set_right_intake_encoder(make_encoder(5));
- reader.set_right_intake_absolute_pwm(make_unique<frc::DigitalInput>(7));
- reader.set_right_intake_potentiometer(make_unique<frc::AnalogInput>(1));
- reader.set_right_intake_spring_angle(make_unique<frc::AnalogInput>(5));
- reader.set_right_intake_cube_detector(make_unique<frc::DigitalInput>(1));
+ sensor_reader.set_right_intake_encoder(make_encoder(5));
+ sensor_reader.set_right_intake_absolute_pwm(
+ make_unique<frc::DigitalInput>(7));
+ sensor_reader.set_right_intake_potentiometer(
+ make_unique<frc::AnalogInput>(1));
+ sensor_reader.set_right_intake_spring_angle(
+ make_unique<frc::AnalogInput>(5));
+ sensor_reader.set_right_intake_cube_detector(
+ make_unique<frc::DigitalInput>(1));
- reader.set_left_intake_encoder(make_encoder(3));
- reader.set_left_intake_absolute_pwm(make_unique<frc::DigitalInput>(4));
- reader.set_left_intake_potentiometer(make_unique<frc::AnalogInput>(0));
- reader.set_left_intake_spring_angle(make_unique<frc::AnalogInput>(4));
- reader.set_left_intake_cube_detector(make_unique<frc::DigitalInput>(0));
+ sensor_reader.set_left_intake_encoder(make_encoder(3));
+ sensor_reader.set_left_intake_absolute_pwm(
+ make_unique<frc::DigitalInput>(4));
+ sensor_reader.set_left_intake_potentiometer(
+ make_unique<frc::AnalogInput>(0));
+ sensor_reader.set_left_intake_spring_angle(
+ make_unique<frc::AnalogInput>(4));
+ sensor_reader.set_left_intake_cube_detector(
+ make_unique<frc::DigitalInput>(0));
- reader.set_claw_beambreak(make_unique<frc::DigitalInput>(8));
- reader.set_box_back_beambreak(make_unique<frc::DigitalInput>(9));
+ sensor_reader.set_claw_beambreak(make_unique<frc::DigitalInput>(8));
+ sensor_reader.set_box_back_beambreak(make_unique<frc::DigitalInput>(9));
- reader.set_pwm_trigger(true);
+ sensor_reader.set_pwm_trigger(true);
- reader.set_lidar_lite_input(make_unique<frc::DigitalInput>(22));
+ sensor_reader.set_lidar_lite_input(make_unique<frc::DigitalInput>(22));
+ AddLoop(&sensor_reader_event_loop);
- ::std::thread reader_thread(::std::ref(reader));
-
+ // Thread 4.
+ ::aos::ShmEventLoop imu_event_loop;
auto imu_trigger = make_unique<frc::DigitalInput>(5);
- ::frc971::wpilib::ADIS16448 imu(&event_loop, frc::SPI::Port::kOnboardCS1,
+ ::frc971::wpilib::ADIS16448 imu(&imu_event_loop, frc::SPI::Port::kOnboardCS1,
imu_trigger.get());
imu.SetDummySPI(frc::SPI::Port::kOnboardCS2);
auto imu_reset = make_unique<frc::DigitalOutput>(6);
imu.set_reset(imu_reset.get());
- ::std::thread imu_thread(::std::ref(imu));
+ AddLoop(&imu_event_loop);
// While as of 2/9/18 the drivetrain Victors are SPX, it appears as though
// they are identical, as far as DrivetrainWriter is concerned, to the SP
// variety so all the Victors are written as SPs.
- ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&event_loop);
+ // Thread 5.
+ ::aos::ShmEventLoop output_event_loop;
+
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
drivetrain_writer.set_left_controller0(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)), false);
drivetrain_writer.set_right_controller0(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)), true);
- ::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
- SuperstructureWriter superstructure_writer(&event_loop);
+ SuperstructureWriter superstructure_writer(&output_event_loop);
superstructure_writer.set_left_intake_elastic_victor(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(4)));
superstructure_writer.set_left_intake_rollers_victor(
@@ -730,66 +751,26 @@
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)));
superstructure_writer.set_distal_victor(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)));
-
superstructure_writer.set_hanger_victor(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(8)));
- ::std::thread superstructure_writer_thread(
- ::std::ref(superstructure_writer));
+ AddLoop(&output_event_loop);
+ // Thread 6.
// This is a separate event loop because we want to run it at much lower
// priority.
- ::aos::ShmEventLoop solenoid_event_loop;
+ ::aos::ShmEventLoop solenoid_writer_event_loop;
::frc971::wpilib::BufferedPcm pcm;
- SolenoidWriter solenoid_writer(&solenoid_event_loop, &pcm);
+ SolenoidWriter solenoid_writer(&solenoid_writer_event_loop, &pcm);
solenoid_writer.set_left_drivetrain_shifter(pcm.MakeSolenoid(0));
solenoid_writer.set_right_drivetrain_shifter(pcm.MakeSolenoid(1));
solenoid_writer.set_claw(pcm.MakeSolenoid(2));
solenoid_writer.set_arm_brakes(pcm.MakeSolenoid(3));
solenoid_writer.set_hook(pcm.MakeSolenoid(4));
solenoid_writer.set_forks(pcm.MakeSolenoid(5));
+ AddLoop(&solenoid_writer_event_loop);
- ::std::thread solenoid_thread(::std::ref(solenoid_writer));
-
- int32_t status = 0;
- HAL_CompressorHandle compressor = HAL_InitializeCompressor(0, &status);
- if (status != 0) {
- LOG(ERROR, "Compressor status is nonzero, %d\n",
- static_cast<int>(status));
- }
- HAL_SetCompressorClosedLoopControl(compressor, true, &status);
- if (status != 0) {
- LOG(ERROR, "Compressor status is nonzero, %d\n",
- static_cast<int>(status));
- }
-
- // 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();
- imu.Quit();
- imu_thread.join();
-
- drivetrain_writer.Quit();
- drivetrain_writer_thread.join();
- superstructure_writer.Quit();
- superstructure_writer_thread.join();
-
- ::aos::Cleanup();
+ RunLoops();
}
};
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 16487b9..31b5158 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -57,7 +57,7 @@
#endif
using ::frc971::control_loops::drivetrain_queue;
-using ::y2019::control_loops::superstructure::superstructure_queue;
+using ::y2019::control_loops::superstructure::SuperstructureQueue;
using ::y2019::constants::Values;
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
@@ -134,7 +134,11 @@
: ::frc971::wpilib::SensorReader(event_loop),
auto_mode_sender_(
event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
- ".frc971.autonomous.auto_mode")) {
+ ".frc971.autonomous.auto_mode")),
+ superstructure_position_sender_(
+ event_loop->MakeSender<SuperstructureQueue::Position>(
+ ".y2019.control_loops.superstructure.superstructure_queue."
+ "position")) {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -242,7 +246,8 @@
const auto values = constants::GetValues();
{
- auto superstructure_message = superstructure_queue.position.MakeMessage();
+ auto superstructure_message =
+ superstructure_position_sender_.MakeMessage();
// Elevator
CopyPosition(elevator_encoder_, &superstructure_message->elevator,
@@ -296,6 +301,7 @@
private:
::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
+ ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
::frc971::wpilib::AbsoluteEncoderAndPotentiometer elevator_encoder_,
wrist_encoder_, stilts_encoder_;
@@ -446,10 +452,13 @@
frc971::wpilib::SpiRxClearer rx_clearer_;
};
-class SuperstructureWriter : public ::frc971::wpilib::LoopOutputHandler {
+class SuperstructureWriter
+ : public ::frc971::wpilib::LoopOutputHandler<SuperstructureQueue::Output> {
public:
SuperstructureWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler(event_loop),
+ : ::frc971::wpilib::LoopOutputHandler<SuperstructureQueue::Output>(
+ event_loop,
+ ".y2019.control_loops.superstructure.superstructure_queue.output"),
robot_state_fetcher_(
event_loop->MakeFetcher<::aos::RobotState>(".aos.robot_state")) {}
@@ -474,29 +483,22 @@
}
private:
- void Read() override {
- ::y2019::control_loops::superstructure::superstructure_queue.output
- .FetchAnother();
- }
-
- void Write() override {
- auto &queue =
- ::y2019::control_loops::superstructure::superstructure_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- elevator_victor_->SetSpeed(::aos::Clip(queue->elevator_voltage,
+ void Write(const SuperstructureQueue::Output &output) override {
+ LOG_STRUCT(DEBUG, "will output", output);
+ elevator_victor_->SetSpeed(::aos::Clip(output.elevator_voltage,
-kMaxBringupPower,
kMaxBringupPower) /
12.0);
- intake_victor_->SetSpeed(::aos::Clip(queue->intake_joint_voltage,
+ intake_victor_->SetSpeed(::aos::Clip(output.intake_joint_voltage,
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- wrist_victor_->SetSpeed(::aos::Clip(-queue->wrist_voltage,
+ wrist_victor_->SetSpeed(::aos::Clip(-output.wrist_voltage,
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- stilts_victor_->SetSpeed(::aos::Clip(queue->stilts_voltage,
+ stilts_victor_->SetSpeed(::aos::Clip(output.stilts_voltage,
-kMaxBringupPower, kMaxBringupPower) /
12.0);
@@ -511,7 +513,7 @@
0.5 * filtered_battery_voltage_ + 0.5 * battery_voltage;
suction_victor_->SetSpeed(::aos::Clip(
- queue->pump_voltage / filtered_battery_voltage_, -1.0, 1.0));
+ output.pump_voltage / filtered_battery_voltage_, -1.0, 1.0));
}
void Stop() override {
@@ -537,11 +539,17 @@
SolenoidWriter(::aos::EventLoop *event_loop)
: event_loop_(event_loop),
superstructure_fetcher_(event_loop->MakeFetcher<
- ::y2019::control_loops::superstructure::
- SuperstructureQueue::Output>(
+ SuperstructureQueue::Output>(
".y2019.control_loops.superstructure.superstructure_queue.output")),
status_light_fetcher_(event_loop->MakeFetcher<::y2019::StatusLight>(
- ".y2019.status_light")) {}
+ ".y2019.status_light")) {
+ ::aos::SetCurrentThreadName("Solenoids");
+ ::aos::SetCurrentThreadRealtimePriority(27);
+
+ event_loop_->AddPhasedLoop([this](int iterations) { Loop(iterations); },
+ ::std::chrono::milliseconds(20),
+ ::std::chrono::milliseconds(1));
+ }
void set_big_suction_cup(int index0, int index1) {
big_suction_cup0_ = pcm_.MakeSolenoid(index0);
@@ -559,76 +567,62 @@
intake_rollers_talon_->EnableCurrentLimit(true);
}
- 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));
+ {
+ superstructure_fetcher_.Fetch();
+ if (superstructure_fetcher_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *superstructure_fetcher_);
- while (run_) {
- {
- const int iterations = phased_loop.SleepUntilNext();
- if (iterations != 1) {
- LOG(DEBUG, "Solenoids skipped %d iterations\n", iterations - 1);
- }
+ big_suction_cup0_->Set(!superstructure_fetcher_->intake_suction_bottom);
+ big_suction_cup1_->Set(!superstructure_fetcher_->intake_suction_bottom);
+ small_suction_cup0_->Set(superstructure_fetcher_->intake_suction_top);
+ small_suction_cup1_->Set(superstructure_fetcher_->intake_suction_top);
+
+ intake_rollers_talon_->Set(
+ ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
+ ::aos::Clip(superstructure_fetcher_->intake_roller_voltage,
+ -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
+ }
+ }
+
+ {
+ ::frc971::wpilib::PneumaticsToLog to_log;
+
+ pcm_.Flush();
+ to_log.read_solenoids = pcm_.GetAll();
+ LOG_STRUCT(DEBUG, "pneumatics info", to_log);
+ }
+
+ status_light_fetcher_.Fetch();
+ // If we don't have a light request (or it's an old one), we are borked.
+ // Flash the red light slowly.
+ if (!status_light_fetcher_.get() ||
+ status_light_fetcher_.get()->sent_time + chrono::milliseconds(100) <
+ event_loop_->monotonic_now()) {
+ StatusLight color;
+ color.red = 0.0;
+ color.green = 0.0;
+ color.blue = 0.0;
+
+ ++light_flash_;
+ if (light_flash_ > 10) {
+ color.red = 0.5;
}
- {
- superstructure_fetcher_.Fetch();
- if (superstructure_fetcher_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *superstructure_fetcher_);
-
- big_suction_cup0_->Set(
- !superstructure_fetcher_->intake_suction_bottom);
- big_suction_cup1_->Set(
- !superstructure_fetcher_->intake_suction_bottom);
- small_suction_cup0_->Set(superstructure_fetcher_->intake_suction_top);
- small_suction_cup1_->Set(superstructure_fetcher_->intake_suction_top);
-
- intake_rollers_talon_->Set(
- ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
- ::aos::Clip(superstructure_fetcher_->intake_roller_voltage,
- -kMaxBringupPower, kMaxBringupPower) /
- 12.0);
- }
+ if (light_flash_ > 20) {
+ light_flash_ = 0;
}
- {
- ::frc971::wpilib::PneumaticsToLog to_log;
-
- pcm_.Flush();
- to_log.read_solenoids = pcm_.GetAll();
- LOG_STRUCT(DEBUG, "pneumatics info", to_log);
- }
-
- status_light_fetcher_.Fetch();
- // If we don't have a light request (or it's an old one), we are borked.
- // Flash the red light slowly.
- if (!status_light_fetcher_.get() ||
- status_light_fetcher_.get()->sent_time + chrono::milliseconds(100) <
- event_loop_->monotonic_now()) {
- StatusLight color;
- color.red = 0.0;
- color.green = 0.0;
- color.blue = 0.0;
-
- ++light_flash_;
- if (light_flash_ > 10) {
- color.red = 0.5;
- }
-
- if (light_flash_ > 20) {
- light_flash_ = 0;
- }
-
- LOG_STRUCT(DEBUG, "color", color);
- SetColor(color);
- } else {
- LOG_STRUCT(DEBUG, "color", *status_light_fetcher_.get());
- SetColor(*status_light_fetcher_.get());
- }
+ LOG_STRUCT(DEBUG, "color", color);
+ SetColor(color);
+ } else {
+ LOG_STRUCT(DEBUG, "color", *status_light_fetcher_.get());
+ SetColor(*status_light_fetcher_.get());
}
}
@@ -659,8 +653,6 @@
}
}
- void Quit() { run_ = false; }
-
private:
::aos::EventLoop *event_loop_;
@@ -679,8 +671,6 @@
::ctre::phoenix::CANifier canifier_{0};
- ::std::atomic<bool> run_{true};
-
double last_red_ = -1.0;
double last_green_ = -1.0;
double last_blue_ = -1.0;
@@ -696,49 +686,51 @@
}
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;
- ::aos::ShmEventLoop solenoid_event_loop;
+ // Thread 2.
+ ::aos::ShmEventLoop pdp_fetcher_event_loop;
+ ::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
+ AddLoop(&pdp_fetcher_event_loop);
- ::frc971::wpilib::JoystickSender joystick_sender(&event_loop);
- ::std::thread joystick_thread(::std::ref(joystick_sender));
+ // Thread 3.
+ ::aos::ShmEventLoop sensor_reader_event_loop;
+ SensorReader sensor_reader(&sensor_reader_event_loop);
+ sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
+ sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
- ::frc971::wpilib::PDPFetcher pdp_fetcher(&event_loop);
- ::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
- SensorReader reader(&event_loop);
+ sensor_reader.set_elevator_encoder(make_encoder(4));
+ sensor_reader.set_elevator_absolute_pwm(make_unique<frc::DigitalInput>(4));
+ sensor_reader.set_elevator_potentiometer(make_unique<frc::AnalogInput>(4));
- reader.set_drivetrain_left_encoder(make_encoder(0));
- reader.set_drivetrain_right_encoder(make_encoder(1));
+ sensor_reader.set_wrist_encoder(make_encoder(5));
+ sensor_reader.set_wrist_absolute_pwm(make_unique<frc::DigitalInput>(5));
+ sensor_reader.set_wrist_potentiometer(make_unique<frc::AnalogInput>(5));
- reader.set_elevator_encoder(make_encoder(4));
- reader.set_elevator_absolute_pwm(make_unique<frc::DigitalInput>(4));
- reader.set_elevator_potentiometer(make_unique<frc::AnalogInput>(4));
+ sensor_reader.set_intake_encoder(make_encoder(2));
+ sensor_reader.set_intake_absolute_pwm(make_unique<frc::DigitalInput>(2));
- reader.set_wrist_encoder(make_encoder(5));
- reader.set_wrist_absolute_pwm(make_unique<frc::DigitalInput>(5));
- reader.set_wrist_potentiometer(make_unique<frc::AnalogInput>(5));
+ sensor_reader.set_stilts_encoder(make_encoder(3));
+ sensor_reader.set_stilts_absolute_pwm(make_unique<frc::DigitalInput>(3));
+ sensor_reader.set_stilts_potentiometer(make_unique<frc::AnalogInput>(3));
- reader.set_intake_encoder(make_encoder(2));
- reader.set_intake_absolute_pwm(make_unique<frc::DigitalInput>(2));
+ sensor_reader.set_pwm_trigger(true);
+ sensor_reader.set_vacuum_sensor(7);
- reader.set_stilts_encoder(make_encoder(3));
- reader.set_stilts_absolute_pwm(make_unique<frc::DigitalInput>(3));
- reader.set_stilts_potentiometer(make_unique<frc::AnalogInput>(3));
+ sensor_reader.set_platform_right_detect(make_unique<frc::DigitalInput>(6));
+ sensor_reader.set_platform_left_detect(make_unique<frc::DigitalInput>(7));
- reader.set_pwm_trigger(true);
- reader.set_vacuum_sensor(7);
+ sensor_reader.set_autonomous_mode(0, make_unique<frc::DigitalInput>(22));
+ sensor_reader.set_autonomous_mode(0, make_unique<frc::DigitalInput>(23));
+ AddLoop(&sensor_reader_event_loop);
- reader.set_platform_right_detect(make_unique<frc::DigitalInput>(6));
- reader.set_platform_left_detect(make_unique<frc::DigitalInput>(7));
-
- reader.set_autonomous_mode(0, make_unique<frc::DigitalInput>(22));
- reader.set_autonomous_mode(0, make_unique<frc::DigitalInput>(23));
-
- ::std::thread reader_thread(::std::ref(reader));
-
- CameraReader camera_reader(&event_loop);
+ // Thread 4.
+ ::aos::ShmEventLoop imu_event_loop;
+ CameraReader camera_reader(&imu_event_loop);
frc::SPI camera_spi(frc::SPI::Port::kOnboardCS3);
camera_reader.set_spi(&camera_spi);
camera_reader.SetDummySPI(frc::SPI::Port::kOnboardCS2);
@@ -747,26 +739,27 @@
camera_reader.set_activate_passthrough(make_unique<frc::DigitalInput>(25));
auto imu_trigger = make_unique<frc::DigitalInput>(0);
- ::frc971::wpilib::ADIS16448 imu(&event_loop, frc::SPI::Port::kOnboardCS1,
+ ::frc971::wpilib::ADIS16448 imu(&imu_event_loop, frc::SPI::Port::kOnboardCS1,
imu_trigger.get());
imu.set_spi_idle_callback(
[&camera_reader]() { camera_reader.DoSpiTransaction(); });
auto imu_reset = make_unique<frc::DigitalOutput>(1);
imu.set_reset(imu_reset.get());
- ::std::thread imu_thread(::std::ref(imu));
+ AddLoop(&imu_event_loop);
// While as of 2/9/18 the drivetrain Victors are SPX, it appears as though
// they are identical, as far as DrivetrainWriter is concerned, to the SP
// variety so all the Victors are written as SPs.
- ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&event_loop);
+ // Thread 5.
+ ::aos::ShmEventLoop output_event_loop;
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
drivetrain_writer.set_left_controller0(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)), true);
drivetrain_writer.set_right_controller0(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)), false);
- ::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
- SuperstructureWriter superstructure_writer(&event_loop);
+ SuperstructureWriter superstructure_writer(&output_event_loop);
superstructure_writer.set_elevator_victor(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(4)));
// TODO(austin): Do the vacuum
@@ -778,47 +771,18 @@
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(5)));
superstructure_writer.set_stilts_victor(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)));
+ AddLoop(&output_event_loop);
- ::std::thread superstructure_writer_thread(
- ::std::ref(superstructure_writer));
-
- SolenoidWriter solenoid_writer(&solenoid_event_loop);
+ // Thread 6.
+ ::aos::ShmEventLoop solenoid_writer_event_loop;
+ SolenoidWriter solenoid_writer(&solenoid_writer_event_loop);
solenoid_writer.set_intake_roller_talon(
make_unique<::ctre::phoenix::motorcontrol::can::TalonSRX>(10));
solenoid_writer.set_big_suction_cup(0, 1);
solenoid_writer.set_small_suction_cup(2, 3);
+ AddLoop(&solenoid_writer_event_loop);
- ::std::thread solenoid_writer_thread(::std::ref(solenoid_writer));
-
- // 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");
-
- solenoid_writer.Quit();
- solenoid_writer_thread.join();
- joystick_sender.Quit();
- joystick_thread.join();
- pdp_fetcher.Quit();
- pdp_fetcher_thread.join();
- reader.Quit();
- reader_thread.join();
- imu.Quit();
- imu_thread.join();
-
- drivetrain_writer.Quit();
- drivetrain_writer_thread.join();
- superstructure_writer.Quit();
- superstructure_writer_thread.join();
-
- ::aos::Cleanup();
+ RunLoops();
}
};