001/*
002 * Copyright (C) Cross The Road Electronics.  All rights reserved.
003 * License information can be found in CTRE_LICENSE.txt
004 * For support and suggestions contact support@ctr-electronics.com or file
005 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases
006 */
007package com.ctre.phoenix6.mechanisms.swerve;
008
009import com.ctre.phoenix6.BaseStatusSignal;
010import com.ctre.phoenix6.StatusCode;
011import com.ctre.phoenix6.StatusSignal;
012import com.ctre.phoenix6.configs.CANcoderConfiguration;
013import com.ctre.phoenix6.configs.MotorOutputConfigs;
014import com.ctre.phoenix6.configs.TalonFXConfiguration;
015import com.ctre.phoenix6.controls.MotionMagicExpoTorqueCurrentFOC;
016import com.ctre.phoenix6.controls.MotionMagicExpoVoltage;
017import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC;
018import com.ctre.phoenix6.controls.MotionMagicVoltage;
019import com.ctre.phoenix6.controls.TorqueCurrentFOC;
020import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
021import com.ctre.phoenix6.controls.VelocityVoltage;
022import com.ctre.phoenix6.controls.VoltageOut;
023import com.ctre.phoenix6.hardware.CANcoder;
024import com.ctre.phoenix6.hardware.TalonFX;
025import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
026import com.ctre.phoenix6.signals.InvertedValue;
027import com.ctre.phoenix6.signals.NeutralModeValue;
028
029import edu.wpi.first.math.geometry.Rotation2d;
030import edu.wpi.first.math.kinematics.SwerveModulePosition;
031import edu.wpi.first.math.kinematics.SwerveModuleState;
032import edu.wpi.first.math.util.Units;
033import edu.wpi.first.units.measure.Angle;
034import edu.wpi.first.units.measure.AngularVelocity;
035
036/**
037 * Swerve Module class that encapsulates a swerve module powered by CTR
038 * Electronics devices.
039 * <p>
040 * This class handles the hardware devices and configures them for
041 * swerve module operation using the Phoenix 6 API.
042 * <p>
043 * This class constructs hardware devices internally, so the user
044 * only specifies the constants (IDs, PID gains, gear ratios, etc).
045 * Getters for these hardware devices are available.
046 */
047public class LegacySwerveModule {
048    /**
049     * Supported closed-loop output types.
050     */
051    public enum ClosedLoopOutputType {
052        Voltage,
053        /** Requires Pro */
054        TorqueCurrentFOC,
055    }
056
057    /**
058     * All possible control requests for the module steer motor.
059     */
060    public enum SteerRequestType {
061        /**
062         * Control the drive motor using a Motion Magic® request.
063         * The control output type is determined by {@link LegacySwerveModuleConstants#SteerMotorClosedLoopOutput}
064         */
065        MotionMagic,
066        /**
067         * Control the drive motor using a Motion Magic® Expo request.
068         * The control output type is determined by {@link LegacySwerveModuleConstants#SteerMotorClosedLoopOutput}
069         */
070        MotionMagicExpo,
071    }
072
073    /**
074     * All possible control requests for the module drive motor.
075     */
076    public enum DriveRequestType {
077        /**
078         * Control the drive motor using an open-loop voltage request.
079         */
080        OpenLoopVoltage,
081        /**
082         * Control the drive motor using a velocity closed-loop request.
083         * The control output type is determined by {@link LegacySwerveModuleConstants#DriveMotorClosedLoopOutput}
084         */
085        Velocity,
086    }
087
088    private final TalonFX m_driveMotor;
089    private final TalonFX m_steerMotor;
090    private final CANcoder m_cancoder;
091
092    private final StatusSignal<Angle> m_drivePosition;
093    private final StatusSignal<AngularVelocity> m_driveVelocity;
094    private final StatusSignal<Angle> m_steerPosition;
095    private final StatusSignal<AngularVelocity> m_steerVelocity;
096    private final BaseStatusSignal[] m_signals;
097    private final double m_driveRotationsPerMeter;
098    private final double m_couplingRatioDriveRotorToCANcoder;
099
100    private final double m_speedAt12VoltsMps;
101
102    /* steer motor controls */
103    private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0);
104    private final MotionMagicTorqueCurrentFOC m_angleTorqueSetter = new MotionMagicTorqueCurrentFOC(0);
105    private final MotionMagicExpoVoltage m_angleVoltageExpoSetter = new MotionMagicExpoVoltage(0);
106    private final MotionMagicExpoTorqueCurrentFOC m_angleTorqueExpoSetter = new MotionMagicExpoTorqueCurrentFOC(0);
107    /* drive motor controls */
108    private final VoltageOut m_voltageOpenLoopSetter = new VoltageOut(0);
109    private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0);
110    /* Velocity Torque current neutral should always be coast, as neutral corresponds to 0-current or maintain velocity, not 0-velocity */
111    private final VelocityTorqueCurrentFOC m_velocityTorqueSetter = new VelocityTorqueCurrentFOC(0).withOverrideCoastDurNeutral(true);
112
113    private final ClosedLoopOutputType m_steerClosedLoopOutput;
114    private final ClosedLoopOutputType m_driveClosedLoopOutput;
115
116    private final SwerveModulePosition m_internalState = new SwerveModulePosition();
117    private SwerveModuleState m_targetState = new SwerveModuleState();
118
119    /**
120     * Construct a LegacySwerveModule with the specified constants.
121     *
122     * @param constants   Constants used to construct the module
123     * @param canbusName  The name of the CAN bus this module is on
124     */
125    public LegacySwerveModule(LegacySwerveModuleConstants constants, String canbusName) {
126        m_driveMotor = new TalonFX(constants.DriveMotorId, canbusName);
127        m_steerMotor = new TalonFX(constants.SteerMotorId, canbusName);
128        m_cancoder = new CANcoder(constants.CANcoderId, canbusName);
129
130        TalonFXConfiguration driveConfigs = constants.DriveMotorInitialConfigs;
131        driveConfigs.MotorOutput.NeutralMode = NeutralModeValue.Brake;
132
133        driveConfigs.Slot0 = constants.DriveMotorGains;
134        driveConfigs.TorqueCurrent.PeakForwardTorqueCurrent = constants.SlipCurrent;
135        driveConfigs.TorqueCurrent.PeakReverseTorqueCurrent = -constants.SlipCurrent;
136        driveConfigs.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent;
137        driveConfigs.CurrentLimits.StatorCurrentLimitEnable = true;
138
139        driveConfigs.MotorOutput.Inverted = constants.DriveMotorInverted ? InvertedValue.Clockwise_Positive
140                : InvertedValue.CounterClockwise_Positive;
141        StatusCode response = m_driveMotor.getConfigurator().apply(driveConfigs);
142        if (!response.isOK()) {
143            System.out.println(
144                    "TalonFX ID " + m_driveMotor.getDeviceID() + " failed config with error " + response.toString());
145        }
146
147        TalonFXConfiguration steerConfigs = constants.SteerMotorInitialConfigs;
148        steerConfigs.MotorOutput.NeutralMode = NeutralModeValue.Brake;
149
150        steerConfigs.Slot0 = constants.SteerMotorGains;
151        // Modify configuration to use remote CANcoder fused
152        steerConfigs.Feedback.FeedbackRemoteSensorID = constants.CANcoderId;
153        switch (constants.FeedbackSource) {
154            case RemoteCANcoder:
155                steerConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder;
156                break;
157            case FusedCANcoder:
158                steerConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
159                break;
160            case SyncCANcoder:
161                steerConfigs.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.SyncCANcoder;
162                break;
163        }
164        steerConfigs.Feedback.RotorToSensorRatio = constants.SteerMotorGearRatio;
165
166        steerConfigs.MotionMagic.MotionMagicCruiseVelocity = 100.0 / constants.SteerMotorGearRatio;
167        steerConfigs.MotionMagic.MotionMagicAcceleration = steerConfigs.MotionMagic.MotionMagicCruiseVelocity / 0.100;
168        steerConfigs.MotionMagic.MotionMagicExpo_kV = 0.12 * constants.SteerMotorGearRatio;
169        steerConfigs.MotionMagic.MotionMagicExpo_kA = 1.2 / constants.SteerMotorGearRatio;
170
171        steerConfigs.ClosedLoopGeneral.ContinuousWrap = true; // Enable continuous wrap for swerve modules
172
173        steerConfigs.MotorOutput.Inverted = constants.SteerMotorInverted
174                ? InvertedValue.Clockwise_Positive
175                : InvertedValue.CounterClockwise_Positive;
176        response = m_steerMotor.getConfigurator().apply(steerConfigs);
177        if (!response.isOK()) {
178            System.out.println(
179                    "TalonFX ID " + m_steerMotor.getDeviceID() + " failed config with error: " + response.toString());
180        }
181
182        CANcoderConfiguration cancoderConfigs = constants.CANcoderInitialConfigs;
183        cancoderConfigs.MagnetSensor.MagnetOffset = constants.CANcoderOffset;
184        response = m_cancoder.getConfigurator().apply(cancoderConfigs);
185        if (!response.isOK()) {
186            System.out.println(
187                    "CANcoder ID " + m_cancoder.getDeviceID() + " failed config with error: " + response.toString());
188        }
189
190        m_drivePosition = m_driveMotor.getPosition().clone();
191        m_driveVelocity = m_driveMotor.getVelocity().clone();
192        m_steerPosition = m_steerMotor.getPosition().clone();
193        m_steerVelocity = m_steerMotor.getVelocity().clone();
194
195        m_signals = new BaseStatusSignal[4];
196        m_signals[0] = m_drivePosition;
197        m_signals[1] = m_driveVelocity;
198        m_signals[2] = m_steerPosition;
199        m_signals[3] = m_steerVelocity;
200
201        /* Calculate the ratio of drive motor rotation to meter on ground */
202        double rotationsPerWheelRotation = constants.DriveMotorGearRatio;
203        double metersPerWheelRotation = 2 * Math.PI * Units.inchesToMeters(constants.WheelRadius);
204        m_driveRotationsPerMeter = rotationsPerWheelRotation / metersPerWheelRotation;
205        m_couplingRatioDriveRotorToCANcoder = constants.CouplingGearRatio;
206
207        /* Make control requests synchronous */
208        m_angleVoltageSetter.UpdateFreqHz = 0;
209        m_angleTorqueSetter.UpdateFreqHz = 0;
210        m_angleVoltageExpoSetter.UpdateFreqHz = 0;
211        m_angleTorqueExpoSetter.UpdateFreqHz = 0;
212
213        m_velocityTorqueSetter.UpdateFreqHz = 0;
214        m_velocityVoltageSetter.UpdateFreqHz = 0;
215        m_voltageOpenLoopSetter.UpdateFreqHz = 0;
216
217        /* Set the drive motor closed-loop output type */
218        m_steerClosedLoopOutput = constants.SteerMotorClosedLoopOutput;
219        m_driveClosedLoopOutput = constants.DriveMotorClosedLoopOutput;
220
221        /* Get the expected speed when applying 12 volts */
222        m_speedAt12VoltsMps = constants.SpeedAt12VoltsMps;
223    }
224
225    /**
226     * Gets the state of this module and passes it back as a
227     * SwerveModulePosition object with latency compensated values.
228     *
229     * @param refresh True if the signals should be refreshed
230     * @return SwerveModulePosition containing this module's state.
231     */
232    public SwerveModulePosition getPosition(boolean refresh) {
233        if (refresh) {
234            /* Refresh all signals */
235            BaseStatusSignal.refreshAll(
236                m_drivePosition,
237                m_driveVelocity,
238                m_steerPosition,
239                m_steerVelocity
240            );
241        }
242
243        /* Now latency-compensate our signals */
244        double drive_rot = BaseStatusSignal.getLatencyCompensatedValueAsDouble(m_drivePosition, m_driveVelocity);
245        double angle_rot = BaseStatusSignal.getLatencyCompensatedValueAsDouble(m_steerPosition, m_steerVelocity);
246
247        /*
248         * Back out the drive rotations based on angle rotations due to coupling between
249         * azimuth and steer
250         */
251        drive_rot -= angle_rot * m_couplingRatioDriveRotorToCANcoder;
252
253        /* And push them into a SwerveModulePosition object to return */
254        m_internalState.distanceMeters = drive_rot / m_driveRotationsPerMeter;
255        /* Angle is already in terms of steer rotations */
256        m_internalState.angle = Rotation2d.fromRotations(angle_rot);
257
258        return m_internalState;
259    }
260
261    /**
262     * Applies the desired SwerveModuleState to this module.
263     *
264     * @param state            Speed and direction the module should target
265     * @param driveRequestType The {@link DriveRequestType} to apply
266     */
267    public void apply(SwerveModuleState state, DriveRequestType driveRequestType) {
268        apply(state, driveRequestType, SteerRequestType.MotionMagic);
269    }
270
271    /**
272     * Applies the desired SwerveModuleState to this module.
273     *
274     * @param state            Speed and direction the module should target
275     * @param driveRequestType The {@link DriveRequestType} to apply
276     * @param steerRequestType The {@link SteerRequestType} to apply; defaults to {@link SteerRequestType#MotionMagic}
277     */
278    public void apply(SwerveModuleState state, DriveRequestType driveRequestType, SteerRequestType steerRequestType) {
279        state.optimize(m_internalState.angle);
280        m_targetState = state;
281
282        double angleToSetDeg = state.angle.getRotations();
283        switch (steerRequestType) {
284            case MotionMagic:
285                switch (m_steerClosedLoopOutput) {
286                    case Voltage:
287                        m_steerMotor.setControl(m_angleVoltageSetter.withPosition(angleToSetDeg));
288                        break;
289
290                    case TorqueCurrentFOC:
291                        m_steerMotor.setControl(m_angleTorqueSetter.withPosition(angleToSetDeg));
292                        break;
293                }
294                break;
295
296            case MotionMagicExpo:
297                switch (m_steerClosedLoopOutput) {
298                    case Voltage:
299                        m_steerMotor.setControl(m_angleVoltageExpoSetter.withPosition(angleToSetDeg));
300                        break;
301
302                    case TorqueCurrentFOC:
303                        m_steerMotor.setControl(m_angleTorqueExpoSetter.withPosition(angleToSetDeg));
304                        break;
305                }
306                break;
307        }
308
309        double velocityToSet = state.speedMetersPerSecond * m_driveRotationsPerMeter;
310
311        /* From FRC 900's whitepaper, we add a cosine compensator to the applied drive velocity */
312        /* To reduce the "skew" that occurs when changing direction */
313        double steerMotorError = angleToSetDeg - m_steerPosition.getValueAsDouble();
314        /* If error is close to 0 rotations, we're already there, so apply full power */
315        /* If the error is close to 0.25 rotations, then we're 90 degrees, so movement doesn't help us at all */
316        double cosineScalar = Math.cos(Units.rotationsToRadians(steerMotorError));
317        /* Make sure we don't invert our drive, even though we shouldn't ever target over 90 degrees anyway */
318        if (cosineScalar < 0.0) {
319            cosineScalar = 0.0;
320        }
321        velocityToSet *= cosineScalar;
322
323        /* Back out the expected shimmy the drive motor will see */
324        /* Find the angular rate to determine what to back out */
325        double azimuthTurnRps = m_steerVelocity.getValueAsDouble();
326        /* Azimuth turn rate multiplied by coupling ratio provides back-out rps */
327        double driveRateBackOut = azimuthTurnRps * m_couplingRatioDriveRotorToCANcoder;
328        velocityToSet += driveRateBackOut;
329
330        switch (driveRequestType) {
331            case OpenLoopVoltage:
332                /* Open loop ignores the driveRotationsPerMeter since it only cares about the open loop at the mechanism */
333                /* But we do care about the backout due to coupling, so we keep it in */
334                velocityToSet /= m_driveRotationsPerMeter;
335                m_driveMotor.setControl(m_voltageOpenLoopSetter.withOutput(velocityToSet / m_speedAt12VoltsMps * 12.0));
336                break;
337
338            case Velocity:
339                switch (m_driveClosedLoopOutput) {
340                    case Voltage:
341                        m_driveMotor.setControl(m_velocityVoltageSetter.withVelocity(velocityToSet));
342                        break;
343
344                    case TorqueCurrentFOC:
345                        m_driveMotor.setControl(m_velocityTorqueSetter.withVelocity(velocityToSet));
346                        break;
347                }
348                break;
349        }
350    }
351
352    /**
353     * Controls this module to the specified steer target, and applies the specific drive request.
354     * <p>
355     * This is intended only to be used for characterization of the robot, do not use this for normal use.
356     *
357     * @param steerTarget The angle the wheels should face for characterization
358     * @param driveRequest The direct voltage to apply to the motor for use during characterization
359     */
360    public void applyCharacterization(Rotation2d steerTarget, VoltageOut driveRequest)
361    {
362        double angleToSetDeg = steerTarget.getRotations();
363        /* Use the configured closed loop output mode */
364        switch (m_steerClosedLoopOutput) {
365            case Voltage:
366                m_steerMotor.setControl(m_angleVoltageSetter.withPosition(angleToSetDeg));
367                break;
368
369            case TorqueCurrentFOC:
370                m_steerMotor.setControl(m_angleTorqueSetter.withPosition(angleToSetDeg));
371                break;
372        }
373
374        /* And apply the high-level drive request */
375        m_driveMotor.setControl(driveRequest);
376    }
377
378    /**
379     * Controls this module to the specified steer target, and applies the specific drive request.
380     * <p>
381     * This is intended only to be used for characterization of the robot, do not use this for normal use.
382     *
383     * @param steerTarget The angle the wheels should face for characterization
384     * @param driveRequest The direct Torque Current to apply to the motor for use during characterization
385     */
386    public void applyCharacterization(Rotation2d steerTarget, TorqueCurrentFOC driveRequest)
387    {
388        double angleToSetDeg = steerTarget.getRotations();
389        /* Use the configured closed loop output mode */
390        switch (m_steerClosedLoopOutput) {
391            case Voltage:
392                m_steerMotor.setControl(m_angleVoltageSetter.withPosition(angleToSetDeg));
393                break;
394
395            case TorqueCurrentFOC:
396                m_steerMotor.setControl(m_angleTorqueSetter.withPosition(angleToSetDeg));
397                break;
398        }
399
400        /* And apply the high-level drive request */
401        m_driveMotor.setControl(driveRequest);
402    }
403
404    /**
405     * Configures the neutral mode to use for the module's drive motor.
406     *
407     * @param neutralMode The drive motor neutral mode
408     * @return Status code response of the request
409     */
410    public StatusCode configNeutralMode(NeutralModeValue neutralMode) {
411        var configs = new MotorOutputConfigs();
412
413        /* First read the configs so they're up-to-date */
414        StatusCode status = m_driveMotor.getConfigurator().refresh(configs);
415        if (status.isOK()) {
416            /* Then set the neutral mode config to the appropriate value */
417            configs.NeutralMode = neutralMode;
418            status = m_driveMotor.getConfigurator().apply(configs);
419        }
420        if (!status.isOK()) {
421            System.out.println(
422                    "TalonFX ID " + m_driveMotor.getDeviceID() + " failed config neutral mode with error " + status.toString());
423        }
424        return status;
425    }
426
427    /**
428     * Gets the last cached swerve module position.
429     * This differs from {@link getPosition} in that it will not
430     * perform any latency compensation or refresh the signals.
431     *
432     * @return Last cached SwerveModulePosition
433     */
434    public SwerveModulePosition getCachedPosition() {
435        return m_internalState;
436    }
437
438    /**
439     * Get the current state of the module.
440     * <p>
441     * This is typically used for telemetry, as the SwerveModulePosition
442     * is used for odometry.
443     *
444     * @return Current state of the module
445     */
446    public SwerveModuleState getCurrentState() {
447        return new SwerveModuleState(m_driveVelocity.getValueAsDouble() / m_driveRotationsPerMeter, Rotation2d.fromRotations(m_steerPosition.getValueAsDouble()));
448    }
449
450    /**
451     * Get the target state of the module.
452     * <p>
453     * This is typically used for telemetry.
454     *
455     * @return Target state of the module
456     */
457    public SwerveModuleState getTargetState() {
458        return m_targetState;
459    }
460
461    /**
462     * Gets the position/velocity signals of the drive and steer
463     *
464     * @return Array of BaseStatusSignals for this module in the following order:
465     *         0 - Drive Position
466     *         1 - Drive Velocity
467     *         2 - Steer Position
468     *         3 - Steer Velocity
469     */
470    BaseStatusSignal[] getSignals() {
471        return m_signals;
472    }
473
474    /**
475     * Resets this module's drive motor position to 0 rotations.
476     */
477    public void resetPosition() {
478        /* Only touch drive pos, not steer */
479        m_driveMotor.setPosition(0);
480    }
481
482    /**
483     * Gets this module's Drive Motor TalonFX reference.
484     * <p>
485     * This should be used only to access signals and change configurations that the
486     * swerve drivetrain does not configure itself.
487     *
488     * @return This module's Drive Motor reference
489     */
490    public TalonFX getDriveMotor() {
491        return m_driveMotor;
492    }
493
494    /**
495     * Gets this module's Steer Motor TalonFX reference.
496     * <p>
497     * This should be used only to access signals and change configurations that the
498     * swerve drivetrain does not configure itself.
499     *
500     * @return This module's Steer Motor reference
501     */
502    public TalonFX getSteerMotor() {
503        return m_steerMotor;
504    }
505
506    /**
507     * Gets this module's CANcoder reference.
508     * <p>
509     * This should be used only to access signals and change configurations that the
510     * swerve drivetrain does not configure itself.
511     *
512     * @return This module's CANcoder reference
513     */
514    public CANcoder getCANcoder() {
515        return m_cancoder;
516    }
517}