TalonFXWrapper.hpp

Go to the documentation of this file.

// Copyright (c) 2026 Yet Another Software Suite
// SPDX-License-Identifier: LGPL-3.0-or-later
 
#pragma once
 
#include <any>
#include <ctre/phoenix6/CANcoder.hpp>
#include <ctre/phoenix6/CANdi.hpp>
#include <ctre/phoenix6/TalonFX.hpp>
#include <ctre/phoenix6/controls/DutyCycleOut.hpp>
#include <ctre/phoenix6/controls/Follower.hpp>
#include <ctre/phoenix6/controls/MotionMagicDutyCycle.hpp>
#include <ctre/phoenix6/controls/MotionMagicExpoDutyCycle.hpp>
#include <ctre/phoenix6/controls/MotionMagicExpoVoltage.hpp>
#include <ctre/phoenix6/controls/MotionMagicTorqueCurrentFOC.hpp>
#include <ctre/phoenix6/controls/MotionMagicVelocityDutyCycle.hpp>
#include <ctre/phoenix6/controls/MotionMagicVelocityTorqueCurrentFOC.hpp>
#include <ctre/phoenix6/controls/MotionMagicVelocityVoltage.hpp>
#include <ctre/phoenix6/controls/MotionMagicVoltage.hpp>
#include <ctre/phoenix6/controls/PositionDutyCycle.hpp>
#include <ctre/phoenix6/controls/PositionTorqueCurrentFOC.hpp>
#include <ctre/phoenix6/controls/PositionVoltage.hpp>
#include <ctre/phoenix6/controls/VelocityDutyCycle.hpp>
#include <ctre/phoenix6/controls/VelocityTorqueCurrentFOC.hpp>
#include <ctre/phoenix6/controls/VelocityVoltage.hpp>
#include <ctre/phoenix6/controls/VoltageOut.hpp>
#include <functional>
#include <memory>
#include <optional>
#include <string>
#include <variant>
#include <wpi/simulation/DCMotorSim.hpp>
#include <wpi/units/frequency.hpp>
#include <wpi/util/Alert.hpp>
 
#include "yams/motorcontrollers/SmartMotorController.hpp"
 
namespace yams::motorcontrollers::remote {
 
class TalonFXWrapper : public SmartMotorController {
 public:
  // Expose SetVelocity(velocity, feedforwardForce) from the base class alongside the overrides.
  using SmartMotorController::SetVelocity;
 
  TalonFXWrapper(ctre::phoenix6::hardware::TalonFX* talon, wpi::math::DCMotor dcMotor,
                 SmartMotorControllerConfig* config);
 
  ~TalonFXWrapper();
 
  // ---- TalonFX specific ---------------------------------------------------
  TalonFXWrapper& EnableFOC();
  TalonFXWrapper& DisableFOC();
  bool UseCANdiPWM1() const;
  bool UseCANdiPWM2() const;
  void SetUpdateFrequency(wpi::units::hertz_t frequency);
  ctre::phoenix::StatusCode ForceConfigApply();
 
  // ---- Telemetry ----------------------------------------------------------
  telemetry::UnsupportedTelemetryFields GetUnsupportedTelemetryFields() override;
 
  // ---- Configuration ------------------------------------------------------
  bool ApplyConfig(const SmartMotorControllerConfig& config) override;
 
  // ---- Simulation ---------------------------------------------------------
  void SetupSimulation() override;
  void SimIterate() override;
 
  // ---- Encoder sync -------------------------------------------------------
  void SeedRelativeEncoder() override;
  void SynchronizeRelativeEncoder() override;
 
  // ---- Open-loop outputs --------------------------------------------------
  void SetDutyCycle(double dutyCycle) override;
  double GetDutyCycle() override;
  void SetVoltage(wpi::units::volt_t voltage) override;
  wpi::units::volt_t GetVoltage() override;
  std::optional<wpi::units::second_t> GetVelocityMeasurementDelay() const override;
 
  // ---- Closed-loop setpoints ----------------------------------------------
  void SetPosition(wpi::units::turn_t angle) override;
  void SetPosition(wpi::units::meter_t distance) override;
  void SetVelocity(wpi::units::turns_per_second_t velocity) override;
  void SetVelocity(wpi::units::meters_per_second_t velocity) override;
  void SetVelocity(wpi::units::turns_per_second_t velocity,
                   wpi::units::newton_t feedforwardForce) override;
 
  // ---- Encoder writes -----------------------------------------------------
  void SetEncoderPosition(wpi::units::turn_t angle) override;
  void SetEncoderPosition(wpi::units::meter_t distance) override;
  void SetEncoderVelocity(wpi::units::turns_per_second_t velocity) override;
  void SetEncoderVelocity(wpi::units::meters_per_second_t velocity) override;
 
  // ---- Encoder reads ------------------------------------------------------
  wpi::units::turn_t GetMechanismPosition() override;
  wpi::units::turns_per_second_t GetMechanismVelocity() override;
  wpi::units::turn_t GetRelativeMechanismPosition() override;
  wpi::units::turns_per_second_t GetRelativeMechanismVelocity() override;
  wpi::units::turns_per_second_squared_t GetMechanismAcceleration() override;
  wpi::units::turn_t GetRotorPosition() override;
  wpi::units::turns_per_second_t GetRotorVelocity() override;
  wpi::units::meter_t GetMeasurementPosition() override;
  wpi::units::meters_per_second_t GetMeasurementVelocity() override;
  wpi::units::meters_per_second_squared_t GetMeasurementAcceleration() override;
  std::optional<wpi::units::turn_t> GetExternalEncoderMechanismPosition() override;
  std::optional<wpi::units::turns_per_second_t> GetExternalEncoderMechanismVelocity() override;
 
  // ---- Motor status -------------------------------------------------------
  std::optional<wpi::units::ampere_t> GetSupplyCurrent() override;
  wpi::units::ampere_t GetStatorCurrent() override;
  wpi::units::celsius_t GetTemperature() override;
  wpi::math::DCMotor GetDCMotor() override;
 
  // ---- Configuration setters (live tuning) --------------------------------
  void SetZeroPower(MotorMode mode) override;
  void SetMotorInverted(bool inverted) override;
  void SetEncoderInverted(bool inverted) override;
  void SetKp(double kP) override;
  void SetKi(double kI) override;
  void SetKd(double kD) override;
  void SetFeedback(double kP, double kI, double kD) override;
  void SetKs(double kS) override;
  void SetKv(double kV) override;
  void SetKa(double kA) override;
  void SetKg(double kG) override;
  void SetFeedforward(double kS, double kV, double kA, double kG) override;
  void SetStatorCurrentLimit(wpi::units::ampere_t currentLimit) override;
  void SetSupplyCurrentLimit(wpi::units::ampere_t currentLimit) override;
  void SetClosedLoopRampRate(wpi::units::second_t rampRate) override;
  void SetOpenLoopRampRate(wpi::units::second_t rampRate) override;
  void SetMechanismUpperLimit(wpi::units::turn_t upperLimit) override;
  void SetMechanismLowerLimit(wpi::units::turn_t lowerLimit) override;
  void SetMechanismLimits(wpi::units::turn_t lower, wpi::units::turn_t upper) override;
  void SetMechanismLimitsEnabled(bool enabled) override;
  void SetMeasurementUpperLimit(wpi::units::meter_t upperLimit) override;
  void SetMeasurementLowerLimit(wpi::units::meter_t lowerLimit) override;
  void SetMotionProfileMaxVelocity(wpi::units::turns_per_second_t maxVelocity) override;
  void SetMotionProfileMaxVelocity(wpi::units::meters_per_second_t maxVelocity) override;
  void SetMotionProfileMaxAcceleration(wpi::units::turns_per_second_squared_t maxAcc) override;
  void SetMotionProfileMaxAcceleration(wpi::units::meters_per_second_squared_t maxAcc) override;
  void SetMotionProfileMaxJerk(wpi::units::angular_jerk::turns_per_second_cubed_t maxJerk) override;
  void SetExponentialProfile(std::optional<double> kV, std::optional<double> kA,
                             std::optional<wpi::units::volt_t> maxInput) override;
  void SetClosedLoopSlot(ClosedLoopControllerSlot slot) override;
  void SetMechanismGearing(const gearing::MechanismGearing& gearing) override;
  void SetMechanismCircumference(wpi::units::meter_t circumference) override;
 
  SmartMotorControllerConfig& GetConfig() override;
  void* GetMotorController() override;
  void* GetMotorControllerConfig() override;
 
 private:
  ctre::phoenix6::hardware::TalonFX* m_talon;
  wpi::math::DCMotor m_dcMotor;
  // Whether StatusSignal refreshes should report errors; false in simulation, where status
  // signals are not always updated before they are read.
  const bool m_reportStatusSignalErrors;
  ctre::phoenix6::configs::TalonFXConfiguration m_talonConfig;
 
  // Active closed-loop control requests variant selects the active request type
  using PositionControlRequest = std::variant<
      ctre::phoenix6::controls::PositionVoltage, ctre::phoenix6::controls::PositionDutyCycle,
      ctre::phoenix6::controls::PositionTorqueCurrentFOC,
      ctre::phoenix6::controls::MotionMagicVoltage, ctre::phoenix6::controls::MotionMagicDutyCycle,
      ctre::phoenix6::controls::MotionMagicExpoVoltage,
      ctre::phoenix6::controls::MotionMagicExpoDutyCycle,
      ctre::phoenix6::controls::MotionMagicTorqueCurrentFOC>;
  using VelocityControlRequest =
      std::variant<ctre::phoenix6::controls::VelocityVoltage,
                   ctre::phoenix6::controls::VelocityDutyCycle,
                   ctre::phoenix6::controls::VelocityTorqueCurrentFOC,
                   ctre::phoenix6::controls::MotionMagicVelocityVoltage,
                   ctre::phoenix6::controls::MotionMagicVelocityDutyCycle,
                   ctre::phoenix6::controls::MotionMagicVelocityTorqueCurrentFOC>;
 
  PositionControlRequest m_positionReq{ctre::phoenix6::controls::PositionVoltage{0_tr}};
  VelocityControlRequest m_velocityReq{ctre::phoenix6::controls::VelocityVoltage{0_tps}};
  ctre::phoenix6::controls::VoltageOut m_voltageReq{0_V};
  ctre::phoenix6::controls::DutyCycleOut m_dutyCycleReq{0.0};
 
  // External sensors
  ctre::phoenix6::hardware::CANcoder* m_cancoder{nullptr};
  ctre::phoenix6::hardware::CANdi* m_candi{nullptr};
 
  // Simulation
  std::optional<wpi::sim::DCMotorSim> m_motorSim;
 
  std::optional<wpi::util::Alert> m_rioControllerAlert;
  std::optional<wpi::util::Alert> m_startingPositionExternalEncoderAlert;
  std::optional<wpi::util::Alert> m_zeroOffsetNoExternalEncoderAlert;
  std::optional<wpi::util::Alert> m_feedbackNotAppliedAlert;
  std::optional<wpi::util::Alert> m_feedforwardNotAppliedAlert;
  std::optional<wpi::util::Alert> m_discontinuityPointNoExternalEncoderAlert;
 
  std::string AlertId(const std::string& alertType) const;
  void EnsureRequest(const std::function<ctre::phoenix::StatusCode()>& request);
  template <typename Group>
  ctre::phoenix::StatusCode ApplyGroup(const Group& group,
                                       wpi::units::second_t retryFor = wpi::units::second_t{0.1});
  void SetFOC(bool foc);
  double PositionGainUnitsPerRotation() const;
  double VelocityGainUnitsPerRotation() const;
  double AccelerationGainUnitsPerRotation() const;
  void WriteSlotPID(ClosedLoopControllerSlot slot, double kP, double kI, double kD);
  void WriteSlotGains(const SmartMotorControllerConfig& config);
  void WriteMotionMagic(const SmartMotorControllerConfig& config);
  void WriteSensorRatios(const SmartMotorControllerConfig& config);
  void ApplyExternalEncoder(const SmartMotorControllerConfig& config);
  std::optional<wpi::units::turn_t> GetExternalEncoderSensorPosition();
  std::optional<wpi::units::turns_per_second_t> GetExternalEncoderSensorVelocity();
  void ApplyFollowers(const SmartMotorControllerConfig& config);
  void ApplyVendorControlRequest(const SmartMotorControllerConfig& config);
};
 
}  // namespace yams::motorcontrollers::remote