001/************************ PROJECT ZIXI ************************/
002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved.*/
003/* This work is licensed under the terms of the MIT license.  */
004/**************************************************************/
005package com.stuypulse.robot.subsystems.swerve;
006
007import static org.wpilib.units.Units.*;
008
009import org.wpilib.hardware.bus.CANPort;
010import org.wpilib.math.linalg.Matrix;
011import org.wpilib.math.numbers.N1;
012import org.wpilib.math.numbers.N3;
013import org.wpilib.units.measure.*;
014
015import com.ctre.phoenix6.CANBus;
016import com.ctre.phoenix6.configs.*;
017import com.ctre.phoenix6.hardware.*;
018import com.ctre.phoenix6.signals.*;
019import com.ctre.phoenix6.swerve.*;
020import com.ctre.phoenix6.swerve.SwerveModuleConstants.*;
021
022// Generated by the Tuner X Swerve Project Generator
023// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
024public class TunerConstants {
025    // Both sets of gains need to be tuned to your individual robot.
026
027    // The steer motor uses any SwerveModule.SteerRequestType control request with
028    // the
029    // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
030    private static final Slot0Configs steerGains =
031            new Slot0Configs()
032                    .withKP(100)
033                    .withKI(0)
034                    .withKD(0.5)
035                    .withKS(0.1)
036                    .withKV(1.91)
037                    .withKA(0)
038                    .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
039    // When using closed-loop control, the drive motor uses the control
040    // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
041    private static final Slot0Configs driveGains =
042            new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124);
043
044    // The closed-loop output type to use for the steer motors;
045    // This affects the PID/FF gains for the steer motors
046    private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage;
047    // The closed-loop output type to use for the drive motors;
048    // This affects the PID/FF gains for the drive motors
049    private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage;
050
051    // The type of motor used for the drive motor
052    private static final DriveMotorArrangement kDriveMotorType =
053            DriveMotorArrangement.TalonFX_Integrated;
054    // The type of motor used for the drive motor
055    private static final SteerMotorArrangement kSteerMotorType =
056            SteerMotorArrangement.TalonFX_Integrated;
057
058    // The remote sensor feedback type to use for the steer motors;
059    // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to
060    // RemoteCANcoder
061    private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
062
063    // The stator current at which the wheels start to slip;
064    // This needs to be tuned to your individual robot
065    private static final Current kSlipCurrent = Amps.of(120.0);
066
067    // Initial configs for the drive and steer motors and the azimuth encoder; these
068    // cannot be null.
069    // Some configs will be overwritten; check the `with*InitialConfigs()` API
070    // documentation.
071    private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
072    private static final TalonFXConfiguration steerInitialConfigs =
073            new TalonFXConfiguration()
074                    .withCurrentLimits(
075                            new CurrentLimitsConfigs()
076                                    // Swerve azimuth does not require much torque output, so we can
077                                    // set a
078                                    // relatively
079                                    // low
080                                    // stator current limit to help avoid brownouts without
081                                    // impacting performance.
082                                    .withStatorCurrentLimit(Amps.of(60))
083                                    .withStatorCurrentLimitEnable(true));
084    private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
085    // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
086    private static final Pigeon2Configuration pigeonConfigs = null;
087
088    // CAN bus that the devices are located on;
089    // All swerve devices must share the same CAN bus
090    public static final CANBus kCANBus = new CANBus(CANPort.CAN_S0);
091
092    // Theoretical free speed (m/s) at 12 V applied output;
093    // This needs to be tuned to your individual robot
094    public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69);
095
096    // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
097    // This may need to be tuned to your individual robot
098    private static final double kCoupleRatio = 3.8181818181818183;
099
100    private static final double kDriveGearRatio = 7.363636363636365;
101    private static final double kSteerGearRatio = 15.42857142857143;
102    private static final Distance kWheelRadius = Inches.of(2.167);
103
104    private static final boolean kInvertLeftSide = false;
105    private static final boolean kInvertRightSide = true;
106
107    private static final int kPigeonId = 1;
108
109    // These are only used for simulation
110    private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004);
111    private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025);
112    // Simulated voltage necessary to overcome friction
113    private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
114    private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
115
116    public static final SwerveDrivetrainConstants DrivetrainConstants =
117            new SwerveDrivetrainConstants()
118                    .withNetwork(kCANBus)
119                    .withPigeon2Id(kPigeonId)
120                    .withPigeon2Configs(pigeonConfigs);
121
122    private static final SwerveModuleConstantsFactory<
123                    TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
124            ConstantCreator =
125                    new SwerveModuleConstantsFactory<
126                                    TalonFXConfiguration,
127                                    TalonFXConfiguration,
128                                    CANcoderConfiguration>()
129                            .withDriveMotorGearRatio(kDriveGearRatio)
130                            .withSteerMotorGearRatio(kSteerGearRatio)
131                            .withCouplingGearRatio(kCoupleRatio)
132                            .withWheelRadius(kWheelRadius)
133                            .withSteerMotorGains(steerGains)
134                            .withDriveMotorGains(driveGains)
135                            .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput)
136                            .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput)
137                            .withSlipCurrent(kSlipCurrent)
138                            .withSpeedAt12Volts(kSpeedAt12Volts)
139                            .withDriveMotorType(kDriveMotorType)
140                            .withSteerMotorType(kSteerMotorType)
141                            .withFeedbackSource(kSteerFeedbackType)
142                            .withDriveMotorInitialConfigs(driveInitialConfigs)
143                            .withSteerMotorInitialConfigs(steerInitialConfigs)
144                            .withEncoderInitialConfigs(encoderInitialConfigs)
145                            .withSteerInertia(kSteerInertia)
146                            .withDriveInertia(kDriveInertia)
147                            .withSteerFrictionVoltage(kSteerFrictionVoltage)
148                            .withDriveFrictionVoltage(kDriveFrictionVoltage);
149
150    // Front Left
151    private static final int kFrontLeftDriveMotorId = 3;
152    private static final int kFrontLeftSteerMotorId = 2;
153    private static final int kFrontLeftEncoderId = 1;
154    private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375);
155    private static final boolean kFrontLeftSteerMotorInverted = true;
156    private static final boolean kFrontLeftEncoderInverted = false;
157
158    private static final Distance kFrontLeftXPos = Inches.of(10);
159    private static final Distance kFrontLeftYPos = Inches.of(10);
160
161    // Front Right
162    private static final int kFrontRightDriveMotorId = 1;
163    private static final int kFrontRightSteerMotorId = 0;
164    private static final int kFrontRightEncoderId = 0;
165    private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875);
166    private static final boolean kFrontRightSteerMotorInverted = true;
167    private static final boolean kFrontRightEncoderInverted = false;
168
169    private static final Distance kFrontRightXPos = Inches.of(10);
170    private static final Distance kFrontRightYPos = Inches.of(-10);
171
172    // Back Left
173    private static final int kBackLeftDriveMotorId = 7;
174    private static final int kBackLeftSteerMotorId = 6;
175    private static final int kBackLeftEncoderId = 3;
176    private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875);
177    private static final boolean kBackLeftSteerMotorInverted = true;
178    private static final boolean kBackLeftEncoderInverted = false;
179
180    private static final Distance kBackLeftXPos = Inches.of(-10);
181    private static final Distance kBackLeftYPos = Inches.of(10);
182
183    // Back Right
184    private static final int kBackRightDriveMotorId = 5;
185    private static final int kBackRightSteerMotorId = 4;
186    private static final int kBackRightEncoderId = 2;
187    private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125);
188    private static final boolean kBackRightSteerMotorInverted = true;
189    private static final boolean kBackRightEncoderInverted = false;
190
191    private static final Distance kBackRightXPos = Inches.of(-10);
192    private static final Distance kBackRightYPos = Inches.of(-10);
193
194    public static final SwerveModuleConstants<
195                    TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
196            FrontLeft =
197                    ConstantCreator.createModuleConstants(
198                            kFrontLeftSteerMotorId,
199                            kFrontLeftDriveMotorId,
200                            kFrontLeftEncoderId,
201                            kFrontLeftEncoderOffset,
202                            kFrontLeftXPos,
203                            kFrontLeftYPos,
204                            kInvertLeftSide,
205                            kFrontLeftSteerMotorInverted,
206                            kFrontLeftEncoderInverted);
207    public static final SwerveModuleConstants<
208                    TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
209            FrontRight =
210                    ConstantCreator.createModuleConstants(
211                            kFrontRightSteerMotorId,
212                            kFrontRightDriveMotorId,
213                            kFrontRightEncoderId,
214                            kFrontRightEncoderOffset,
215                            kFrontRightXPos,
216                            kFrontRightYPos,
217                            kInvertRightSide,
218                            kFrontRightSteerMotorInverted,
219                            kFrontRightEncoderInverted);
220    public static final SwerveModuleConstants<
221                    TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
222            BackLeft =
223                    ConstantCreator.createModuleConstants(
224                            kBackLeftSteerMotorId,
225                            kBackLeftDriveMotorId,
226                            kBackLeftEncoderId,
227                            kBackLeftEncoderOffset,
228                            kBackLeftXPos,
229                            kBackLeftYPos,
230                            kInvertLeftSide,
231                            kBackLeftSteerMotorInverted,
232                            kBackLeftEncoderInverted);
233    public static final SwerveModuleConstants<
234                    TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
235            BackRight =
236                    ConstantCreator.createModuleConstants(
237                            kBackRightSteerMotorId,
238                            kBackRightDriveMotorId,
239                            kBackRightEncoderId,
240                            kBackRightEncoderOffset,
241                            kBackRightXPos,
242                            kBackRightYPos,
243                            kInvertRightSide,
244                            kBackRightSteerMotorInverted,
245                            kBackRightEncoderInverted);
246
247    /**
248     * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot
249     * program,.
250     */
251    // public static CommandSwerveDrivetrain createDrivetrain() {
252    // return new CommandSwerveDrivetrain(
253    // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight);
254    // }
255
256    /**
257     * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types.
258     */
259    public static class TunerSwerveDrivetrain extends SwerveDrivetrain<TalonFX, TalonFX, CANcoder> {
260        /**
261         * Constructs a CTRE SwerveDrivetrain using the specified constants.
262         *
263         * <p>This constructs the underlying hardware devices, so users should not construct the
264         * devices themselves. If they need the devices, they can access them through getters in the
265         * classes.
266         *
267         * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
268         * @param modules Constants for each specific module
269         */
270        public TunerSwerveDrivetrain(
271                SwerveDrivetrainConstants drivetrainConstants,
272                SwerveModuleConstants<?, ?, ?>... modules) {
273            super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules);
274        }
275
276        /**
277         * Constructs a CTRE SwerveDrivetrain using the specified constants.
278         *
279         * <p>This constructs the underlying hardware devices, so users should not construct the
280         * devices themselves. If they need the devices, they can access them through getters in the
281         * classes.
282         *
283         * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
284         * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or
285         *     set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0.
286         * @param modules Constants for each specific module
287         */
288        public TunerSwerveDrivetrain(
289                SwerveDrivetrainConstants drivetrainConstants,
290                double odometryUpdateFrequency,
291                SwerveModuleConstants<?, ?, ?>... modules) {
292            super(
293                    TalonFX::new,
294                    TalonFX::new,
295                    CANcoder::new,
296                    drivetrainConstants,
297                    odometryUpdateFrequency,
298                    modules);
299        }
300
301        /**
302         * Constructs a CTRE SwerveDrivetrain using the specified constants.
303         *
304         * <p>This constructs the underlying hardware devices, so users should not construct the
305         * devices themselves. If they need the devices, they can access them through getters in the
306         * classes.
307         *
308         * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
309         * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or
310         *     set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0.
311         * @param odometryStandardDeviation The standard deviation for odometry calculation in the
312         *     form [x, y, theta]ᵀ, with units in meters and radians
313         * @param visionStandardDeviation The standard deviation for vision calculation in the form
314         *     [x, y, theta]ᵀ, with units in meters and radians
315         * @param modules Constants for each specific module
316         */
317        public TunerSwerveDrivetrain(
318                SwerveDrivetrainConstants drivetrainConstants,
319                double odometryUpdateFrequency,
320                Matrix<N3, N1> odometryStandardDeviation,
321                Matrix<N3, N1> visionStandardDeviation,
322                SwerveModuleConstants<?, ?, ?>... modules) {
323            super(
324                    TalonFX::new,
325                    TalonFX::new,
326                    CANcoder::new,
327                    drivetrainConstants,
328                    odometryUpdateFrequency,
329                    odometryStandardDeviation,
330                    visionStandardDeviation,
331                    modules);
332        }
333    }
334}