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