Add 2023 drivetrain writer to wpilib_interface
Uses ctre phoenixpro to write to the falcons on CAN
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: I43bb89c893b4cae58c309bb29727490a1cccd526
diff --git a/y2023/BUILD b/y2023/BUILD
index cf5d60f..274480b 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -3,6 +3,36 @@
load("//tools/build_rules:template.bzl", "jinja2_template")
robot_downloader(
+ binaries = [
+ "//aos/network:web_proxy_main",
+ "//aos/events/logging:log_cat",
+ ],
+ data = [
+ ":aos_config",
+ ":message_bridge_client.sh",
+ "@ctre_phoenix_api_cpp_athena//:shared_libraries",
+ "@ctre_phoenix_cci_athena//:shared_libraries",
+ "@ctre_phoenixpro_api_cpp_athena//:shared_libraries",
+ "@ctre_phoenixpro_tools_athena//:shared_libraries",
+ ],
+ dirs = [
+ "//y2023/www:www_files",
+ ],
+ start_binaries = [
+ "//aos/events/logging:logger_main",
+ "//aos/network:web_proxy_main",
+ ":joystick_reader",
+ ":wpilib_interface",
+ "//aos/network:message_bridge_client",
+ "//aos/network:message_bridge_server",
+ "//y2023/control_loops/drivetrain:drivetrain",
+ "//y2023/control_loops/drivetrain:trajectory_generator",
+ "//y2023/control_loops/superstructure:superstructure",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+robot_downloader(
name = "pi_download",
binaries = [
"//y2020/vision:calibration",
@@ -146,6 +176,7 @@
"//aos/network:timestamp_fbs",
"//y2019/control_loops/drivetrain:target_selector_fbs",
"//y2023/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2023/control_loops/drivetrain:drivetrain_can_position_fbs",
"//y2023/control_loops/superstructure:superstructure_output_fbs",
"//y2023/control_loops/superstructure:superstructure_position_fbs",
"//y2023/control_loops/superstructure:superstructure_status_fbs",
@@ -202,6 +233,7 @@
":constants",
"//aos:init",
"//aos:math",
+ "//aos/containers:sized_array",
"//aos/events:shm_event_loop",
"//aos/logging",
"//aos/stl_mutex",
@@ -228,7 +260,9 @@
"//frc971/wpilib:wpilib_interface",
"//frc971/wpilib:wpilib_robot_base",
"//third_party:phoenix",
+ "//third_party:phoenixpro",
"//third_party:wpilib",
+ "//y2023/control_loops/drivetrain:drivetrain_can_position_fbs",
"//y2023/control_loops/superstructure:superstructure_output_fbs",
"//y2023/control_loops/superstructure:superstructure_position_fbs",
],
diff --git a/y2023/constants.h b/y2023/constants.h
index 03a6cae..edcb794 100644
--- a/y2023/constants.h
+++ b/y2023/constants.h
@@ -20,6 +20,10 @@
struct Values {
static const int kZeroingSampleSize = 200;
+ static const int kDrivetrainWriterPriority = 35;
+ static const int kDrivetrainTxPriority = 36;
+ static const int kDrivetrainRxPriority = 36;
+
static constexpr double kDrivetrainCyclesPerRevolution() { return 512.0; }
static constexpr double kDrivetrainEncoderCountsPerRevolution() {
return kDrivetrainCyclesPerRevolution() * 4;
@@ -32,6 +36,9 @@
kDrivetrainEncoderCountsPerRevolution();
}
+ static constexpr double kDrivetrainSupplyCurrentLimit() { return 35.0; }
+ static constexpr double kDrivetrainStatorCurrentLimit() { return 40.0; }
+
static double DrivetrainEncoderToMeters(int32_t in) {
return ((static_cast<double>(in) /
kDrivetrainEncoderCountsPerRevolution()) *
@@ -39,6 +46,12 @@
kDrivetrainEncoderRatio() * control_loops::drivetrain::kWheelRadius;
}
+ static double DrivetrainCANEncoderToMeters(double rotations) {
+ return (rotations * (2.0 * M_PI)) *
+ control_loops::drivetrain::kHighOutputRatio *
+ control_loops::drivetrain::kWheelRadius;
+ }
+
struct PotConstants {
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
::frc971::zeroing::RelativeEncoderZeroingEstimator>
diff --git a/y2023/control_loops/drivetrain/BUILD b/y2023/control_loops/drivetrain/BUILD
index 8a06d00..ac00e2b 100644
--- a/y2023/control_loops/drivetrain/BUILD
+++ b/y2023/control_loops/drivetrain/BUILD
@@ -1,4 +1,5 @@
load("//aos:config.bzl", "aos_config")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
genrule(
name = "genrule_drivetrain",
@@ -111,3 +112,12 @@
"//frc971/control_loops/drivetrain:trajectory_generator",
],
)
+
+flatbuffer_cc_library(
+ name = "drivetrain_can_position_fbs",
+ srcs = [
+ "drivetrain_can_position.fbs",
+ ],
+ gen_reflections = 1,
+ visibility = ["//visibility:public"],
+)
diff --git a/y2023/control_loops/drivetrain/drivetrain_can_position.fbs b/y2023/control_loops/drivetrain/drivetrain_can_position.fbs
new file mode 100644
index 0000000..10d7c9a
--- /dev/null
+++ b/y2023/control_loops/drivetrain/drivetrain_can_position.fbs
@@ -0,0 +1,39 @@
+namespace y2023.control_loops.drivetrain;
+
+table CANFalcon {
+ // The CAN id of the falcon
+ id:int (id: 0);
+
+ // In Amps
+ supply_current:float (id: 1);
+
+ // In Amps
+ // Stator current where positive current means torque is applied in
+ // the motor's forward direction as determined by its Inverted setting.
+ torque_current:float (id: 2);
+
+ // In Volts
+ supply_voltage:float (id: 3);
+
+ // In degrees Celsius
+ device_temp:float (id: 4);
+
+ // In meters traveled on the drivetrain
+ position:float (id: 5);
+}
+
+// CAN readings from the CAN sensor reader loop
+table CANPosition {
+ falcons: [CANFalcon] (id: 0);
+
+ // The timestamp of the measurement on the canivore clock in nanoseconds
+ // This will have less jitter than the
+ // timestamp of the message being sent out.
+ timestamp:int64 (id: 1);
+
+ // The ctre::phoenix::StatusCode of the measurement
+ // Should be OK = 0
+ status:int (id: 2);
+}
+
+root_type CANPosition;
diff --git a/y2023/control_loops/drivetrain/drivetrain_main.cc b/y2023/control_loops/drivetrain/drivetrain_main.cc
index cf8aa8a..0817b3d 100644
--- a/y2023/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2023/control_loops/drivetrain/drivetrain_main.cc
@@ -19,6 +19,9 @@
std::make_unique<::frc971::control_loops::drivetrain::DeadReckonEkf>(
&event_loop,
::y2023::control_loops::drivetrain::GetDrivetrainConfig());
+ std::unique_ptr<DrivetrainLoop> drivetrain = std::make_unique<DrivetrainLoop>(
+ y2023::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
+ localizer.get());
event_loop.Run();
diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index 77ff5af..690d4bf 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -23,6 +23,7 @@
#undef ERROR
#include "aos/commonmath.h"
+#include "aos/containers/sized_array.h"
#include "aos/events/event_loop.h"
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
@@ -32,8 +33,10 @@
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
+#include "ctre/phoenix/cci/Diagnostics_CCI.h"
#include "ctre/phoenix/motorcontrol/can/TalonFX.h"
#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
+#include "ctre/phoenixpro/TalonFX.hpp"
#include "frc971/autonomous/auto_mode_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/input/robot_state_generated.h"
@@ -51,12 +54,18 @@
#include "frc971/wpilib/sensor_reader.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2023/constants.h"
+#include "y2023/control_loops/drivetrain/drivetrain_can_position_generated.h"
#include "y2023/control_loops/superstructure/superstructure_output_generated.h"
#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
+DEFINE_bool(ctre_diag_server, false,
+ "If true, enable the diagnostics server for interacting with "
+ "devices on the CAN bus using Phoenix Tuner");
+
using ::aos::monotonic_clock;
using ::y2023::constants::Values;
namespace superstructure = ::y2023::control_loops::superstructure;
+namespace drivetrain = ::y2023::control_loops::drivetrain;
namespace chrono = ::std::chrono;
using std::make_unique;
@@ -122,6 +131,16 @@
autonomous_modes_.at(i) = ::std::move(sensor);
}
+ void set_heading_input(::std::unique_ptr<frc::DigitalInput> sensor) {
+ imu_heading_input_ = ::std::move(sensor);
+ imu_heading_reader_.set_input(imu_heading_input_.get());
+ }
+
+ void set_yaw_rate_input(::std::unique_ptr<frc::DigitalInput> sensor) {
+ imu_yaw_rate_input_ = ::std::move(sensor);
+ imu_yaw_rate_reader_.set_input(imu_yaw_rate_input_.get());
+ }
+
void RunIteration() override {
superstructure_reading_->Set(true);
{ auto builder = superstructure_position_sender_.MakeBuilder(); }
@@ -220,6 +239,8 @@
std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
+ std::unique_ptr<frc::DigitalInput> imu_heading_input_, imu_yaw_rate_input_;
+
frc971::wpilib::DMAPulseWidthReader imu_heading_reader_, imu_yaw_rate_reader_;
};
@@ -256,6 +277,326 @@
}
};
+static constexpr int kCANFalconCount = 6;
+static constexpr int kCANSignalsCount = 3;
+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> *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, 3);
+ CHECK_NOTNULL(signals);
+ CHECK_LE(signals->size() + 3u, 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());
+ }
+ }
+
+ 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(35);
+ 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>
+ *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) {
+ 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);
+ }
+
+ private:
+ void Loop() {
+ CHECK_EQ(signals_.size(), 18u);
+ 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]});
+
+ 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_}) {
+ 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>
+ signals_;
+ aos::Sender<drivetrain::CANPosition> can_position_sender_;
+
+ std::shared_ptr<Falcon> right_front_, right_back_, right_under_, left_front_,
+ left_back_, left_under_;
+
+ // Pointer to the timer handler used to modify the wakeup.
+ ::aos::TimerHandler *timer_handler_;
+};
+
+class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler<
+ ::frc971::control_loops::drivetrain::Output> {
+ public:
+ DrivetrainWriter(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::LoopOutputHandler<
+ ::frc971::control_loops::drivetrain::Output>(event_loop,
+ "/drivetrain") {
+ event_loop->SetRuntimeRealtimePriority(
+ constants::Values::kDrivetrainWriterPriority);
+
+ if (!FLAGS_ctre_diag_server) {
+ c_Phoenix_Diagnostics_SetSecondsToStart(-1);
+ c_Phoenix_Diagnostics_Dispose();
+ }
+
+ ctre::phoenix::platform::can::CANComm_SetRxSchedPriority(
+ constants::Values::kDrivetrainRxPriority, true, "Drivetrain Bus");
+ ctre::phoenix::platform::can::CANComm_SetTxSchedPriority(
+ constants::Values::kDrivetrainTxPriority, true, "Drivetrain Bus");
+
+ event_loop->OnRun([this]() { WriteConfigs(); });
+ }
+
+ 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) {
+ 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);
+ }
+
+ void set_right_inverted(ctre::phoenixpro::signals::InvertedValue invert) {
+ right_inverted_ = invert;
+ }
+
+ void set_left_inverted(ctre::phoenixpro::signals::InvertedValue invert) {
+ left_inverted_ = invert;
+ }
+
+ private:
+ void WriteConfigs() {
+ for (auto falcon :
+ {right_front_.get(), right_back_.get(), right_under_.get()}) {
+ falcon->WriteConfigs(right_inverted_);
+ }
+
+ for (auto falcon :
+ {left_front_.get(), left_back_.get(), left_under_.get()}) {
+ falcon->WriteConfigs(left_inverted_);
+ }
+ }
+
+ void Write(
+ const ::frc971::control_loops::drivetrain::Output &output) override {
+ ctre::phoenixpro::controls::DutyCycleOut left_control(
+ SafeSpeed(output.left_voltage()));
+ left_control.UpdateFreqHz = 0_Hz;
+ left_control.EnableFOC = true;
+
+ ctre::phoenixpro::controls::DutyCycleOut right_control(
+ SafeSpeed(output.right_voltage()));
+ right_control.UpdateFreqHz = 0_Hz;
+ right_control.EnableFOC = true;
+
+ for (auto falcon :
+ {left_front_.get(), left_back_.get(), left_under_.get()}) {
+ ctre::phoenix::StatusCode status =
+ falcon->talon()->SetControl(left_control);
+
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to write control to falcon: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
+ }
+
+ for (auto falcon :
+ {right_front_.get(), right_back_.get(), right_under_.get()}) {
+ ctre::phoenix::StatusCode status =
+ falcon->talon()->SetControl(right_control);
+
+ if (!status.IsOK()) {
+ AOS_LOG(ERROR, "Failed to write control to falcon: %s: %s",
+ status.GetName(), status.GetDescription());
+ }
+ }
+ }
+
+ void Stop() override {
+ AOS_LOG(WARNING, "drivetrain output too old\n");
+ ctre::phoenixpro::controls::NeutralOut stop_command;
+ for (auto falcon :
+ {right_front_.get(), right_back_.get(), right_under_.get(),
+ left_front_.get(), left_back_.get(), left_under_.get()}) {
+ falcon->talon()->SetControl(stop_command);
+ }
+ }
+
+ double SafeSpeed(double voltage) {
+ return (::aos::Clip(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
+ }
+
+ ctre::phoenixpro::signals::InvertedValue left_inverted_, right_inverted_;
+ std::shared_ptr<Falcon> right_front_, right_back_, right_under_, left_front_,
+ left_back_, left_under_;
+};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
public:
@@ -272,6 +613,11 @@
aos::configuration::ReadConfig("aos_config.json");
// Thread 1.
+ ::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
+ ::frc971::wpilib::JoystickSender joystick_sender(
+ &joystick_sender_event_loop);
+ AddLoop(&joystick_sender_event_loop);
+
// Thread 2.
::aos::ShmEventLoop pdp_fetcher_event_loop(&config.message());
::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
@@ -288,11 +634,43 @@
sensor_reader.set_drivetrain_right_encoder(make_encoder(0));
sensor_reader.set_superstructure_reading(superstructure_reading);
+ sensor_reader.set_heading_input(make_unique<frc::DigitalInput>(9));
+ sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(8));
+
AddLoop(&sensor_reader_event_loop);
// Thread 4.
+ ::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
+ 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());
+
+ can_sensor_reader.set_falcons(right_front, right_back, right_under,
+ left_front, left_back, left_under);
+
+ AddLoop(&can_sensor_reader_event_loop);
+
+ // Thread 5.
::aos::ShmEventLoop output_event_loop(&config.message());
- ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
+ DrivetrainWriter drivetrain_writer(&output_event_loop);
+
+ drivetrain_writer.set_falcons(right_front, right_back, right_under,
+ left_front, left_back, left_under);
+ drivetrain_writer.set_right_inverted(
+ ctre::phoenixpro::signals::InvertedValue::Clockwise_Positive);
+ drivetrain_writer.set_left_inverted(
+ ctre::phoenixpro::signals::InvertedValue::CounterClockwise_Positive);
SuperstructureWriter superstructure_writer(&output_event_loop);
diff --git a/y2023/y2023_roborio.json b/y2023/y2023_roborio.json
index d0b2251..172940a 100644
--- a/y2023/y2023_roborio.json
+++ b/y2023/y2023_roborio.json
@@ -287,6 +287,14 @@
},
{
"name": "/drivetrain",
+ "type": "y2023.control_loops.drivetrain.CANPosition",
+ "source_node": "roborio",
+ "frequency": 220,
+ "num_senders": 2,
+ "max_size": 400
+ },
+ {
+ "name": "/drivetrain",
"type": "frc971.sensors.GyroReading",
"source_node": "roborio",
"frequency": 200,
@@ -489,7 +497,7 @@
]
},
{
- "name": "web_proxy",
+ "name": "roborio_web_proxy",
"executable_name": "web_proxy_main",
"args": ["--min_ice_port=5800", "--max_ice_port=5810"],
"nodes": [
@@ -505,7 +513,7 @@
]
},
{
- "name": "message_bridge_server",
+ "name": "roborio_message_bridge_server",
"executable_name": "message_bridge_server",
"args": ["--rt_priority=16"],
"nodes": [