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.configs.CANcoderConfiguration;
010import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs;
011import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
012import com.ctre.phoenix6.configs.FeedbackConfigs;
013import com.ctre.phoenix6.configs.MagnetSensorConfigs;
014import com.ctre.phoenix6.configs.MotionMagicConfigs;
015import com.ctre.phoenix6.configs.MotorOutputConfigs;
016import com.ctre.phoenix6.configs.Slot0Configs;
017import com.ctre.phoenix6.configs.TalonFXConfiguration;
018import com.ctre.phoenix6.configs.TorqueCurrentConfigs;
019import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.ClosedLoopOutputType;
020import com.ctre.phoenix6.signals.NeutralModeValue;
021
022/**
023 * All constants for a swerve module.
024 */
025public class SwerveModuleConstants {
026    /**
027     * Supported feedback sensors for the steer motors.
028     */
029    public enum SteerFeedbackType {
030        RemoteCANcoder,
031        /** Requires Pro */
032        FusedCANcoder,
033        /** Requires Pro */
034        SyncCANcoder,
035    }
036
037    /** CAN ID of the drive motor. */
038    public int DriveMotorId = 0;
039    /** CAN ID of the steer motor. */
040    public int SteerMotorId = 0;
041    /** CAN ID of the CANcoder used for azimuth. */
042    public int CANcoderId = 0;
043    /** Offset of the CANcoder in rotations. */
044    public double CANcoderOffset = 0;
045
046    /** Gear ratio between the drive motor and the wheel. */
047    public double DriveMotorGearRatio = 0;
048    /**
049     * Gear ratio between the steer motor and the CANcoder.
050     * For example, the SDS Mk4 has a steering ratio of 12.8.
051     */
052    public double SteerMotorGearRatio = 0;
053    /**
054     * Coupled gear ratio between the CANcoder and the drive motor.
055     * <p>
056     * For a typical swerve module, the azimuth turn motor also drives the wheel a nontrivial
057     * amount, which affects the accuracy of odometry and control. This ratio represents the
058     * number of rotations of the drive motor caused by a rotation of the azimuth.
059     */
060    public double CouplingGearRatio = 0;
061
062    /** Radius of the driving wheel in inches. */
063    public double WheelRadius = 0;
064    /**
065     * The location of this module's wheels relative to the physical center of the robot in meters
066     * along the X axis of the robot.
067     */
068    public double LocationX = 0;
069    /**
070     * The location of this module's wheels relative to the physical center of the robot in meters
071     * along the Y axis of the robot.
072     */
073    public double LocationY = 0;
074
075    /**
076     * The steer motor closed-loop gains.
077     * <p>
078     * The steer motor uses the control ouput type specified by
079     * {@link #SteerMotorClosedLoopOutput} and any {@link SwerveModule.SteerRequestType}.
080     */
081    public Slot0Configs SteerMotorGains = new Slot0Configs();
082    /**
083     * The drive motor closed-loop gains.
084     * <p>
085     * When using closed-loop control, the drive motor uses the control output
086     * type specified by {@link #DriveMotorClosedLoopOutput} and any closed-loop
087     * {@link SwerveModule.DriveRequestType}.
088     */
089    public Slot0Configs DriveMotorGains = new Slot0Configs();
090
091    /** The closed-loop output type to use for the steer motors. */
092    public ClosedLoopOutputType SteerMotorClosedLoopOutput = ClosedLoopOutputType.Voltage;
093    /** The closed-loop output type to use for the drive motors. */
094    public ClosedLoopOutputType DriveMotorClosedLoopOutput = ClosedLoopOutputType.Voltage;
095
096    /** The maximum amount of stator current the drive motors can apply without slippage. */
097    public double SlipCurrent = 400;
098
099    /** True if the steering motor is reversed from the CANcoder. */
100    public boolean SteerMotorInverted = false;
101    /** True if the driving motor is reversed. */
102    public boolean DriveMotorInverted = false;
103
104    /**
105     * When using open-loop drive control, this specifies the speed at which the robot travels
106     * when driven with 12 volts, in meters per second. This is used to approximate the output
107     * for a desired velocity. If using closed loop control, this value is ignored.
108     */
109    public double SpeedAt12VoltsMps = 0;
110
111
112    /** Sim-specific constants **/
113    /** Simulated azimuthal inertia in kilogram meters squared. */
114    public double SteerInertia = 0.00001;
115    /** Simulated drive inertia in kilogram meters squared. */
116    public double DriveInertia = 0.001;
117    /** Simulated steer voltage required to overcome friction. */
118    public double SteerFrictionVoltage = 0.25;
119    /** Simulated drive voltage required to overcome friction. */
120    public double DriveFrictionVoltage = 0.25;
121
122    /**
123     * Choose how the feedback sensors should be configured.
124     * <p>
125     * If the robot does not support Pro, then this should remain as RemoteCANcoder.
126     * Otherwise, users have the option to use either FusedCANcoder or SyncCANcoder depending
127     * on if there is a risk that the CANcoder can fail in a way to provide "good" data.
128     */
129    public SteerFeedbackType FeedbackSource = SteerFeedbackType.RemoteCANcoder;
130
131    /**
132     * The initial configs used to configure the drive motor of the swerve module.
133     * The default value is the factory-default.
134     * <p>
135     * Users may change the initial configuration as they need.
136     * Any config that's not referenced in the {@link SwerveModuleConstants}
137     * class is available to be changed.
138     * <p>
139     * The list of configs that will be overwritten is as follows:
140     * <ul>
141     *   <li> {@link MotorOutputConfigs#NeutralMode} (Brake mode, overwritten with {@link SwerveModule#configNeutralMode(NeutralModeValue)})
142     *   <li> {@link MotorOutputConfigs#Inverted} ({@link SwerveModuleConstants#DriveMotorInverted})
143     *   <li> {@link Slot0Configs} ({@link SwerveModuleConstants#DriveMotorGains})
144     *   <li> {@link CurrentLimitsConfigs#StatorCurrentLimit} / {@link TorqueCurrentConfigs#PeakForwardTorqueCurrent}
145     *        / {@link TorqueCurrentConfigs#PeakReverseTorqueCurrent} ({@link SwerveModuleConstants#SlipCurrent})
146     *   <li> {@link CurrentLimitsConfigs#StatorCurrentLimitEnable} (Enabled)
147     * </ul>
148     */
149    public TalonFXConfiguration DriveMotorInitialConfigs = new TalonFXConfiguration();
150    /**
151     * The initial configs used to configure the steer motor of the swerve module.
152     * The default value is the factory-default.
153     * <p>
154     * Users may change the initial configuration as they need.
155     * Any config that's not referenced in the {@link SwerveModuleConstants}
156     * class is available to be changed.
157     * <p>
158     * The list of configs that will be overwritten is as follows:
159     * <ul>
160     *   <li> {@link MotorOutputConfigs#NeutralMode} (Brake mode)
161     *   <li> {@link MotorOutputConfigs#Inverted} ({@link SwerveModuleConstants#SteerMotorInverted})
162     *   <li> {@link Slot0Configs} ({@link SwerveModuleConstants#SteerMotorGains})
163     *   <li> {@link FeedbackConfigs#FeedbackRemoteSensorID} ({@link SwerveModuleConstants#CANcoderId})
164     *   <li> {@link FeedbackConfigs#FeedbackSensorSource} ({@link SwerveModuleConstants#FeedbackSource})
165     *   <li> {@link FeedbackConfigs#RotorToSensorRatio} ({@link SwerveModuleConstants#SteerMotorGearRatio})
166     *   <li> {@link MotionMagicConfigs} (Calculated from gear ratios)
167     *   <li> {@link ClosedLoopGeneralConfigs#ContinuousWrap} (true)
168     * </ul>
169     */
170    public TalonFXConfiguration SteerMotorInitialConfigs = new TalonFXConfiguration();
171    /**
172     * The initial configs used to configure the CANcoder of the swerve module.
173     * The default value is the factory-default.
174     * <p>
175     * Users may change the initial configuration as they need.
176     * Any config that's not referenced in the {@link #SwerveModuleConstants}
177     * class is available to be changed.
178     * <p>
179     * The list of configs that will be overwritten is as follows:
180     * <ul>
181     *   <li> {@link MagnetSensorConfigs#MagnetOffset} ({@link SwerveModuleConstants#CANcoderOffset})
182     * </ul>
183     */
184    public CANcoderConfiguration CANcoderInitialConfigs = new CANcoderConfiguration();
185
186    /**
187     * Sets the CAN ID of the drive motor.
188     *
189     * @param id CAN ID of the drive motor
190     * @return this object
191     */
192    public SwerveModuleConstants withDriveMotorId(int id) {
193        this.DriveMotorId = id;
194        return this;
195    }
196
197    /**
198     * Sets the CAN ID of the steer motor.
199     *
200     * @param id CAN ID of the steer motor
201     * @return this object
202     */
203    public SwerveModuleConstants withSteerMotorId(int id) {
204        this.SteerMotorId = id;
205        return this;
206    }
207
208    /**
209     * Sets the CAN ID of the CANcoder used for azimuth.
210     *
211     * @param id CAN ID of the CANcoder used for azimuth
212     * @return this object
213     */
214    public SwerveModuleConstants withCANcoderId(int id) {
215        this.CANcoderId = id;
216        return this;
217    }
218
219    /**
220     * Sets the offset of the CANcoder in rotations.
221     *
222     * @param offset Offset of the CANcoder in rotations
223     * @return this object
224     */
225    public SwerveModuleConstants withCANcoderOffset(double offset) {
226        this.CANcoderOffset = offset;
227        return this;
228    }
229
230    /**
231     * Sets the gear ratio between the drive motor and the wheel.
232     *
233     * @param ratio Gear ratio between the drive motor and the wheel
234     * @return this object
235     */
236    public SwerveModuleConstants withDriveMotorGearRatio(double ratio) {
237        this.DriveMotorGearRatio = ratio;
238        return this;
239    }
240
241    /**
242     * Sets the gear ratio between the steer motor and the CANcoder.
243     * For example, the SDS Mk4 has a steering ratio of 12.8.
244     *
245     * @param ratio Gear ratio between the steer motor and the CANcoder
246     * @return this object
247     */
248    public SwerveModuleConstants withSteerMotorGearRatio(double ratio) {
249        this.SteerMotorGearRatio = ratio;
250        return this;
251    }
252
253    /**
254     * Sets the coupled gear ratio between the CANcoder and the drive motor.
255     * <p>
256     * For a typical swerve module, the azimuth turn motor also drives the wheel a nontrivial
257     * amount, which affects the accuracy of odometry and control. This ratio represents the
258     * number of rotations of the drive motor caused by a rotation of the azimuth.
259     *
260     * @param ratio Coupled gear ratio between the CANcoder and the drive motor
261     * @return this object
262     */
263    public SwerveModuleConstants withCouplingGearRatio(double ratio) {
264        this.CouplingGearRatio = ratio;
265        return this;
266    }
267
268    /**
269     * Sets the radius of the driving wheel in inches.
270     *
271     * @param radius Radius of the driving wheel in inches
272     * @return this object
273     */
274    public SwerveModuleConstants withWheelRadius(double radius) {
275        this.WheelRadius = radius;
276        return this;
277    }
278
279    /**
280     * Sets the location of this module's wheels relative to the physical center of the robot in
281     * meters along the X axis of the robot.
282     *
283     * @param locationXMeters Location of this module's wheels
284     * @return this object
285     */
286    public SwerveModuleConstants withLocationX(double locationXMeters) {
287        this.LocationX = locationXMeters;
288        return this;
289    }
290
291    /**
292     * Sets the location of this module's wheels relative to the physical center of the robot in
293     * meters along the Y axis of the robot.
294     *
295     * @param locationYMeters Location of this module's wheels
296     * @return this object
297     */
298    public SwerveModuleConstants withLocationY(double locationYMeters) {
299        this.LocationY = locationYMeters;
300        return this;
301    }
302
303    /**
304     * Sets the steer motor closed-loop gains.
305     * <p>
306     * The steer motor uses the control ouput type specified by
307     * {@link #SteerMotorClosedLoopOutput} and any {@link SwerveModule.SteerRequestType}.
308     *
309     * @param gains Steer motor closed-loop gains
310     * @return this object
311     */
312    public SwerveModuleConstants withSteerMotorGains(Slot0Configs gains) {
313        this.SteerMotorGains = gains;
314        return this;
315    }
316
317    /**
318     * Sets the drive motor closed-loop gains.
319     * <p>
320     * When using closed-loop control, the drive motor uses the control output
321     * type specified by {@link #DriveMotorClosedLoopOutput} and any closed-loop
322     * {@link SwerveModule.DriveRequestType}.
323     *
324     * @param gains Drive motor closed-loop gains
325     * @return this object
326     */
327    public SwerveModuleConstants withDriveMotorGains(Slot0Configs gains) {
328        this.DriveMotorGains = gains;
329        return this;
330    }
331
332    /**
333     * Sets closed-loop output type to use for the steer motors.
334     *
335     * @param outputType Closed-loop output type to use for the steer motors
336     * @return this object
337     */
338    public SwerveModuleConstants withSteerMotorClosedLoopOutput(ClosedLoopOutputType outputType) {
339        this.SteerMotorClosedLoopOutput = outputType;
340        return this;
341    }
342
343    /**
344     * Sets closed-loop output type to use for the drive motors.
345     *
346     * @param outputType Closed-loop output type to use for the drive motors
347     * @return this object
348     */
349    public SwerveModuleConstants withDriveMotorClosedLoopOutput(ClosedLoopOutputType outputType) {
350        this.DriveMotorClosedLoopOutput = outputType;
351        return this;
352    }
353
354    /**
355     * Sets the maximum amount of stator current the drive motors can
356     * apply without slippage.
357     *
358     * @param slipCurrent Maximum amount of stator current
359     * @return this object
360     */
361    public SwerveModuleConstants withSlipCurrent(double slipCurrent) {
362        this.SlipCurrent = slipCurrent;
363        return this;
364    }
365
366    /**
367     * Sets whether the steering motor is reversed from the CANcoder.
368     *
369     * @param steerMotorInverted True if the steering motor is reversed from the CANcoder
370     * @return this object
371     */
372    public SwerveModuleConstants withSteerMotorInverted(boolean steerMotorInverted) {
373        this.SteerMotorInverted = steerMotorInverted;
374        return this;
375    }
376
377    /**
378     * Sets whether the driving motor is reversed.
379     *
380     * @param driveMotorInverted True if the driving motor is reversed
381     * @return this object
382     */
383    public SwerveModuleConstants withDriveMotorInverted(boolean driveMotorInverted) {
384        this.DriveMotorInverted = driveMotorInverted;
385        return this;
386    }
387
388    /**
389     * When using open-loop drive control, this specifies the speed at which the robot travels
390     * when driven with 12 volts, in meters per second. This is used to approximate the output
391     * for a desired velocity. If using closed loop control, this value is ignored.
392     *
393     * @param speedAt12VoltsMps Speed at which the robot travels when driven with
394     *                          12 volts, in meters per second
395     * @return this object
396     */
397    public SwerveModuleConstants withSpeedAt12VoltsMps(double speedAt12VoltsMps) {
398        this.SpeedAt12VoltsMps = speedAt12VoltsMps;
399        return this;
400    }
401
402    /**
403     * Sets the simulated azimuthal inertia in kilogram meters squared.
404     *
405     * @param steerInertia Azimuthal inertia in kilogram meters squared
406     * @return this object
407     */
408    public SwerveModuleConstants withSteerInertia(double steerInertia) {
409        this.SteerInertia = steerInertia;
410        return this;
411    }
412
413    /**
414     * Sets the simulated drive inertia in kilogram meters squared.
415     *
416     * @param driveInertia Drive inertia in kilogram meters squared
417     * @return this object
418     */
419    public SwerveModuleConstants withDriveInertia(double driveInertia) {
420        this.DriveInertia = driveInertia;
421        return this;
422    }
423
424    /**
425     * Sets the simulated steer voltage required to overcome friction.
426     *
427     * @param voltage Steer voltage required to overcome friction
428     * @return this object
429     */
430    public SwerveModuleConstants withSteerFrictionVoltage(double voltage) {
431        this.SteerFrictionVoltage = voltage;
432        return this;
433    }
434
435    /**
436     * Sets the simulated drive voltage required to overcome friction.
437     *
438     * @param voltage Drive voltage required to overcome friction
439     * @return this object
440     */
441    public SwerveModuleConstants withDriveFrictionVoltage(double voltage) {
442        this.DriveFrictionVoltage = voltage;
443        return this;
444    }
445
446    /**
447     * Chooses how the feedback sensors should be configured.
448     * <p>
449     * If the robot does not support Pro, then this should remain as RemoteCANcoder.
450     * Otherwise, users have the option to use either FusedCANcoder or SyncCANcoder depending
451     * on if there is a risk that the CANcoder can fail in a way to provide "good" data.
452     *
453     * @param source The feedback sensor source
454     * @return this object
455     */
456    public SwerveModuleConstants withFeedbackSource(SteerFeedbackType source) {
457        this.FeedbackSource = source;
458        return this;
459    }
460    /**
461     * The initial configs used to configure the drive motor of the swerve module.
462     * The default value is the factory-default.
463     * <p>
464     * Users may change the initial configuration as they need.
465     * Any config that's not referenced in the {@link SwerveModuleConstants}
466     * class is available to be changed.
467     * <p>
468     * The list of configs that will be overwritten is as follows:
469     * <ul>
470     *   <li> {@link MotorOutputConfigs#NeutralMode} (Brake mode, overwritten with {@link SwerveModule#configNeutralMode(NeutralModeValue)})
471     *   <li> {@link MotorOutputConfigs#Inverted} ({@link SwerveModuleConstants#DriveMotorInverted})
472     *   <li> {@link Slot0Configs} ({@link SwerveModuleConstants#DriveMotorGains})
473     *   <li> {@link CurrentLimitsConfigs#StatorCurrentLimit} / {@link TorqueCurrentConfigs#PeakForwardTorqueCurrent}
474     *        / {@link TorqueCurrentConfigs#PeakReverseTorqueCurrent} ({@link SwerveModuleConstants#SlipCurrent})
475     *   <li> {@link CurrentLimitsConfigs#StatorCurrentLimitEnable} (Enabled)
476     * </ul>
477     *
478     * @param configs Configs to set as an initial set of configs
479     * @return this object
480     */
481    public SwerveModuleConstants withDriveMotorInitialConfigs(TalonFXConfiguration configs) {
482        this.DriveMotorInitialConfigs = configs;
483        return this;
484    }
485    /**
486     * The initial configs used to configure the steer motor of the swerve module.
487     * The default value is the factory-default.
488     * <p>
489     * Users may change the initial configuration as they need.
490     * Any config that's not referenced in the {@link SwerveModuleConstants}
491     * class is available to be changed.
492     * <p>
493     * The list of configs that will be overwritten is as follows:
494     * <ul>
495     *   <li> {@link MotorOutputConfigs#NeutralMode} (Brake mode)
496     *   <li> {@link MotorOutputConfigs#Inverted} ({@link SwerveModuleConstants#SteerMotorInverted})
497     *   <li> {@link Slot0Configs} ({@link SwerveModuleConstants#SteerMotorGains})
498     *   <li> {@link FeedbackConfigs#FeedbackRemoteSensorID} ({@link SwerveModuleConstants#CANcoderId})
499     *   <li> {@link FeedbackConfigs#FeedbackSensorSource} ({@link SwerveModuleConstants#FeedbackSource})
500     *   <li> {@link FeedbackConfigs#RotorToSensorRatio} ({@link SwerveModuleConstants#SteerMotorGearRatio})
501     *   <li> {@link MotionMagicConfigs} (Calculated from gear ratios)
502     *   <li> {@link ClosedLoopGeneralConfigs#ContinuousWrap} (true)
503     * </ul>
504     *
505     * @param configs Configs to set as an initial set of configs
506     * @return this object
507     */
508    public SwerveModuleConstants withSteerMotorInitialConfigs(TalonFXConfiguration configs) {
509        this.SteerMotorInitialConfigs = configs;
510        return this;
511    }
512    /**
513     * The initial configs used to configure the CANcoder of the swerve module.
514     * The default value is the factory-default.
515     * <p>
516     * Users may change the initial configuration as they need.
517     * Any config that's not referenced in the {@link #SwerveModuleConstants}
518     * class is available to be changed.
519     * <p>
520     * The list of configs that will be overwritten is as follows:
521     * <ul>
522     *   <li> {@link MagnetSensorConfigs#MagnetOffset} ({@link SwerveModuleConstants#CANcoderOffset})
523     * </ul>
524     *
525     * @param configs Configs to set as an initial set of configs
526     * @return this object
527     */
528    public SwerveModuleConstants withCANcoderInitialConfigs(CANcoderConfiguration configs) {
529        this.CANcoderInitialConfigs = configs;
530        return this;
531    }
532}