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.CurrentLimitsConfigs;
014import com.ctre.phoenix6.configs.MotorOutputConfigs;
015import com.ctre.phoenix6.configs.TalonFXConfiguration;
016import com.ctre.phoenix6.configs.TorqueCurrentConfigs;
017import com.ctre.phoenix6.controls.MotionMagicExpoTorqueCurrentFOC;
018import com.ctre.phoenix6.controls.MotionMagicExpoVoltage;
019import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC;
020import com.ctre.phoenix6.controls.MotionMagicVoltage;
021import com.ctre.phoenix6.controls.TorqueCurrentFOC;
022import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC;
023import com.ctre.phoenix6.controls.VelocityVoltage;
024import com.ctre.phoenix6.controls.VoltageOut;
025import com.ctre.phoenix6.hardware.CANcoder;
026import com.ctre.phoenix6.hardware.TalonFX;
027import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
028import com.ctre.phoenix6.signals.InvertedValue;
029import com.ctre.phoenix6.signals.NeutralModeValue;
030
031import edu.wpi.first.math.geometry.Rotation2d;
032import edu.wpi.first.math.kinematics.SwerveModulePosition;
033import edu.wpi.first.math.kinematics.SwerveModuleState;
034import edu.wpi.first.math.util.Units;
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 SwerveModule {
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 SwerveModuleConstants#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 SwerveModuleConstants#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 SwerveModuleConstants#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<Double> m_drivePosition;
093    private final StatusSignal<Double> m_driveVelocity;
094    private final StatusSignal<Double> m_steerPosition;
095    private final StatusSignal<Double> 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 SwerveModule 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 SwerveModule(SwerveModuleConstants 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 = 0.1;
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            m_drivePosition.refresh();
236            m_driveVelocity.refresh();
237            m_steerPosition.refresh();
238            m_steerVelocity.refresh();
239        }
240
241        /* Now latency-compensate our signals */
242        double drive_rot = BaseStatusSignal.getLatencyCompensatedValue(m_drivePosition, m_driveVelocity);
243        double angle_rot = BaseStatusSignal.getLatencyCompensatedValue(m_steerPosition, m_steerVelocity);
244
245        /*
246         * Back out the drive rotations based on angle rotations due to coupling between
247         * azimuth and steer
248         */
249        drive_rot -= angle_rot * m_couplingRatioDriveRotorToCANcoder;
250
251        /* And push them into a SwerveModulePosition object to return */
252        m_internalState.distanceMeters = drive_rot / m_driveRotationsPerMeter;
253        /* Angle is already in terms of steer rotations */
254        m_internalState.angle = Rotation2d.fromRotations(angle_rot);
255
256        return m_internalState;
257    }
258
259    /**
260     * Applies the desired SwerveModuleState to this module.
261     *
262     * @param state            Speed and direction the module should target
263     * @param driveRequestType The {@link DriveRequestType} to apply
264     */
265    public void apply(SwerveModuleState state, DriveRequestType driveRequestType) {
266        apply(state, driveRequestType, SteerRequestType.MotionMagic);
267    }
268
269    /**
270     * Applies the desired SwerveModuleState to this module.
271     *
272     * @param state            Speed and direction the module should target
273     * @param driveRequestType The {@link DriveRequestType} to apply
274     * @param steerRequestType The {@link SteerRequestType} to apply; defaults to {@link SteerRequestType#MotionMagic}
275     */
276    public void apply(SwerveModuleState state, DriveRequestType driveRequestType, SteerRequestType steerRequestType) {
277        var optimized = SwerveModuleState.optimize(state, m_internalState.angle);
278        m_targetState = optimized;
279
280        double angleToSetDeg = optimized.angle.getRotations();
281        switch (steerRequestType) {
282            case MotionMagic:
283                switch (m_steerClosedLoopOutput) {
284                    case Voltage:
285                        m_steerMotor.setControl(m_angleVoltageSetter.withPosition(angleToSetDeg));
286                        break;
287
288                    case TorqueCurrentFOC:
289                        m_steerMotor.setControl(m_angleTorqueSetter.withPosition(angleToSetDeg));
290                        break;
291                }
292                break;
293
294            case MotionMagicExpo:
295                switch (m_steerClosedLoopOutput) {
296                    case Voltage:
297                        m_steerMotor.setControl(m_angleVoltageExpoSetter.withPosition(angleToSetDeg));
298                        break;
299
300                    case TorqueCurrentFOC:
301                        m_steerMotor.setControl(m_angleTorqueExpoSetter.withPosition(angleToSetDeg));
302                        break;
303                }
304                break;
305        }
306
307        double velocityToSet = optimized.speedMetersPerSecond * m_driveRotationsPerMeter;
308
309        /* From FRC 900's whitepaper, we add a cosine compensator to the applied drive velocity */
310        /* To reduce the "skew" that occurs when changing direction */
311        double steerMotorError = angleToSetDeg - m_steerPosition.getValue();
312        /* If error is close to 0 rotations, we're already there, so apply full power */
313        /* If the error is close to 0.25 rotations, then we're 90 degrees, so movement doesn't help us at all */
314        double cosineScalar = Math.cos(Units.rotationsToRadians(steerMotorError));
315        /* Make sure we don't invert our drive, even though we shouldn't ever target over 90 degrees anyway */
316        if (cosineScalar < 0.0) {
317            cosineScalar = 0.0;
318        }
319        velocityToSet *= cosineScalar;
320
321        /* Back out the expected shimmy the drive motor will see */
322        /* Find the angular rate to determine what to back out */
323        double azimuthTurnRps = m_steerVelocity.getValue();
324        /* Azimuth turn rate multiplied by coupling ratio provides back-out rps */
325        double driveRateBackOut = azimuthTurnRps * m_couplingRatioDriveRotorToCANcoder;
326        velocityToSet += driveRateBackOut;
327
328        switch (driveRequestType) {
329            case OpenLoopVoltage:
330                /* Open loop ignores the driveRotationsPerMeter since it only cares about the open loop at the mechanism */
331                /* But we do care about the backout due to coupling, so we keep it in */
332                velocityToSet /= m_driveRotationsPerMeter;
333                m_driveMotor.setControl(m_voltageOpenLoopSetter.withOutput(velocityToSet / m_speedAt12VoltsMps * 12.0));
334                break;
335
336            case Velocity:
337                switch (m_driveClosedLoopOutput) {
338                    case Voltage:
339                        m_driveMotor.setControl(m_velocityVoltageSetter.withVelocity(velocityToSet));
340                        break;
341
342                    case TorqueCurrentFOC:
343                        m_driveMotor.setControl(m_velocityTorqueSetter.withVelocity(velocityToSet));
344                        break;
345                }
346                break;
347        }
348    }
349
350    /**
351     * Controls this module to the specified steer target, and applies the specific drive request.
352     * <p>
353     * This is intended only to be used for characterization of the robot, do not use this for normal use.
354     *
355     * @param steerTarget The angle the wheels should face for characterization
356     * @param driveRequest The direct voltage to apply to the motor for use during characterization
357     */
358    public void applyCharacterization(Rotation2d steerTarget, VoltageOut driveRequest)
359    {
360        double angleToSetDeg = steerTarget.getRotations();
361        /* Use the configured closed loop output mode */
362        switch (m_steerClosedLoopOutput) {
363            case Voltage:
364                m_steerMotor.setControl(m_angleVoltageSetter.withPosition(angleToSetDeg));
365                break;
366
367            case TorqueCurrentFOC:
368                m_steerMotor.setControl(m_angleTorqueSetter.withPosition(angleToSetDeg));
369                break;
370        }
371
372        /* And apply the high-level drive request */
373        m_driveMotor.setControl(driveRequest);
374    }
375
376    /**
377     * Controls this module to the specified steer target, and applies the specific drive request.
378     * <p>
379     * This is intended only to be used for characterization of the robot, do not use this for normal use.
380     *
381     * @param steerTarget The angle the wheels should face for characterization
382     * @param driveRequest The direct Torque Current to apply to the motor for use during characterization
383     */
384    public void applyCharacterization(Rotation2d steerTarget, TorqueCurrentFOC driveRequest)
385    {
386        double angleToSetDeg = steerTarget.getRotations();
387        /* Use the configured closed loop output mode */
388        switch (m_steerClosedLoopOutput) {
389            case Voltage:
390                m_steerMotor.setControl(m_angleVoltageSetter.withPosition(angleToSetDeg));
391                break;
392
393            case TorqueCurrentFOC:
394                m_steerMotor.setControl(m_angleTorqueSetter.withPosition(angleToSetDeg));
395                break;
396        }
397
398        /* And apply the high-level drive request */
399        m_driveMotor.setControl(driveRequest);
400    }
401
402    /**
403     * Configures the neutral mode to use for the module's drive motor.
404     *
405     * @param neutralMode The drive motor neutral mode
406     * @return Status code response of the request
407     */
408    public StatusCode configNeutralMode(NeutralModeValue neutralMode) {
409        var configs = new MotorOutputConfigs();
410
411        /* First read the configs so they're up-to-date */
412        StatusCode status = m_driveMotor.getConfigurator().refresh(configs);
413        if (status.isOK()) {
414            /* Then set the neutral mode config to the appropriate value */
415            configs.NeutralMode = neutralMode;
416            status = m_driveMotor.getConfigurator().apply(configs);
417        }
418        if (!status.isOK()) {
419            System.out.println(
420                    "TalonFX ID " + m_driveMotor.getDeviceID() + " failed config neutral mode with error " + status.toString());
421        }
422        return status;
423    }
424
425    /**
426     * Gets the last cached swerve module position.
427     * This differs from {@link getPosition} in that it will not
428     * perform any latency compensation or refresh the signals.
429     *
430     * @return Last cached SwerveModulePosition
431     */
432    public SwerveModulePosition getCachedPosition() {
433        return m_internalState;
434    }
435
436    /**
437     * Get the current state of the module.
438     * <p>
439     * This is typically used for telemetry, as the SwerveModulePosition
440     * is used for odometry.
441     *
442     * @return Current state of the module
443     */
444    public SwerveModuleState getCurrentState() {
445        return new SwerveModuleState(m_driveVelocity.getValue() / m_driveRotationsPerMeter, Rotation2d.fromRotations(m_steerPosition.getValue()));
446    }
447
448    /**
449     * Get the target state of the module.
450     * <p>
451     * This is typically used for telemetry.
452     *
453     * @return Target state of the module
454     */
455    public SwerveModuleState getTargetState() {
456        return m_targetState;
457    }
458
459    /**
460     * Gets the position/velocity signals of the drive and steer
461     *
462     * @return Array of BaseStatusSignals for this module in the following order:
463     *         0 - Drive Position
464     *         1 - Drive Velocity
465     *         2 - Steer Position
466     *         3 - Steer Velocity
467     */
468    BaseStatusSignal[] getSignals() {
469        return m_signals;
470    }
471
472    /**
473     * Resets this module's drive motor position to 0 rotations.
474     */
475    public void resetPosition() {
476        /* Only touch drive pos, not steer */
477        m_driveMotor.setPosition(0);
478    }
479
480    /**
481     * Gets this module's Drive Motor TalonFX reference.
482     * <p>
483     * This should be used only to access signals and change configurations that the
484     * swerve drivetrain does not configure itself.
485     *
486     * @return This module's Drive Motor reference
487     */
488    public TalonFX getDriveMotor() {
489        return m_driveMotor;
490    }
491
492    /**
493     * Gets this module's Steer Motor TalonFX reference.
494     * <p>
495     * This should be used only to access signals and change configurations that the
496     * swerve drivetrain does not configure itself.
497     *
498     * @return This module's Steer Motor reference
499     */
500    public TalonFX getSteerMotor() {
501        return m_steerMotor;
502    }
503
504    /**
505     * Gets this module's CANcoder reference.
506     * <p>
507     * This should be used only to access signals and change configurations that the
508     * swerve drivetrain does not configure itself.
509     *
510     * @return This module's CANcoder reference
511     */
512    public CANcoder getCANcoder() {
513        return m_cancoder;
514    }
515}