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