Add flatbuffer for roller current
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: I4e7ac183400260ff04d24623e2467eac0d08c801
diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index 8ec3c2c..45d93d9 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -110,11 +110,283 @@
} // namespace
+static constexpr int kCANFalconCount = 6;
+static constexpr int kCANSignalsCount = 4;
+static constexpr int kRollerSignalsCount = 4;
+static constexpr units::frequency::hertz_t kCANUpdateFreqHz = 200_Hz;
+
+class Falcon {
+ public:
+ Falcon(int device_id, std::string canbus,
+ aos::SizedArray<ctre::phoenixpro::BaseStatusSignalValue *,
+ kCANFalconCount * kCANSignalsCount +
+ kRollerSignalsCount> *signals)
+ : talon_(device_id, canbus),
+ device_id_(device_id),
+ device_temp_(talon_.GetDeviceTemp()),
+ supply_voltage_(talon_.GetSupplyVoltage()),
+ supply_current_(talon_.GetSupplyCurrent()),
+ torque_current_(talon_.GetTorqueCurrent()),
+ position_(talon_.GetPosition()) {
+ // device temp is not timesynced so don't add it to the list of signals
+ device_temp_.SetUpdateFrequency(kCANUpdateFreqHz);
+
+ CHECK_EQ(kCANSignalsCount, 4);
+ CHECK_NOTNULL(signals);
+ CHECK_LE(signals->size() + 4u, signals->capacity());
+
+ supply_voltage_.SetUpdateFrequency(kCANUpdateFreqHz);
+ signals->push_back(&supply_voltage_);
+
+ supply_current_.SetUpdateFrequency(kCANUpdateFreqHz);
+ signals->push_back(&supply_current_);
+
+ torque_current_.SetUpdateFrequency(kCANUpdateFreqHz);
+ signals->push_back(&torque_current_);
+
+ position_.SetUpdateFrequency(kCANUpdateFreqHz);
+ signals->push_back(&position_);
+ }
+
+ void WriteConfigs(ctre::phoenixpro::signals::InvertedValue invert) {
+ inverted_ = invert;
+
+ ctre::phoenixpro::configs::CurrentLimitsConfigs current_limits;
+ current_limits.StatorCurrentLimit =
+ constants::Values::kDrivetrainStatorCurrentLimit();
+ current_limits.StatorCurrentLimitEnable = true;
+ current_limits.SupplyCurrentLimit =
+ constants::Values::kDrivetrainSupplyCurrentLimit();
+ current_limits.SupplyCurrentLimitEnable = true;
+
+ ctre::phoenixpro::configs::MotorOutputConfigs output_configs;
+ output_configs.NeutralMode =
+ ctre::phoenixpro::signals::NeutralModeValue::Brake;
+ output_configs.DutyCycleNeutralDeadband = 0;
+
+ output_configs.Inverted = inverted_;
+
+ ctre::phoenixpro::configs::TalonFXConfiguration configuration;
+ configuration.CurrentLimits = current_limits;
+ configuration.MotorOutput = output_configs;
+
+ ctre::phoenix::StatusCode status =
+ talon_.GetConfigurator().Apply(configuration);
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
+ }
+
+ void WriteRollerConfigs() {
+ ctre::phoenixpro::configs::CurrentLimitsConfigs current_limits;
+ current_limits.StatorCurrentLimit =
+ constants::Values::kRollerStatorCurrentLimit();
+ current_limits.StatorCurrentLimitEnable = true;
+ current_limits.SupplyCurrentLimit =
+ constants::Values::kRollerSupplyCurrentLimit();
+ current_limits.SupplyCurrentLimitEnable = true;
+
+ ctre::phoenixpro::configs::MotorOutputConfigs output_configs;
+ output_configs.NeutralMode =
+ ctre::phoenixpro::signals::NeutralModeValue::Brake;
+ output_configs.DutyCycleNeutralDeadband = 0;
+
+ ctre::phoenixpro::configs::TalonFXConfiguration configuration;
+ configuration.CurrentLimits = current_limits;
+ configuration.MotorOutput = output_configs;
+
+ ctre::phoenix::StatusCode status =
+ talon_.GetConfigurator().Apply(configuration);
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
+ }
+
+ ctre::phoenixpro::hardware::TalonFX *talon() { return &talon_; }
+
+ flatbuffers::Offset<drivetrain::CANFalcon> WritePosition(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ drivetrain::CANFalcon::Builder builder(*fbb);
+ builder.add_id(device_id_);
+ builder.add_device_temp(device_temp());
+ builder.add_supply_voltage(supply_voltage());
+ builder.add_supply_current(supply_current());
+ builder.add_torque_current(torque_current());
+
+ double invert =
+ (inverted_ ==
+ ctre::phoenixpro::signals::InvertedValue::Clockwise_Positive
+ ? 1
+ : -1);
+
+ builder.add_position(
+ constants::Values::DrivetrainCANEncoderToMeters(position()) * invert);
+
+ return builder.Finish();
+ }
+
+ int device_id() const { return device_id_; }
+ float device_temp() const { return device_temp_.GetValue().value(); }
+ float supply_voltage() const { return supply_voltage_.GetValue().value(); }
+ float supply_current() const { return supply_current_.GetValue().value(); }
+ float torque_current() const { return torque_current_.GetValue().value(); }
+ float position() const { return position_.GetValue().value(); }
+
+ // returns the monotonic timestamp of the latest timesynced reading in the
+ // timebase of the the syncronized CAN bus clock.
+ int64_t GetTimestamp() {
+ std::chrono::nanoseconds latest_timestamp =
+ torque_current_.GetTimestamp().GetTime();
+
+ return latest_timestamp.count();
+ }
+
+ void RefreshNontimesyncedSignals() { device_temp_.Refresh(); };
+
+ private:
+ ctre::phoenixpro::hardware::TalonFX talon_;
+ int device_id_;
+
+ ctre::phoenixpro::signals::InvertedValue inverted_;
+
+ ctre::phoenixpro::StatusSignalValue<units::temperature::celsius_t>
+ device_temp_;
+ ctre::phoenixpro::StatusSignalValue<units::voltage::volt_t> supply_voltage_;
+ ctre::phoenixpro::StatusSignalValue<units::current::ampere_t> supply_current_,
+ torque_current_;
+ ctre::phoenixpro::StatusSignalValue<units::angle::turn_t> position_;
+};
+
+class CANSensorReader {
+ public:
+ CANSensorReader(aos::EventLoop *event_loop)
+ : event_loop_(event_loop),
+ can_position_sender_(
+ event_loop->MakeSender<drivetrain::CANPosition>("/drivetrain")),
+ roller_falcon_data_(std::nullopt) {
+ event_loop->SetRuntimeRealtimePriority(40);
+ event_loop->SetRuntimeAffinity(aos::MakeCpusetFromCpus({1}));
+ timer_handler_ = event_loop->AddTimer([this]() { Loop(); });
+ timer_handler_->set_name("CANSensorReader Loop");
+
+ event_loop->OnRun([this]() {
+ timer_handler_->Setup(event_loop_->monotonic_now(), 1 / kCANUpdateFreqHz);
+ });
+ }
+
+ aos::SizedArray<ctre::phoenixpro::BaseStatusSignalValue *,
+ kCANFalconCount * kCANSignalsCount + kRollerSignalsCount>
+ *get_signals_registry() {
+ return &signals_;
+ };
+
+ void set_falcons(std::shared_ptr<Falcon> right_front,
+ std::shared_ptr<Falcon> right_back,
+ std::shared_ptr<Falcon> right_under,
+ std::shared_ptr<Falcon> left_front,
+ std::shared_ptr<Falcon> left_back,
+ std::shared_ptr<Falcon> left_under,
+ std::shared_ptr<Falcon> roller_falcon) {
+ right_front_ = std::move(right_front);
+ right_back_ = std::move(right_back);
+ right_under_ = std::move(right_under);
+ left_front_ = std::move(left_front);
+ left_back_ = std::move(left_back);
+ left_under_ = std::move(left_under);
+ roller_falcon_ = std::move(roller_falcon);
+ }
+
+ std::optional<superstructure::CANFalconT> roller_falcon_data() {
+ std::unique_lock<aos::stl_mutex> lock(roller_mutex_);
+ return roller_falcon_data_;
+ }
+
+ private:
+ void Loop() {
+ CHECK_EQ(signals_.size(), 28u);
+ ctre::phoenix::StatusCode status =
+ ctre::phoenixpro::BaseStatusSignalValue::WaitForAll(
+ 2000_ms, {signals_[0], signals_[1], signals_[2], signals_[3],
+ signals_[4], signals_[5], signals_[6], signals_[7],
+ signals_[8], signals_[9], signals_[10], signals_[11],
+ signals_[12], signals_[13], signals_[14], signals_[15],
+ signals_[16], signals_[17], signals_[18], signals_[19],
+ signals_[20], signals_[21], signals_[22], signals_[23],
+ signals_[24], signals_[25], signals_[26], signals_[27]});
+
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to read signals from falcons: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
+
+ auto builder = can_position_sender_.MakeBuilder();
+
+ for (auto falcon : {right_front_, right_back_, right_under_, left_front_,
+ left_back_, left_under_, roller_falcon_}) {
+ falcon->RefreshNontimesyncedSignals();
+ }
+
+ aos::SizedArray<flatbuffers::Offset<drivetrain::CANFalcon>, kCANFalconCount>
+ falcons;
+
+ for (auto falcon : {right_front_, right_back_, right_under_, left_front_,
+ left_back_, left_under_}) {
+ falcons.push_back(falcon->WritePosition(builder.fbb()));
+ }
+
+ auto falcons_list =
+ builder.fbb()->CreateVector<flatbuffers::Offset<drivetrain::CANFalcon>>(
+ falcons);
+
+ drivetrain::CANPosition::Builder can_position_builder =
+ builder.MakeBuilder<drivetrain::CANPosition>();
+
+ can_position_builder.add_falcons(falcons_list);
+ can_position_builder.add_timestamp(right_front_->GetTimestamp());
+ can_position_builder.add_status(static_cast<int>(status));
+
+ builder.CheckOk(builder.Send(can_position_builder.Finish()));
+
+ {
+ std::unique_lock<aos::stl_mutex> lock(roller_mutex_);
+ superstructure::CANFalconT roller_falcon_data;
+ roller_falcon_data.id = roller_falcon_->device_id();
+ roller_falcon_data.supply_current = roller_falcon_->supply_current();
+ roller_falcon_data.torque_current = roller_falcon_->torque_current();
+ roller_falcon_data.supply_voltage = roller_falcon_->supply_voltage();
+ roller_falcon_data.device_temp = roller_falcon_->device_temp();
+ roller_falcon_data.position = roller_falcon_->position();
+ roller_falcon_data_ =
+ std::make_optional<superstructure::CANFalconT>(roller_falcon_data);
+ }
+ }
+
+ aos::EventLoop *event_loop_;
+
+ aos::SizedArray<ctre::phoenixpro::BaseStatusSignalValue *,
+ kCANFalconCount * kCANSignalsCount + kRollerSignalsCount>
+ signals_;
+ aos::Sender<drivetrain::CANPosition> can_position_sender_;
+
+ std::shared_ptr<Falcon> right_front_, right_back_, right_under_, left_front_,
+ left_back_, left_under_, roller_falcon_;
+
+ std::optional<superstructure::CANFalconT> roller_falcon_data_;
+
+ aos::stl_mutex roller_mutex_;
+
+ // Pointer to the timer handler used to modify the wakeup.
+ ::aos::TimerHandler *timer_handler_;
+};
+
// Class to send position messages with sensor readings to our loops.
class SensorReader : public ::frc971::wpilib::SensorReader {
public:
SensorReader(::aos::ShmEventLoop *event_loop,
- std::shared_ptr<const Values> values)
+ std::shared_ptr<const Values> values,
+ CANSensorReader *can_sensor_reader)
: ::frc971::wpilib::SensorReader(event_loop),
values_(std::move(values)),
auto_mode_sender_(
@@ -128,7 +400,8 @@
->MakeSender<::frc971::control_loops::drivetrain::Position>(
"/drivetrain")),
gyro_sender_(event_loop->MakeSender<::frc971::sensors::GyroReading>(
- "/drivetrain")) {
+ "/drivetrain")),
+ can_sensor_reader_(can_sensor_reader) {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -192,6 +465,13 @@
flatbuffers::Offset<frc971::AbsolutePosition> wrist_offset =
frc971::AbsolutePosition::Pack(*builder.fbb(), &wrist);
+ flatbuffers::Offset<superstructure::CANFalcon> roller_falcon_offset;
+ auto optional_roller_falcon = can_sensor_reader_->roller_falcon_data();
+ if (optional_roller_falcon.has_value()) {
+ roller_falcon_offset = superstructure::CANFalcon::Pack(
+ *builder.fbb(), &optional_roller_falcon.value());
+ }
+
superstructure::Position::Builder position_builder =
builder.MakeBuilder<superstructure::Position>();
@@ -201,6 +481,9 @@
end_effector_cube_beam_break_->Get());
position_builder.add_cone_position(cone_position_sensor_.last_width() /
cone_position_sensor_.last_period());
+ if (!roller_falcon_offset.IsNull()) {
+ position_builder.add_roller_falcon(roller_falcon_offset);
+ }
builder.CheckOk(builder.Send(position_builder.Finish()));
}
@@ -374,6 +657,8 @@
frc971::wpilib::DMAPulseWidthReader cone_position_sensor_;
std::unique_ptr<frc::DigitalInput> cone_position_input_;
+
+ CANSensorReader *can_sensor_reader_;
};
class SuperstructureWriter
@@ -442,248 +727,6 @@
::std::unique_ptr<::frc::VictorSP> roll_joint_victor_, wrist_victor_;
};
-static constexpr int kCANFalconCount = 6;
-static constexpr int kCANSignalsCount = 4;
-static constexpr int kRollerSignalsCount = 4;
-static constexpr units::frequency::hertz_t kCANUpdateFreqHz = 200_Hz;
-
-class Falcon {
- public:
- Falcon(int device_id, std::string canbus,
- aos::SizedArray<ctre::phoenixpro::BaseStatusSignalValue *,
- kCANFalconCount * kCANSignalsCount +
- kRollerSignalsCount> *signals)
- : talon_(device_id, canbus),
- device_id_(device_id),
- device_temp_(talon_.GetDeviceTemp()),
- supply_voltage_(talon_.GetSupplyVoltage()),
- supply_current_(talon_.GetSupplyCurrent()),
- torque_current_(talon_.GetTorqueCurrent()),
- position_(talon_.GetPosition()) {
- // device temp is not timesynced so don't add it to the list of signals
- device_temp_.SetUpdateFrequency(kCANUpdateFreqHz);
-
- CHECK_EQ(kCANSignalsCount, 4);
- CHECK_NOTNULL(signals);
- CHECK_LE(signals->size() + 4u, signals->capacity());
-
- supply_voltage_.SetUpdateFrequency(kCANUpdateFreqHz);
- signals->push_back(&supply_voltage_);
-
- supply_current_.SetUpdateFrequency(kCANUpdateFreqHz);
- signals->push_back(&supply_current_);
-
- torque_current_.SetUpdateFrequency(kCANUpdateFreqHz);
- signals->push_back(&torque_current_);
-
- position_.SetUpdateFrequency(kCANUpdateFreqHz);
- signals->push_back(&position_);
- }
-
- void WriteConfigs(ctre::phoenixpro::signals::InvertedValue invert) {
- inverted_ = invert;
-
- ctre::phoenixpro::configs::CurrentLimitsConfigs current_limits;
- current_limits.StatorCurrentLimit =
- constants::Values::kDrivetrainStatorCurrentLimit();
- current_limits.StatorCurrentLimitEnable = true;
- current_limits.SupplyCurrentLimit =
- constants::Values::kDrivetrainSupplyCurrentLimit();
- current_limits.SupplyCurrentLimitEnable = true;
-
- ctre::phoenixpro::configs::MotorOutputConfigs output_configs;
- output_configs.NeutralMode =
- ctre::phoenixpro::signals::NeutralModeValue::Brake;
- output_configs.DutyCycleNeutralDeadband = 0;
-
- output_configs.Inverted = inverted_;
-
- ctre::phoenixpro::configs::TalonFXConfiguration configuration;
- configuration.CurrentLimits = current_limits;
- configuration.MotorOutput = output_configs;
-
- ctre::phoenix::StatusCode status =
- talon_.GetConfigurator().Apply(configuration);
- if (!status.IsOK()) {
- AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
- status.GetName(), status.GetDescription());
- }
- }
-
- void WriteRollerConfigs() {
- ctre::phoenixpro::configs::CurrentLimitsConfigs current_limits;
- current_limits.StatorCurrentLimit =
- constants::Values::kRollerStatorCurrentLimit();
- current_limits.StatorCurrentLimitEnable = true;
- current_limits.SupplyCurrentLimit =
- constants::Values::kRollerSupplyCurrentLimit();
- current_limits.SupplyCurrentLimitEnable = true;
-
- ctre::phoenixpro::configs::MotorOutputConfigs output_configs;
- output_configs.NeutralMode =
- ctre::phoenixpro::signals::NeutralModeValue::Brake;
- output_configs.DutyCycleNeutralDeadband = 0;
-
- ctre::phoenixpro::configs::TalonFXConfiguration configuration;
- configuration.CurrentLimits = current_limits;
- configuration.MotorOutput = output_configs;
-
- ctre::phoenix::StatusCode status =
- talon_.GetConfigurator().Apply(configuration);
- if (!status.IsOK()) {
- AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
- status.GetName(), status.GetDescription());
- }
- }
-
- ctre::phoenixpro::hardware::TalonFX *talon() { return &talon_; }
-
- flatbuffers::Offset<drivetrain::CANFalcon> WritePosition(
- flatbuffers::FlatBufferBuilder *fbb) {
- drivetrain::CANFalcon::Builder builder(*fbb);
- builder.add_id(device_id_);
- builder.add_device_temp(device_temp_.GetValue().value());
- builder.add_supply_voltage(supply_voltage_.GetValue().value());
- builder.add_supply_current(supply_current_.GetValue().value());
- builder.add_torque_current(torque_current_.GetValue().value());
-
- double invert =
- (inverted_ ==
- ctre::phoenixpro::signals::InvertedValue::Clockwise_Positive
- ? 1
- : -1);
-
- builder.add_position(constants::Values::DrivetrainCANEncoderToMeters(
- position_.GetValue().value()) *
- invert);
-
- return builder.Finish();
- }
-
- // returns the monotonic timestamp of the latest timesynced reading in the
- // timebase of the the syncronized CAN bus clock.
- int64_t GetTimestamp() {
- std::chrono::nanoseconds latest_timestamp =
- torque_current_.GetTimestamp().GetTime();
-
- return latest_timestamp.count();
- }
-
- void RefreshNontimesyncedSignals() { device_temp_.Refresh(); };
-
- private:
- ctre::phoenixpro::hardware::TalonFX talon_;
- int device_id_;
-
- ctre::phoenixpro::signals::InvertedValue inverted_;
-
- ctre::phoenixpro::StatusSignalValue<units::temperature::celsius_t>
- device_temp_;
- ctre::phoenixpro::StatusSignalValue<units::voltage::volt_t> supply_voltage_;
- ctre::phoenixpro::StatusSignalValue<units::current::ampere_t> supply_current_,
- torque_current_;
- ctre::phoenixpro::StatusSignalValue<units::angle::turn_t> position_;
-};
-
-class CANSensorReader {
- public:
- CANSensorReader(aos::EventLoop *event_loop)
- : event_loop_(event_loop),
- can_position_sender_(
- event_loop->MakeSender<drivetrain::CANPosition>("/drivetrain")) {
- event_loop->SetRuntimeRealtimePriority(40);
- event_loop->SetRuntimeAffinity(aos::MakeCpusetFromCpus({1}));
- timer_handler_ = event_loop->AddTimer([this]() { Loop(); });
- timer_handler_->set_name("CANSensorReader Loop");
-
- event_loop->OnRun([this]() {
- timer_handler_->Setup(event_loop_->monotonic_now(), 1 / kCANUpdateFreqHz);
- });
- }
-
- aos::SizedArray<ctre::phoenixpro::BaseStatusSignalValue *,
- kCANFalconCount * kCANSignalsCount + kRollerSignalsCount>
- *get_signals_registry() {
- return &signals_;
- };
-
- void set_falcons(std::shared_ptr<Falcon> right_front,
- std::shared_ptr<Falcon> right_back,
- std::shared_ptr<Falcon> right_under,
- std::shared_ptr<Falcon> left_front,
- std::shared_ptr<Falcon> left_back,
- std::shared_ptr<Falcon> left_under,
- std::shared_ptr<Falcon> roller_falcon) {
- right_front_ = std::move(right_front);
- right_back_ = std::move(right_back);
- right_under_ = std::move(right_under);
- left_front_ = std::move(left_front);
- left_back_ = std::move(left_back);
- left_under_ = std::move(left_under);
- roller_falcon_ = std::move(roller_falcon);
- }
-
- private:
- void Loop() {
- CHECK_EQ(signals_.size(), 28u);
- ctre::phoenix::StatusCode status =
- ctre::phoenixpro::BaseStatusSignalValue::WaitForAll(
- 2000_ms, {signals_[0], signals_[1], signals_[2], signals_[3],
- signals_[4], signals_[5], signals_[6], signals_[7],
- signals_[8], signals_[9], signals_[10], signals_[11],
- signals_[12], signals_[13], signals_[14], signals_[15],
- signals_[16], signals_[17], signals_[18], signals_[19],
- signals_[20], signals_[21], signals_[22], signals_[23],
- signals_[24], signals_[25], signals_[26], signals_[27]});
-
- if (!status.IsOK()) {
- AOS_LOG(ERROR, "Failed to read signals from falcons: %s: %s",
- status.GetName(), status.GetDescription());
- }
-
- auto builder = can_position_sender_.MakeBuilder();
-
- for (auto falcon : {right_front_, right_back_, right_under_, left_front_,
- left_back_, left_under_, roller_falcon_}) {
- falcon->RefreshNontimesyncedSignals();
- }
-
- aos::SizedArray<flatbuffers::Offset<drivetrain::CANFalcon>, kCANFalconCount>
- falcons;
-
- for (auto falcon : {right_front_, right_back_, right_under_, left_front_,
- left_back_, left_under_}) {
- falcons.push_back(falcon->WritePosition(builder.fbb()));
- }
-
- auto falcons_list =
- builder.fbb()->CreateVector<flatbuffers::Offset<drivetrain::CANFalcon>>(
- falcons);
-
- drivetrain::CANPosition::Builder can_position_builder =
- builder.MakeBuilder<drivetrain::CANPosition>();
-
- can_position_builder.add_falcons(falcons_list);
- can_position_builder.add_timestamp(right_front_->GetTimestamp());
- can_position_builder.add_status(static_cast<int>(status));
-
- builder.CheckOk(builder.Send(can_position_builder.Finish()));
- }
-
- aos::EventLoop *event_loop_;
-
- aos::SizedArray<ctre::phoenixpro::BaseStatusSignalValue *,
- kCANFalconCount * kCANSignalsCount + kRollerSignalsCount>
- signals_;
- aos::Sender<drivetrain::CANPosition> can_position_sender_;
-
- std::shared_ptr<Falcon> right_front_, right_back_, right_under_, left_front_,
- left_back_, left_under_, roller_falcon_;
-
- // Pointer to the timer handler used to modify the wakeup.
- ::aos::TimerHandler *timer_handler_;
-};
-
class SuperstructureCANWriter
: public ::frc971::wpilib::LoopOutputHandler<superstructure::Output> {
public:
@@ -863,8 +906,34 @@
make_unique<frc::DigitalOutput>(25);
// Thread 3.
+ ::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
+ can_sensor_reader_event_loop.set_name("CANSensorReader");
+ CANSensorReader can_sensor_reader(&can_sensor_reader_event_loop);
+
+ std::shared_ptr<Falcon> right_front = std::make_shared<Falcon>(
+ 1, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
+ std::shared_ptr<Falcon> right_back = std::make_shared<Falcon>(
+ 2, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
+ std::shared_ptr<Falcon> right_under = std::make_shared<Falcon>(
+ 3, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
+ std::shared_ptr<Falcon> left_front = std::make_shared<Falcon>(
+ 4, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
+ std::shared_ptr<Falcon> left_back = std::make_shared<Falcon>(
+ 5, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
+ std::shared_ptr<Falcon> left_under = std::make_shared<Falcon>(
+ 6, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
+ std::shared_ptr<Falcon> roller = std::make_shared<Falcon>(
+ 13, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
+
+ can_sensor_reader.set_falcons(right_front, right_back, right_under,
+ left_front, left_back, left_under, roller);
+
+ AddLoop(&can_sensor_reader_event_loop);
+
+ // Thread 4.
::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
- SensorReader sensor_reader(&sensor_reader_event_loop, values);
+ SensorReader sensor_reader(&sensor_reader_event_loop, values,
+ &can_sensor_reader);
sensor_reader.set_pwm_trigger(true);
sensor_reader.set_drivetrain_left_encoder(make_encoder(1));
sensor_reader.set_drivetrain_right_encoder(make_encoder(0));
@@ -895,31 +964,6 @@
AddLoop(&sensor_reader_event_loop);
- // Thread 4.
- ::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
- can_sensor_reader_event_loop.set_name("CANSensorReader");
- CANSensorReader can_sensor_reader(&can_sensor_reader_event_loop);
-
- std::shared_ptr<Falcon> right_front = std::make_shared<Falcon>(
- 1, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
- std::shared_ptr<Falcon> right_back = std::make_shared<Falcon>(
- 2, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
- std::shared_ptr<Falcon> right_under = std::make_shared<Falcon>(
- 3, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
- std::shared_ptr<Falcon> left_front = std::make_shared<Falcon>(
- 4, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
- std::shared_ptr<Falcon> left_back = std::make_shared<Falcon>(
- 5, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
- std::shared_ptr<Falcon> left_under = std::make_shared<Falcon>(
- 6, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
- std::shared_ptr<Falcon> roller = std::make_shared<Falcon>(
- 13, "Drivetrain Bus", can_sensor_reader.get_signals_registry());
-
- can_sensor_reader.set_falcons(right_front, right_back, right_under,
- left_front, left_back, left_under, roller);
-
- AddLoop(&can_sensor_reader_event_loop);
-
// Thread 5.
// Setup CAN.
if (!FLAGS_ctre_diag_server) {