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/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);