Rename our Falcons to TalonFX
This is done because both the Falcons and Krakens use a TalonFX motor
controller and our api to use them will be the same.
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: I97249c5583e42f5ca346e754499748e555cd9f8b
diff --git a/frc971/wpilib/talonfx.h b/frc971/wpilib/talonfx.h
new file mode 100644
index 0000000..603f4a2
--- /dev/null
+++ b/frc971/wpilib/talonfx.h
@@ -0,0 +1,110 @@
+#ifndef FRC971_WPILIB_TALONFX_MOTOR_H_
+#define FRC971_WPILIB_TALONFX_MOTOR_H_
+
+#include <chrono>
+#include <cinttypes>
+#include <vector>
+
+#include "ctre/phoenix6/TalonFX.hpp"
+#include "glog/logging.h"
+
+#include "aos/commonmath.h"
+#include "aos/init.h"
+#include "aos/logging/logging.h"
+#include "frc971/control_loops/drivetrain/drivetrain_can_position_generated.h"
+
+namespace control_loops = ::frc971::control_loops;
+
+namespace frc971 {
+namespace wpilib {
+
+struct TalonFXParams {
+ int device_id;
+ bool inverted;
+};
+
+static constexpr units::frequency::hertz_t kCANUpdateFreqHz = 200_Hz;
+static constexpr double kMaxBringupPower = 12.0;
+
+// Class which represents a motor controlled by a TalonFX motor controller over
+// CAN.
+class TalonFX {
+ public:
+ TalonFX(int device_id, bool inverted, std::string canbus,
+ std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
+ double stator_current_limit, double supply_current_limit);
+
+ TalonFX(TalonFXParams params, std::string canbus,
+ std::vector<ctre::phoenix6::BaseStatusSignal *> *signals,
+ double stator_current_limit, double supply_current_limit);
+
+ void PrintConfigs();
+
+ void WriteConfigs();
+ ctre::phoenix::StatusCode WriteCurrent(double current, double max_voltage);
+
+ ctre::phoenix::StatusCode WriteVoltage(double voltage);
+
+ ctre::phoenix6::hardware::TalonFX *talon() { return &talon_; }
+
+ // The position of the TalonFX output shaft is multiplied by gear_ratio
+ void SerializePosition(flatbuffers::FlatBufferBuilder *fbb,
+ double gear_ratio);
+
+ std::optional<flatbuffers::Offset<control_loops::CANTalonFX>> TakeOffset();
+
+ 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 duty_cycle() const { return duty_cycle_.GetValue().value(); }
+ float position() const {
+ return static_cast<units::angle::radian_t>(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(); };
+
+ void set_stator_current_limit(double stator_current_limit) {
+ stator_current_limit_ = stator_current_limit;
+ }
+
+ void set_supply_current_limit(double supply_current_limit) {
+ supply_current_limit_ = supply_current_limit;
+ }
+
+ static double SafeSpeed(double voltage) {
+ return (::aos::Clip(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
+ }
+
+ private:
+ ctre::phoenix6::hardware::TalonFX talon_;
+ int device_id_;
+
+ ctre::phoenix6::signals::InvertedValue inverted_;
+
+ ctre::phoenix6::StatusSignal<units::temperature::celsius_t> device_temp_;
+ ctre::phoenix6::StatusSignal<units::voltage::volt_t> supply_voltage_;
+ ctre::phoenix6::StatusSignal<units::current::ampere_t> supply_current_,
+ torque_current_;
+ ctre::phoenix6::StatusSignal<units::angle::turn_t> position_;
+ ctre::phoenix6::StatusSignal<units::dimensionless::scalar_t> duty_cycle_;
+
+ double stator_current_limit_;
+ double supply_current_limit_;
+
+ std::optional<flatbuffers::Offset<control_loops::CANTalonFX>>
+ last_position_offset_;
+};
+} // namespace wpilib
+} // namespace frc971
+#endif // FRC971_WPILIB_TALONFX_MOTOR_H_