PhoenixController.h

 
#pragma once
 
#include <ctime>
#include <ctre/phoenix6/sim/ChassisReference.hpp>
#include <memory>
#include <string>
#include <unordered_map>
 
#include <ctre/phoenix6/CANBus.hpp>
#include <ctre/phoenix6/CANcoder.hpp>
#include <ctre/phoenix6/TalonFX.hpp>
#include <units/voltage.h>
#include <frc/RobotController.h>
 
#include "valkyrie/controllers/BaseController.h"
#include "valkyrie/util/PhoenixSignalManager.h"
 
// Conversion guide:
// https://v6.docs.ctr-electronics.com/en/latest/docs/migration/migration-guide/closed-loop-guide.html
 
namespace valor {
 
enum PhoenixControllerType { KRAKEN_X60_FOC, KRAKEN_X60, KRAKEN_X44, KRAKEN_X44_FOC, FALCON_FOC, FALCON };
 
template <class VelocityOutput = ctre::phoenix6::controls::VelocityVoltage,
          class PositionOutput = ctre::phoenix6::controls::DynamicMotionMagicVoltage>
#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable : 4250)  // Disable "inherits via dominance" warning
#endif
class PhoenixController : public BaseController, public ctre::phoenix6::hardware::core::CoreTalonFX {
#ifdef _MSC_VER
#pragma warning(pop)
#endif
    static_assert(std::is_base_of_v<ctre::phoenix6::controls::ControlRequest, VelocityOutput>);
    static_assert(std::is_base_of_v<ctre::phoenix6::controls::ControlRequest, PositionOutput>);
 
    static inline const std::unordered_map<PhoenixControllerType, frc::DCMotor> MOTOR_SPEC_MAP{
        {KRAKEN_X60, frc::DCMotor::KrakenX60()}, {KRAKEN_X60_FOC, frc::DCMotor::KrakenX60FOC()},
        {KRAKEN_X44, frc::DCMotor::KrakenX44()}, {KRAKEN_X44_FOC, frc::DCMotor::KrakenX44FOC()},
        {FALCON, frc::DCMotor::Falcon500()},     {FALCON_FOC, frc::DCMotor::Falcon500FOC()}};
 
   public:
    PhoenixController(valor::PhoenixControllerType controllerType, int canID, valor::NeutralMode neutralMode, bool inverted,
                      std::string canbus = "", units::millisecond_t _loggingPeriod = 100_ms)
          : BaseController{MOTOR_SPEC_MAP.at(controllerType)},
            ctre::phoenix6::hardware::core::CoreTalonFX{canID, ctre::phoenix6::CANBus{canbus}} {
        // Remove default LiveWindow entry for TalonFX
        valor::PIDF pidf;
        pidf.P = FALCON_PIDF_KP;
        pidf.I = FALCON_PIDF_KI;
        pidf.D = FALCON_PIDF_KD;
        pidf.error = 0_tr;
        pidf.maxVelocity = FALCON_PIDF_KV;
        pidf.maxAcceleration = FALCON_PIDF_KA;
 
        config.MotorOutput.Inverted = inverted;
 
        req_pos_out.Slot = 0;
        req_pos_out.UpdateFreqHz = 0_Hz;
        req_vel_out.Slot = 0;
        req_vel_out.UpdateFreqHz = 0_Hz;
 
        PhoenixController::setNeutralMode(neutralMode);
        PhoenixController::setCurrentLimits(STATOR_CURRENT_LIMIT, SUPPLY_CURRENT_LIMIT, SUPPLY_CURRENT_THRESHOLD, SUPPLY_TIME_THRESHOLD);
        PhoenixController::setPIDF(pidf);
        util::PhoenixSignalManager::GetInstance().AddSignals(GetNetwork(), GetPosition(false), GetVelocity(false), GetStatorCurrent(false),
                                                             GetMotorVoltage(false));
 
        GetSimState().Orientation = inverted ? ctre::phoenix6::sim::ChassisReference::Clockwise_Positive
                                             : ctre::phoenix6::sim::ChassisReference::CounterClockwise_Positive;
 
        if (controllerType == KRAKEN_X44 || controllerType == KRAKEN_X44_FOC)
            GetSimState().SetMotorType(ctre::phoenix6::sim::TalonFXSimState::MotorType::KrakenX44);
        else
            GetSimState().SetMotorType(ctre::phoenix6::sim::TalonFXSimState::MotorType::KrakenX60);
 
        setLoggingPeriod(_loggingPeriod);
    };
 
    void enableFOC(bool enableFOC) { focEnabled = enableFOC; }
 
    void applyConfig() override { GetConfigurator().Apply(config, 5_s); }
 
    void reset() override { setEncoderPosition(0_tr); }
 
    void setNeutralMode(valor::NeutralMode mode, bool saveImmediately = false) {
        config.MotorOutput.DutyCycleNeutralDeadband = DEADBAND.value();
        config.MotorOutput.NeutralMode = mode == valor::NeutralMode::Brake ? ctre::phoenix6::signals::NeutralModeValue::Brake
                                                                           : ctre::phoenix6::signals::NeutralModeValue::Coast;
 
        if (saveImmediately)
            GetConfigurator().Apply(config.MotorOutput);
    }
 
    void setCurrentLimits(units::ampere_t statorCurrentLimit, units::ampere_t supplyCurrentLimit, units::ampere_t supplyCurrentThreshold,
                          units::second_t supplyTimeThreshold, bool saveImmediately = false) {
        config.CurrentLimits.StatorCurrentLimit = statorCurrentLimit;
        config.CurrentLimits.StatorCurrentLimitEnable = true;
        config.CurrentLimits.SupplyCurrentLimit = supplyCurrentLimit;
        config.CurrentLimits.SupplyCurrentLimitEnable = true;
        config.CurrentLimits.SupplyCurrentLowerTime = supplyTimeThreshold;
        config.CurrentLimits.SupplyCurrentLowerLimit = supplyCurrentThreshold;
        config.TorqueCurrent.PeakForwardTorqueCurrent = PEAK_TORQUE_CURRENT;
        config.TorqueCurrent.PeakReverseTorqueCurrent = -PEAK_TORQUE_CURRENT;
 
        if (saveImmediately) {
            GetConfigurator().Apply(config.CurrentLimits);
            GetConfigurator().Apply(config.TorqueCurrent);
        }
    }
 
    void setVoltageLimits(units::volt_t reverseLimit, units::volt_t forwardLimit, bool saveImmediately = false) {
        config.Voltage.PeakForwardVoltage = forwardLimit;
        config.Voltage.PeakReverseVoltage = reverseLimit;
 
        if (saveImmediately) {
            GetConfigurator().Apply(config.Voltage);
        }
    }
 
    units::turn_t getPosition() override { return GetPosition(false).GetValue(); }
 
    units::turns_per_second_t getSpeed() override { return GetVelocity(false).GetValue(); }
 
    units::ampere_t getCurrent() override { return GetStatorCurrent(false).GetValue(); }
 
    void setEncoderPosition(units::turn_t position) override { SetPosition(position); }
 
    void setContinuousWrap(bool continuousWrap, bool saveImmediately = false) {
        config.ClosedLoopGeneral.ContinuousWrap = continuousWrap;
 
        if (saveImmediately)
            GetConfigurator().Apply(config.ClosedLoopGeneral);
    }
 
    void positionSetter(units::turn_t position, int slot = 0) override {
        WriteLog("reqPosition", position);
        SetControl(tryEnableFOC(req_pos_out.WithSlot(slot).WithPosition(position)));
    }
 
    void setPositionWithFeedForward(units::turn_t position, units::turns_per_second_t feedforwardVelocity, int slot = 0) override {
        if (getOpMode() != OperationalMode::Operational) {
            BaseController::setPositionWithFeedForward(position, feedforwardVelocity, slot);
            return;
        }
        // Convert velocity to voltage: kV * velocity + kS * sign(velocity)
        double kV = (voltageCompensation / getMaxMechSpeed()).value();
        units::volt_t ff = units::volt_t{kV * feedforwardVelocity.value()};
        if (units::math::abs(feedforwardVelocity) > 0.01_tps) {
            ff += units::volt_t{req_pos_out.Slot < 2 ? (req_pos_out.Slot == 1 ? config.Slot1.kS : config.Slot0.kS) : config.Slot2.kS} *
                  (feedforwardVelocity.value() > 0 ? 1.0 : -1.0);
        }
        WriteLog("reqPosition", position);
        WriteLog("reqFeedForward", ff);
        req_pos_out.FeedForward = ff;
        SetControl(tryEnableFOC(req_pos_out.WithSlot(slot).WithPosition(position)));
    }
 
    using BaseController::setPosition;
    void setPosition(units::turn_t position, units::turns_per_second_t maxVelocity, units::turns_per_second_squared_t maxAcceleration,
                     units::turns_per_second_cubed_t maxJerk, int slot = 0) {
        req_pos_out.Velocity = maxVelocity;
        req_pos_out.Acceleration = maxAcceleration;
        req_pos_out.Jerk = maxJerk;
        setPosition(position, slot);
    }
 
    void speedSetter(units::turns_per_second_t velocity, int slot = 0) override {
        WriteLog("reqSpeed", velocity);
        SetControl(tryEnableFOC(req_vel_out.WithSlot(slot).WithVelocity(velocity)));
    }
 
    void powerSetter(double out) override { SetControl(ctre::phoenix6::controls::DutyCycleOut{out}.WithEnableFOC(focEnabled)); }
    void powerSetter(units::volt_t out) override { SetControl(ctre::phoenix6::controls::VoltageOut{out}.WithEnableFOC(focEnabled)); }
    void powerSetter(units::ampere_t out) override { SetControl(ctre::phoenix6::controls::TorqueCurrentFOC{out}); }
 
    void useTimesync(units::hertz_t controlFreq, bool saveImmediately = false) {
        config.MotorOutput.ControlTimesyncFreqHz = controlFreq;
        req_pos_out.UseTimesync = true;
        req_vel_out.UseTimesync = true;
 
        if (saveImmediately)
            GetConfigurator().Apply(config.MotorOutput);
    }
 
    void setupFollower(int canID, bool followerInverted = false) override {
        followerMotor = std::make_unique<PhoenixController<>>(
            PhoenixControllerType::KRAKEN_X60_FOC, canID,
            config.MotorOutput.NeutralMode == ctre::phoenix6::signals::NeutralModeValue::Brake ? NeutralMode::Brake : NeutralMode::Coast,
            followerInverted, std::string{GetNetwork().GetName()});
        followerMotor->applyConfig();
        followerMotor->SetControl(ctre::phoenix6::controls::StrictFollower(GetDeviceID()));
        LogChild("Follower", followerMotor.get());
    }
 
    void setPIDF(valor::PIDF pidf, int slot = 0, bool saveImmediately = false) override {
        if constexpr (requires { req_pos_out.Jerk; }) {
            req_pos_out.Jerk = pidf.maxJerk;
            req_pos_out.Acceleration = pidf.maxAcceleration;
            req_pos_out.Velocity = pidf.maxVelocity;
        }
        config.MotionMagic.MotionMagicJerk = pidf.maxJerk;
        config.MotionMagic.MotionMagicAcceleration = pidf.maxAcceleration;
        config.MotionMagic.MotionMagicCruiseVelocity = pidf.maxVelocity;
 
        if (saveImmediately) {
            GetConfigurator().Apply(config.MotionMagic);
        }
 
        if (slot == 1)
            setPIDFSlot(config.Slot1, pidf, saveImmediately);
        else if (slot == 2)
            setPIDFSlot(config.Slot2, pidf, saveImmediately);
        else
            setPIDFSlot(config.Slot0, pidf, saveImmediately);  // Default case
    }
 
    void setupReverseHardwareLimit(int canID, ctre::phoenix6::signals::ReverseLimitTypeValue type, units::turn_t autosetPosition = 0_tr,
                                   bool saveImmediately = false) {
        config.HardwareLimitSwitch.ReverseLimitType = type;
        config.HardwareLimitSwitch.ReverseLimitEnable = true;
        config.HardwareLimitSwitch.ReverseLimitRemoteSensorID = canID;
        config.HardwareLimitSwitch.ReverseLimitAutosetPositionValue = autosetPosition;
        config.HardwareLimitSwitch.ReverseLimitAutosetPositionEnable = false;
        config.HardwareLimitSwitch.ReverseLimitSource = ctre::phoenix6::signals::ReverseLimitSourceValue::RemoteCANdiS1;
 
        if (saveImmediately)
            GetConfigurator().Apply(config.HardwareLimitSwitch);
    }
 
    void setupForwardHardwareLimit(int canID, ctre::phoenix6::signals::ForwardLimitTypeValue type, units::turn_t autosetPosition = 0_tr,
                                   bool saveImmediately = false) {
        config.HardwareLimitSwitch.ForwardLimitType = type;
        config.HardwareLimitSwitch.ForwardLimitEnable = true;
        config.HardwareLimitSwitch.ForwardLimitRemoteSensorID = canID;
        config.HardwareLimitSwitch.ForwardLimitAutosetPositionValue = autosetPosition;
        config.HardwareLimitSwitch.ForwardLimitAutosetPositionEnable = true;
        config.HardwareLimitSwitch.ForwardLimitSource = ctre::phoenix6::signals::ReverseLimitSourceValue::RemoteCANdiS1;
 
        if (saveImmediately)
            GetConfigurator().Apply(config.HardwareLimitSwitch);
    }
 
    void setForwardLimit(units::turn_t forward, bool saveImmediately = false) override {
        config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
        config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = forward;
 
        if (saveImmediately)
            GetConfigurator().Apply(config.SoftwareLimitSwitch);
    }
 
    void setReverseLimit(units::turn_t reverse, bool saveImmediately = false) override {
        config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
        config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = reverse;
 
        if (saveImmediately)
            GetConfigurator().Apply(config.SoftwareLimitSwitch);
    }
 
    void setGearRatios(double _rotorToSensor, double _sensorToMech, bool saveImmediately = false) override {
        rotorToSensor = _rotorToSensor;
        sensorToMech = _sensorToMech;
 
        config.Feedback.RotorToSensorRatio = rotorToSensor;
        config.Feedback.SensorToMechanismRatio = sensorToMech;
 
        if (saveImmediately)
            GetConfigurator().Apply(config.Feedback);
    }
 
    ctre::phoenix6::signals::MagnetHealthValue getMagnetHealth() {
        if (!cancoder)
            return ctre::phoenix6::signals::MagnetHealthValue::Magnet_Invalid;
        return cancoder->GetMagnetHealth(false).GetValue();
    }
 
    units::turn_t getAbsEncoderPosition() override {
        if (cancoder == nullptr)
            return 0_tr;
        return cancoder->GetAbsolutePosition(false).GetValue();
    }
 
    void setupCANCoder(int deviceId, units::turn_t offset, bool clockwise, std::string canbus = "", units::turn_t absoluteRange = 1_tr,
                       bool saveImmediately = false) {
        cancoder = std::make_unique<ctre::phoenix6::hardware::CANcoder>(deviceId, ctre::phoenix6::CANBus{canbus});
        wpi::SendableRegistry::Remove(cancoder.get());
 
        cancoderConfig = std::make_unique<ctre::phoenix6::configs::MagnetSensorConfigs>();
        cancoderConfig->AbsoluteSensorDiscontinuityPoint = absoluteRange;
        cancoderConfig->SensorDirection = clockwise ? ctre::phoenix6::signals::SensorDirectionValue::Clockwise_Positive
                                                    : ctre::phoenix6::signals::SensorDirectionValue::CounterClockwise_Positive;
        cancoderConfig->MagnetOffset = -offset;
        cancoder->GetConfigurator().Apply(*cancoderConfig);
 
        config.Feedback.FeedbackRemoteSensorID = cancoder->GetDeviceID();
        config.Feedback.FeedbackSensorSource = ctre::phoenix6::signals::FeedbackSensorSourceValue::FusedCANcoder;
 
        cancoder->GetSimState().Orientation = clockwise ? ctre::phoenix6::sim::ChassisReference::Clockwise_Positive
                                                        : ctre::phoenix6::sim::ChassisReference::CounterClockwise_Positive;
 
        util::PhoenixSignalManager::GetInstance().AddSignals(cancoder->GetNetwork(), cancoder->GetAbsolutePosition(),
                                                             cancoder->GetMagnetHealth(), cancoder->GetFault_BadMagnet());
 
        if (saveImmediately)
            GetConfigurator().Apply(config.Feedback);
    }
 
    ctre::phoenix6::hardware::CANcoder* getCANCoder() { return cancoder.get(); }
 
    float getBusUtil(const char* canBusName) {
        // @todo Initialize a CANBus, and get utilization
        return 0;
    }
 
    void setOpenLoopRamp(units::second_t time, bool saveImmediately = false) override {
        config.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = time;
        if (saveImmediately)
            GetConfigurator().Apply(config.OpenLoopRamps);
    }
 
    units::volt_t calculateAppliedVoltage() override {
        GetSimState().SetSupplyVoltage(frc::RobotController::GetBatteryVoltage());
        if (cancoder)
            cancoder->GetSimState().SetSupplyVoltage(frc::RobotController::GetBatteryVoltage());
 
        // Return the motor's applied voltage
        return GetSimState().GetMotorVoltage();
    }
 
    void setSimState(frc::sim::DCMotorSim& sim) override {
        GetSimState().SetRawRotorPosition(sim.GetAngularPosition() * rotorToSensor * sensorToMech);
        GetSimState().SetRotorVelocity(sim.GetAngularVelocity() * rotorToSensor * sensorToMech);
        GetSimState().SetRotorAcceleration(sim.GetAngularAcceleration() * rotorToSensor * sensorToMech);
 
        /*
        The + cancoderMagnetOffset is needed because the magnet offset is needed in
        physical space to align motor and cancoder zero. SetRawPosition bypasses the
        magnet offset, which means that the magnet offset is applied on top of it
        and skews the reported position of the cancoder, which by proxy skews the
        position of the motor that is using it as a feedback sensor.
        */
        if (cancoder) {
            cancoder->GetSimState().SetRawPosition(sim.GetAngularPosition() * sensorToMech - cancoderConfig->MagnetOffset);
            cancoder->GetSimState().SetVelocity(sim.GetAngularVelocity() * sensorToMech);
        }
    }
 
    bool GetFault_BadMagnet(bool refresh = true) {
        if (cancoder) {
            auto& faultSignal = cancoder->GetFault_BadMagnet();
            if (refresh) {
                faultSignal.Refresh();
            }
            return faultSignal.GetValue();
        }
        return false;
    }
 
    void LoggablePeriodic() override {
        WriteLog("Stator Current", GetStatorCurrent(false).GetValue());
        WriteLog("Position", GetPosition(false).GetValue());
        WriteLog("Speed", GetVelocity(false).GetValue());
        WriteLog("Out Volt", GetMotorVoltage(false).GetValue());
        if (cancoder)
            WriteLog("Absolute Position", cancoder->GetAbsolutePosition(false).GetValue());
    }
 
   private:
    static constexpr units::ampere_t SUPPLY_CURRENT_THRESHOLD = 65_A;
    static constexpr units::ampere_t STATOR_CURRENT_LIMIT = 80_A;
    static constexpr units::ampere_t SUPPLY_CURRENT_LIMIT = 80_A;
    static constexpr units::millisecond_t SUPPLY_TIME_THRESHOLD = 1000_ms;
    static constexpr units::ampere_t PEAK_TORQUE_CURRENT = 100_A;
    static constexpr units::turn_t DEADBAND = 0.01_tr;
 
    static constexpr double FALCON_PIDF_KP = 10.0f;
    static constexpr double FALCON_PIDF_KI = 0.0f;
    static constexpr double FALCON_PIDF_KD = 0.0f;
    static constexpr units::turns_per_second_t FALCON_PIDF_KV = 6_tps;
    static constexpr units::turns_per_second_squared_t FALCON_PIDF_KA = 130_tr_per_s_sq;
    static constexpr units::turns_per_second_cubed_t FALCON_PIDF_KJ = 650_tr_per_s_cu;
 
    template <class S>
    void setPIDFSlot(S& slot, valor::PIDF pidf, bool saveImmediately) {
        // Generic PIDF configurations
        // Numerator for closed loop controls will be in volts
        slot.kP = pidf.P;
        slot.kI = pidf.I;
        slot.kD = pidf.D;
        slot.kS = pidf.S;
        slot.kV = pidf.kV.value_or((voltageCompensation / getMaxMechSpeed()).value());
 
        // Feedforward gain configuration
        if (pidf.aFF != 0) {
            slot.GravityArmPositionOffset = pidf.aFFTarget;
            slot.GravityType = pidf.aFFType == valor::FeedForwardType::LINEAR ? ctre::phoenix6::signals::GravityTypeValue::Elevator_Static
                                                                              : ctre::phoenix6::signals::GravityTypeValue::Arm_Cosine;
            slot.kG = pidf.aFF;
        }
 
        if (saveImmediately)
            GetConfigurator().Apply(slot);
    }
 
    // Helper function for enableFOC that only enables if the template has the field "EnableFOC"
    template <typename T>
    inline T tryEnableFOC(T request) {
        if constexpr (requires { request.EnableFOC; })
            request.EnableFOC = focEnabled;
        return request;
    }
 
    inline PositionOutput createPositionRequest() {
        if constexpr (requires { PositionOutput{0_tr}; })
            return {0_tr};
        else
            return {0_tr, 0_tps, 0_tr_per_s_sq};
    }
 
    std::unique_ptr<PhoenixController<>> followerMotor;
 
    VelocityOutput req_vel_out{0_tps};
    PositionOutput req_pos_out = createPositionRequest();
 
    std::unique_ptr<ctre::phoenix6::hardware::CANcoder> cancoder;
    std::unique_ptr<ctre::phoenix6::configs::MagnetSensorConfigs> cancoderConfig;
 
    ctre::phoenix6::configs::TalonFXConfiguration config;
 
    bool focEnabled = true;
};
 
}  // namespace valor