From 5346c7cc219d41d3992bf88bd75d915a805e406b Mon Sep 17 00:00:00 2001 From: Nate Laverdure <41549690+nlaverdure@users.noreply.github.com> Date: Tue, 16 Jun 2026 20:15:37 -0400 Subject: [PATCH 01/10] Abstract drive gear ratio selection (#11) * added Ratio enum * formatting * add gear ratios * use varags, reduce field visibility * remove DRIVE_MOTOR_REDUCTION field * remove unnecessary vendordeps * refine DriveGearRatio couple ratio to support multiple chassis-frame stages Add chassisStages parameter to explicitly separate chassis-frame gear reduction stages (whose product is the couple ratio) from azimuth-frame stages. Add Javadoc to constructor, getDriveMotorReduction, and getCoupleRatio. Co-Authored-By: Claude Sonnet 4.6 --------- Co-authored-by: BenGamer3 Co-authored-by: Claude Sonnet 4.6 --- .../subsystems/drive/DriveConstants.java | 57 +++++++++++++++---- 1 file changed, 47 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 516fa7f..32c8b5e 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -65,10 +65,49 @@ public class DriveConstants { public static final Distance WHEEL_RADIUS = Inches.of(2); public static final double WHEEL_RADIUS_METERS = WHEEL_RADIUS.in(Meters); - // public static enum DRIVE_MOTOR_REDUCTIONS { SDS_MK5_R1, SDS_MK5_R2, SDS_MK5_R3 } + private static enum DriveGearRatio { + SDS_MK5i_R1(1, 54.0 / 12.0, 25.0 / 32.0, 30.0 / 15.0), + SDS_MK5i_R2(1, 54.0 / 14.0, 25.0 / 32.0, 30.0 / 15.0), + SDS_MK5i_R3(1, 54.0 / 16.0, 25.0 / 32.0, 30.0 / 15.0); - public static final double DRIVE_MOTOR_REDUCTION = - (54.0 / 14.0) * (25.0 / 32.0) * (30.0 / 15.0); // SDS MK5 R2 + private final int chassisStages; + private final double[] reductions; + + /** + * @param chassisStages the number of leading reduction stages fixed to the chassis reference + * frame; their product defines the couple ratio + * @param reductions all gear reduction stages in order, chassis-frame stages first followed by + * azimuth-frame stages + */ + private DriveGearRatio(int chassisStages, double... reductions) { + this.chassisStages = chassisStages; + this.reductions = reductions; + } + + /** + * Returns the total gear reduction from the drive motor to the wheel, combining all stages + * regardless of reference frame. Used to convert drive motor rotations to wheel rotations. + */ + public double getDriveMotorReduction() { + double product = 1.0; + for (double r : reductions) product *= r; + return product; + } + + /** + * Returns the number of drive motor rotations per one full rotation of the steering azimuth, + * due to mechanical coupling. This is the product of all gear reduction stages that are fixed + * to the chassis reference frame (i.e., before the azimuth pivot). The swerve odometry uses + * this to compensate for the apparent wheel displacement caused by azimuth rotation. + */ + public double getCoupleRatio() { + double product = 1.0; + for (int i = 0; i < chassisStages; i++) product *= reductions[i]; + return product; + } + } + + private static final DriveGearRatio SELECTED_RATIO = DriveGearRatio.SDS_MK5i_R2; public static final DCMotor DRIVE_GEARBOX = DCMotor.getKrakenX60Foc(1); public static final LinearVelocity DRIVETRAIN_SPEED_LIMIT = MetersPerSecond.of( @@ -76,7 +115,7 @@ public class DriveConstants { * (WHEEL_RADIUS_METERS * 2.0 * Math.PI) * DRIVE_GEARBOX.freeSpeedRadPerSec / (2.0 * Math.PI) - / DRIVE_MOTOR_REDUCTION); + / SELECTED_RATIO.getDriveMotorReduction()); // Chassis movement limits private static final LinearVelocity DRIVER_SPEED_LIMIT = MetersPerSecond.of(5); @@ -101,9 +140,7 @@ public class DriveConstants { // Turn motor configuration public static final boolean TURN_INVERTED = false; - public static final double TURN_MOTOR_REDUCTION = 26; // SDS MK5 R2 - // Every 1 rotation of the azimuth results in COUPLE_RATIO drive motor turns - private static final double COUPLE_RATIO = (54.0 / 14.0); // SDS MK4 L2 + public static final double TURN_MOTOR_REDUCTION = 26.0; // SDS MK5i public static final DCMotor TURN_GEARBOX = DCMotor.getKrakenX60Foc(1); // Absolute turn encoder configuration @@ -122,7 +159,7 @@ public class DriveConstants { WHEEL_RADIUS_METERS, DRIVETRAIN_SPEED_LIMIT.in(MetersPerSecond), WHEEL_COF, - DRIVE_GEARBOX.withReduction(DRIVE_MOTOR_REDUCTION), + DRIVE_GEARBOX.withReduction(SELECTED_RATIO.getDriveMotorReduction()), KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT, 1), MODULE_TRANSLATIONS); @@ -215,9 +252,9 @@ public class DriveConstants { CONSTANT_CREATOR = new SwerveModuleConstantsFactory< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() - .withDriveMotorGearRatio(DRIVE_MOTOR_REDUCTION) + .withDriveMotorGearRatio(SELECTED_RATIO.getDriveMotorReduction()) .withSteerMotorGearRatio(TURN_MOTOR_REDUCTION) - .withCouplingGearRatio(COUPLE_RATIO) + .withCouplingGearRatio(SELECTED_RATIO.getCoupleRatio()) .withWheelRadius(WHEEL_RADIUS) .withSteerMotorGains(STEER_GAINS) .withDriveMotorGains(DRIVE_GAINS) From a00b4cf5f93b27ddfe49a6ee7f173f4a8c856735 Mon Sep 17 00:00:00 2001 From: JShriller <98921217+Jatanimo@users.noreply.github.com> Date: Tue, 16 Jun 2026 20:26:32 -0400 Subject: [PATCH 02/10] Fixing inlining coders (#16) * Updating the trigger encoders for glass * Fixing the trigger encoders and made a build of the robot code --- src/main/java/frc/robot/Robot.java | 22 ++++++++++++++-------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e969e7e..8a946d6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -24,7 +24,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.CommandGenericHID; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -226,13 +225,20 @@ public Robot() { CommandScheduler.getInstance().onCommandFinish(cmd -> activeCommands.remove(cmd.getName())); CommandScheduler.getInstance().onCommandInterrupt(cmd -> activeCommands.remove(cmd.getName())); - new Trigger( - NetworkTableInstance.getDefault() - .getTable("Triggers") - .getBooleanTopic("Align Encoders") - .subscribe(false) - ::get) - .onTrue(new InstantCommand(drive::zeroAbsoluteEncoders).ignoringDisable(true)); + var alignEncodersEntry = + NetworkTableInstance.getDefault() + .getTable("Triggers") + .getBooleanTopic("Align Encoders") + .getEntry(false); + alignEncodersEntry.set(false); + new Trigger(alignEncodersEntry::get) + .onTrue( + Commands.runOnce( + () -> { + drive.zeroAbsoluteEncoders(); + alignEncodersEntry.set(false); + }) + .ignoringDisable(true)); Field.plotRegions(); } From 1706f1fb515bb080da3f3f6685c659717df64dac Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 26 Jun 2026 11:22:59 -0400 Subject: [PATCH 03/10] Add ModuleIOSimTalonFX and centralize sim constants in DriveConstants - Add ModuleIOSimTalonFX: Phoenix 6 hardware-in-the-loop sim using TalonFXSimState + CANcoderSimState driven by DCMotorSim physics - Expose DRIVE_INITIAL_CONFIGS and TURN_INITIAL_CONFIGS as public in DriveConstants so both TalonFX IO implementations share the same source - Add createDriveSim() / createTurnSim() factory methods to DriveConstants, eliminating duplicated DCMotorSim construction in ModuleIOSimWPI and ModuleIOSimTalonFX - Replace hardcoded 0.02s with Robot.defaultPeriodSecs in both sim classes Co-Authored-By: Claude Sonnet 4.6 --- .../subsystems/drive/DriveConstants.java | 20 +- .../subsystems/drive/ModuleIOSimTalonFX.java | 273 ++++++++++++++++++ .../subsystems/drive/ModuleIOSimWPI.java | 20 +- 3 files changed, 296 insertions(+), 17 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOSimTalonFX.java diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 26250ea..d7ffaf0 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -33,6 +33,7 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; @@ -41,6 +42,7 @@ import edu.wpi.first.units.measure.Mass; import edu.wpi.first.units.measure.MomentOfInertia; import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.Constants.CANBusPorts.SC1; import frc.robot.Constants.MotorConstants.KrakenX60Constants; @@ -173,7 +175,7 @@ public class DriveConstants { // since steering requires minimal torque compared to driving. static final int STEER_STATOR_CURRENT_LIMIT = 60; - private static final TalonFXConfiguration DRIVE_INITIAL_CONFIGS = + public static final TalonFXConfiguration DRIVE_INITIAL_CONFIGS = new TalonFXConfiguration() .withTorqueCurrent( new TorqueCurrentConfigs() @@ -188,7 +190,7 @@ public class DriveConstants { // Azimuth does not require much torque; keep stator limit low to reduce brownout risk // since steering requires minimal torque compared to driving. - private static final TalonFXConfiguration TURN_INITIAL_CONFIGS = + public static final TalonFXConfiguration TURN_INITIAL_CONFIGS = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() @@ -207,6 +209,20 @@ public class DriveConstants { private static final Voltage STEER_FRICTION_VOLTAGE = Volts.of(0.2); private static final Voltage DRIVE_FRICTION_VOLTAGE = Volts.of(0.2); + public static DCMotorSim createDriveSim() { + return new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DRIVE_GEARBOX, DRIVE_INERTIA.in(KilogramSquareMeters), DRIVE_MOTOR_REDUCTION), + DRIVE_GEARBOX); + } + + public static DCMotorSim createTurnSim() { + return new DCMotorSim( + LinearSystemId.createDCMotorSystem( + TURN_GEARBOX, STEER_INERTIA.in(KilogramSquareMeters), TURN_MOTOR_REDUCTION), + TURN_GEARBOX); + } + public static final SwerveDrivetrainConstants DRIVETRAIN_CONSTANTS = new SwerveDrivetrainConstants().withCANBusName(SC1.BUS.getName()); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimTalonFX.java new file mode 100644 index 0000000..93fe03a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimTalonFX.java @@ -0,0 +1,273 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.drive; + +import static frc.robot.util.PhoenixUtil.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.sim.CANcoderSimState; +import com.ctre.phoenix6.sim.TalonFXSimState; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import frc.robot.Constants.CANBusPorts.SC1; +import frc.robot.Robot; + +/** + * Module IO implementation using Phoenix 6 TalonFX sim layer for hardware-in-the-loop style + * simulation. Uses the same hardware configuration and control requests as ModuleIOTalonFX, with + * DCMotorSim physics driving the Phoenix sim state. + */ +public class ModuleIOSimTalonFX implements ModuleIO { + private final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + constants; + + // Hardware objects + private final TalonFX driveTalon; + private final TalonFX turnTalon; + private final CANcoder cancoder; + private final CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); + + // Sim states + private final TalonFXSimState driveSimState; + private final TalonFXSimState turnSimState; + private final CANcoderSimState cancoderSimState; + + // Physics simulation (same gearboxes as ModuleIOSimWPI) + private final DCMotorSim driveSim; + private final DCMotorSim turnSim; + + // Voltage control requests + private final VoltageOut voltageRequest = new VoltageOut(0); + private final PositionVoltage positionVoltageRequest = new PositionVoltage(0.0); + private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); + + // Torque-current control requests + private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0); + private final PositionTorqueCurrentFOC positionTorqueCurrentRequest = + new PositionTorqueCurrentFOC(0.0); + private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = + new VelocityTorqueCurrentFOC(0.0); + + // Status signals + private final StatusSignal drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + private final StatusSignal turnAbsolutePosition; + private final StatusSignal turnPosition; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + + public ModuleIOSimTalonFX( + SwerveModuleConstants + constants) { + this.constants = constants; + driveTalon = new TalonFX(constants.DriveMotorId, SC1.BUS); + turnTalon = new TalonFX(constants.SteerMotorId, SC1.BUS); + cancoder = new CANcoder(constants.EncoderId, SC1.BUS); + + // Configure drive motor (mirrors ModuleIOTalonFX; DRIVE_INITIAL_CONFIGS is the source) + var driveConfig = constants.DriveMotorInitialConfigs; + driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + driveConfig.Slot0 = constants.DriveMotorGains; + driveConfig.Feedback.SensorToMechanismRatio = constants.DriveMotorGearRatio; + driveConfig.MotorOutput.Inverted = + constants.DriveMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); + tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); + + // Configure turn motor (mirrors ModuleIOTalonFX; TURN_INITIAL_CONFIGS is the source) + var turnConfig = constants.SteerMotorInitialConfigs; + turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + turnConfig.Slot0 = constants.SteerMotorGains; + turnConfig.Feedback.FeedbackRemoteSensorID = constants.EncoderId; + turnConfig.Feedback.FeedbackSensorSource = + switch (constants.FeedbackSource) { + case RemoteCANcoder -> FeedbackSensorSourceValue.RemoteCANcoder; + case FusedCANcoder -> FeedbackSensorSourceValue.FusedCANcoder; + case SyncCANcoder -> FeedbackSensorSourceValue.SyncCANcoder; + default -> + throw new RuntimeException( + "You are using an unsupported swerve configuration, which this template does not support without manual customization. The 2025 release of Phoenix supports some swerve configurations which were not available during 2025 beta testing, preventing any development and support from the AdvantageKit developers."); + }; + turnConfig.Feedback.RotorToSensorRatio = constants.SteerMotorGearRatio; + turnConfig.MotionMagic.MotionMagicCruiseVelocity = 100.0 / constants.SteerMotorGearRatio; + turnConfig.MotionMagic.MotionMagicAcceleration = + turnConfig.MotionMagic.MotionMagicCruiseVelocity / 0.100; + turnConfig.MotionMagic.MotionMagicExpo_kV = 0.12 * constants.SteerMotorGearRatio; + turnConfig.MotionMagic.MotionMagicExpo_kA = 0.1; + turnConfig.ClosedLoopGeneral.ContinuousWrap = true; + turnConfig.MotorOutput.Inverted = + constants.SteerMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25)); + + // Configure CANcoder + cancoder.getConfigurator().refresh(cancoderConfig); + cancoderConfig.MagnetSensor.SensorDirection = + constants.EncoderInverted + ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive; + cancoder.getConfigurator().apply(cancoderConfig); + + // Get Phoenix sim states + driveSimState = driveTalon.getSimState(); + turnSimState = turnTalon.getSimState(); + cancoderSimState = cancoder.getSimState(); + + // Create physics models + driveSim = DriveConstants.createDriveSim(); + turnSim = DriveConstants.createTurnSim(); + + // Create status signals + drivePosition = driveTalon.getPosition(); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getStatorCurrent(); + turnAbsolutePosition = cancoder.getAbsolutePosition(); + turnPosition = turnTalon.getPosition(); + turnVelocity = turnTalon.getVelocity(); + turnAppliedVolts = turnTalon.getMotorVoltage(); + turnCurrent = turnTalon.getStatorCurrent(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Feed battery voltage into Phoenix sim devices + double busVoltage = RoboRioSim.getVInVoltage(); + driveSimState.setSupplyVoltage(busVoltage); + turnSimState.setSupplyVoltage(busVoltage); + cancoderSimState.setSupplyVoltage(busVoltage); + + // Drive physics: Phoenix output voltage → DCMotorSim → rotor state back to Phoenix + driveSim.setInputVoltage( + MathUtil.clamp(driveSimState.getMotorVoltage(), -busVoltage, busVoltage)); + turnSim.setInputVoltage( + MathUtil.clamp(turnSimState.getMotorVoltage(), -busVoltage, busVoltage)); + driveSim.update(Robot.defaultPeriodSecs); + turnSim.update(Robot.defaultPeriodSecs); + + // Feed physics results back to Phoenix sim state (rotor level = mechanism × gear ratio) + driveSimState.setRawRotorPosition( + Units.radiansToRotations(driveSim.getAngularPosition()) * constants.DriveMotorGearRatio); + driveSimState.setRotorVelocity( + Units.radiansToRotations(driveSim.getAngularVelocity()) * constants.DriveMotorGearRatio); + turnSimState.setRawRotorPosition( + Units.radiansToRotations(turnSim.getAngularPosition()) * constants.SteerMotorGearRatio); + turnSimState.setRotorVelocity( + Units.radiansToRotations(turnSim.getAngularVelocity()) * constants.SteerMotorGearRatio); + // CANcoder reports mechanism position (1 rotation = full wheel turn) + cancoderSimState.setRawPosition(Units.radiansToRotations(turnSim.getAngularPosition())); + cancoderSimState.setVelocity(Units.radiansToRotations(turnSim.getAngularVelocity())); + + // Refresh and read status signals + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnPosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); + + inputs.driveConnected = true; + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); + + inputs.turnConnected = true; + inputs.turnEncoderConnected = true; + inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); + inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); + inputs.turnZero = Rotation2d.fromRotations(cancoderConfig.MagnetSensor.MagnetOffset); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); + + // 50Hz odometry (high-frequency odometry in sim doesn't matter) + inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; + inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; + inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; + } + + @Override + public void setDriveOpenLoop(double output) { + driveTalon.setControl( + switch (constants.DriveMotorClosedLoopOutput) { + case Voltage -> voltageRequest.withOutput(output); + case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); + }); + } + + @Override + public void setTurnOpenLoop(double output) { + turnTalon.setControl( + switch (constants.SteerMotorClosedLoopOutput) { + case Voltage -> voltageRequest.withOutput(output); + case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); + }); + } + + @Override + public void setDriveVelocity(double velocityRadPerSec) { + double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec); + driveTalon.setControl( + switch (constants.DriveMotorClosedLoopOutput) { + case Voltage -> velocityVoltageRequest.withVelocity(velocityRotPerSec); + case TorqueCurrentFOC -> velocityTorqueCurrentRequest.withVelocity(velocityRotPerSec); + }); + } + + @Override + public void setTurnPosition(Rotation2d rotation) { + turnTalon.setControl( + switch (constants.SteerMotorClosedLoopOutput) { + case Voltage -> positionVoltageRequest.withPosition(rotation.getRotations()); + case TorqueCurrentFOC -> + positionTorqueCurrentRequest.withPosition(rotation.getRotations()); + }); + } + + @Override + public void setTurnZero(Rotation2d rotation) { + cancoderConfig.MagnetSensor.MagnetOffset = rotation.getRotations(); + cancoder.getConfigurator().apply(cancoderConfig); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java index baf112f..10984e3 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java @@ -13,11 +13,11 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import frc.robot.Robot; /** Physics sim implementation of module IO. */ public class ModuleIOSimWPI implements ModuleIO { @@ -45,18 +45,8 @@ public ModuleIOSimWPI( SwerveModuleConstants constants) { // Create drive and turn sim models - driveSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DriveConstants.DRIVE_GEARBOX, - constants.DriveInertia, - constants.DriveMotorGearRatio), - DriveConstants.DRIVE_GEARBOX); - turnSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DriveConstants.TURN_GEARBOX, constants.SteerInertia, constants.SteerMotorGearRatio), - DriveConstants.TURN_GEARBOX); + driveSim = DriveConstants.createDriveSim(); + turnSim = DriveConstants.createTurnSim(); // Enable wrapping for turn PID turnController.enableContinuousInput(-Math.PI, Math.PI); @@ -80,8 +70,8 @@ public void updateInputs(ModuleIOInputs inputs) { double busVoltage = RoboRioSim.getVInVoltage(); driveSim.setInputVoltage(MathUtil.clamp(driveAppliedVolts, -busVoltage, busVoltage)); turnSim.setInputVoltage(MathUtil.clamp(turnAppliedVolts, -busVoltage, busVoltage)); - driveSim.update(0.02); - turnSim.update(0.02); + driveSim.update(Robot.defaultPeriodSecs); + turnSim.update(Robot.defaultPeriodSecs); // Update drive inputs inputs.driveConnected = true; From b61d45abae2028687daed0c494a13bdee3044074 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 26 Jun 2026 11:30:12 -0400 Subject: [PATCH 04/10] Centralize TalonFX motor config into DriveConstants factory methods Add buildDriveConfig(), buildTurnConfig(), and configureCANcoder() to DriveConstants, eliminating ~40 lines of duplicated config mutation from both ModuleIOTalonFX and ModuleIOSimTalonFX constructors. Co-Authored-By: Claude Sonnet 4.6 --- .../subsystems/drive/DriveConstants.java | 58 +++++++++++++++++++ .../subsystems/drive/ModuleIOSimTalonFX.java | 53 +++-------------- .../subsystems/drive/ModuleIOTalonFX.java | 53 +++-------------- 3 files changed, 75 insertions(+), 89 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index d7ffaf0..ba0a85c 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -16,6 +16,10 @@ import com.ctre.phoenix6.configs.TorqueCurrentConfigs; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.ctre.phoenix6.swerve.SwerveDrivetrain; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; @@ -223,6 +227,60 @@ public static DCMotorSim createTurnSim() { TURN_GEARBOX); } + public static TalonFXConfiguration buildDriveConfig( + SwerveModuleConstants + constants) { + var config = constants.DriveMotorInitialConfigs; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.Slot0 = constants.DriveMotorGains; + config.Feedback.SensorToMechanismRatio = constants.DriveMotorGearRatio; + config.MotorOutput.Inverted = + constants.DriveMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + return config; + } + + public static TalonFXConfiguration buildTurnConfig( + SwerveModuleConstants + constants) { + var config = constants.SteerMotorInitialConfigs; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + config.Slot0 = constants.SteerMotorGains; + config.Feedback.FeedbackRemoteSensorID = constants.EncoderId; + config.Feedback.FeedbackSensorSource = + switch (constants.FeedbackSource) { + case RemoteCANcoder -> FeedbackSensorSourceValue.RemoteCANcoder; + case FusedCANcoder -> FeedbackSensorSourceValue.FusedCANcoder; + case SyncCANcoder -> FeedbackSensorSourceValue.SyncCANcoder; + default -> + throw new RuntimeException( + "You are using an unsupported swerve configuration, which this template does not support without manual customization. The 2025 release of Phoenix supports some swerve configurations which were not available during 2025 beta testing, preventing any development and support from the AdvantageKit developers."); + }; + config.Feedback.RotorToSensorRatio = constants.SteerMotorGearRatio; + config.MotionMagic.MotionMagicCruiseVelocity = 100.0 / constants.SteerMotorGearRatio; + config.MotionMagic.MotionMagicAcceleration = + config.MotionMagic.MotionMagicCruiseVelocity / 0.100; + config.MotionMagic.MotionMagicExpo_kV = 0.12 * constants.SteerMotorGearRatio; + config.MotionMagic.MotionMagicExpo_kA = 0.1; + config.ClosedLoopGeneral.ContinuousWrap = true; + config.MotorOutput.Inverted = + constants.SteerMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + return config; + } + + public static void configureCANcoder( + CANcoderConfiguration config, + SwerveModuleConstants + constants) { + config.MagnetSensor.SensorDirection = + constants.EncoderInverted + ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive; + } + public static final SwerveDrivetrainConstants DRIVETRAIN_CONSTANTS = new SwerveDrivetrainConstants().withCANBusName(SC1.BUS.getName()); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimTalonFX.java index 93fe03a..e836590 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSimTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimTalonFX.java @@ -21,10 +21,6 @@ import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.sim.CANcoderSimState; import com.ctre.phoenix6.sim.TalonFXSimState; import com.ctre.phoenix6.swerve.SwerveModuleConstants; @@ -97,51 +93,20 @@ public ModuleIOSimTalonFX( turnTalon = new TalonFX(constants.SteerMotorId, SC1.BUS); cancoder = new CANcoder(constants.EncoderId, SC1.BUS); - // Configure drive motor (mirrors ModuleIOTalonFX; DRIVE_INITIAL_CONFIGS is the source) - var driveConfig = constants.DriveMotorInitialConfigs; - driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - driveConfig.Slot0 = constants.DriveMotorGains; - driveConfig.Feedback.SensorToMechanismRatio = constants.DriveMotorGearRatio; - driveConfig.MotorOutput.Inverted = - constants.DriveMotorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); + // Configure drive motor + tryUntilOk( + 5, + () -> driveTalon.getConfigurator().apply(DriveConstants.buildDriveConfig(constants), 0.25)); tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); - // Configure turn motor (mirrors ModuleIOTalonFX; TURN_INITIAL_CONFIGS is the source) - var turnConfig = constants.SteerMotorInitialConfigs; - turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - turnConfig.Slot0 = constants.SteerMotorGains; - turnConfig.Feedback.FeedbackRemoteSensorID = constants.EncoderId; - turnConfig.Feedback.FeedbackSensorSource = - switch (constants.FeedbackSource) { - case RemoteCANcoder -> FeedbackSensorSourceValue.RemoteCANcoder; - case FusedCANcoder -> FeedbackSensorSourceValue.FusedCANcoder; - case SyncCANcoder -> FeedbackSensorSourceValue.SyncCANcoder; - default -> - throw new RuntimeException( - "You are using an unsupported swerve configuration, which this template does not support without manual customization. The 2025 release of Phoenix supports some swerve configurations which were not available during 2025 beta testing, preventing any development and support from the AdvantageKit developers."); - }; - turnConfig.Feedback.RotorToSensorRatio = constants.SteerMotorGearRatio; - turnConfig.MotionMagic.MotionMagicCruiseVelocity = 100.0 / constants.SteerMotorGearRatio; - turnConfig.MotionMagic.MotionMagicAcceleration = - turnConfig.MotionMagic.MotionMagicCruiseVelocity / 0.100; - turnConfig.MotionMagic.MotionMagicExpo_kV = 0.12 * constants.SteerMotorGearRatio; - turnConfig.MotionMagic.MotionMagicExpo_kA = 0.1; - turnConfig.ClosedLoopGeneral.ContinuousWrap = true; - turnConfig.MotorOutput.Inverted = - constants.SteerMotorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25)); + // Configure turn motor + tryUntilOk( + 5, + () -> turnTalon.getConfigurator().apply(DriveConstants.buildTurnConfig(constants), 0.25)); // Configure CANcoder cancoder.getConfigurator().refresh(cancoderConfig); - cancoderConfig.MagnetSensor.SensorDirection = - constants.EncoderInverted - ? SensorDirectionValue.Clockwise_Positive - : SensorDirectionValue.CounterClockwise_Positive; + DriveConstants.configureCANcoder(cancoderConfig, constants); cancoder.getConfigurator().apply(cancoderConfig); // Get Phoenix sim states diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index a01ad29..e701d8a 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -22,10 +22,6 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.ParentDevice; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Rotation2d; @@ -101,52 +97,19 @@ public ModuleIOTalonFX( cancoder = new CANcoder(constants.EncoderId, SC1.BUS); // Configure drive motor - var driveConfig = constants.DriveMotorInitialConfigs; - driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - driveConfig.Slot0 = constants.DriveMotorGains; - driveConfig.Feedback.SensorToMechanismRatio = constants.DriveMotorGearRatio; - driveConfig.MotorOutput.Inverted = - constants.DriveMotorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); + tryUntilOk( + 5, + () -> driveTalon.getConfigurator().apply(DriveConstants.buildDriveConfig(constants), 0.25)); tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); // Configure turn motor - var turnConfig = constants.SteerMotorInitialConfigs; - turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - turnConfig.Slot0 = constants.SteerMotorGains; - turnConfig.Feedback.FeedbackRemoteSensorID = constants.EncoderId; - turnConfig.Feedback.FeedbackSensorSource = - switch (constants.FeedbackSource) { - case RemoteCANcoder -> FeedbackSensorSourceValue.RemoteCANcoder; - case FusedCANcoder -> FeedbackSensorSourceValue.FusedCANcoder; - case SyncCANcoder -> FeedbackSensorSourceValue.SyncCANcoder; - default -> - throw new RuntimeException( - "You are using an unsupported swerve configuration, which this template does not support without manual customization. The 2025 release of Phoenix supports some swerve configurations which were not available during 2025 beta testing, preventing any development and support from the AdvantageKit developers."); - }; - turnConfig.Feedback.RotorToSensorRatio = constants.SteerMotorGearRatio; - turnConfig.MotionMagic.MotionMagicCruiseVelocity = 100.0 / constants.SteerMotorGearRatio; - turnConfig.MotionMagic.MotionMagicAcceleration = - turnConfig.MotionMagic.MotionMagicCruiseVelocity / 0.100; - turnConfig.MotionMagic.MotionMagicExpo_kV = 0.12 * constants.SteerMotorGearRatio; - turnConfig.MotionMagic.MotionMagicExpo_kA = 0.1; - turnConfig.ClosedLoopGeneral.ContinuousWrap = true; - turnConfig.MotorOutput.Inverted = - constants.SteerMotorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25)); + tryUntilOk( + 5, + () -> turnTalon.getConfigurator().apply(DriveConstants.buildTurnConfig(constants), 0.25)); - // Configure CANCoder - // CANcoderConfiguration cancoderConfig = constants.EncoderInitialConfigs; - // cancoderConfig.MagnetSensor.MagnetOffset = constants.EncoderOffset; + // Configure CANcoder cancoder.getConfigurator().refresh(cancoderConfig); - cancoderConfig.MagnetSensor.SensorDirection = - constants.EncoderInverted - ? SensorDirectionValue.Clockwise_Positive - : SensorDirectionValue.CounterClockwise_Positive; + DriveConstants.configureCANcoder(cancoderConfig, constants); cancoder.getConfigurator().apply(cancoderConfig); // Create timestamp queue From 2f65d6c5a5d7ce218fe0acddddb132085f5fec66 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 26 Jun 2026 11:35:44 -0400 Subject: [PATCH 05/10] Extract ModuleIOTalonFXBase to eliminate duplication between TalonFX IO classes Move shared hardware fields, all 6 control requests, all 9 status signals, device construction, config application, signal initialization, all 5 set* methods, and signal-reading helper into abstract base class. ModuleIOTalonFX and ModuleIOSimTalonFX now only contain what is unique to each: odometry queues/debouncers and physics sim state respectively. Co-Authored-By: Claude Sonnet 4.6 --- .../subsystems/drive/ModuleIOSimTalonFX.java | 155 +--------------- .../subsystems/drive/ModuleIOTalonFX.java | 158 +---------------- .../subsystems/drive/ModuleIOTalonFXBase.java | 165 ++++++++++++++++++ 3 files changed, 172 insertions(+), 306 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimTalonFX.java index e836590..609bcde 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSimTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimTalonFX.java @@ -7,34 +7,17 @@ package frc.robot.subsystems.drive; -import static frc.robot.util.PhoenixUtil.*; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.TorqueCurrentFOC; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.sim.CANcoderSimState; import com.ctre.phoenix6.sim.TalonFXSimState; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; -import frc.robot.Constants.CANBusPorts.SC1; import frc.robot.Robot; /** @@ -42,92 +25,25 @@ * simulation. Uses the same hardware configuration and control requests as ModuleIOTalonFX, with * DCMotorSim physics driving the Phoenix sim state. */ -public class ModuleIOSimTalonFX implements ModuleIO { - private final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - constants; - - // Hardware objects - private final TalonFX driveTalon; - private final TalonFX turnTalon; - private final CANcoder cancoder; - private final CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); - - // Sim states +public class ModuleIOSimTalonFX extends ModuleIOTalonFXBase { private final TalonFXSimState driveSimState; private final TalonFXSimState turnSimState; private final CANcoderSimState cancoderSimState; - // Physics simulation (same gearboxes as ModuleIOSimWPI) private final DCMotorSim driveSim; private final DCMotorSim turnSim; - // Voltage control requests - private final VoltageOut voltageRequest = new VoltageOut(0); - private final PositionVoltage positionVoltageRequest = new PositionVoltage(0.0); - private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); - - // Torque-current control requests - private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0); - private final PositionTorqueCurrentFOC positionTorqueCurrentRequest = - new PositionTorqueCurrentFOC(0.0); - private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = - new VelocityTorqueCurrentFOC(0.0); - - // Status signals - private final StatusSignal drivePosition; - private final StatusSignal driveVelocity; - private final StatusSignal driveAppliedVolts; - private final StatusSignal driveCurrent; - private final StatusSignal turnAbsolutePosition; - private final StatusSignal turnPosition; - private final StatusSignal turnVelocity; - private final StatusSignal turnAppliedVolts; - private final StatusSignal turnCurrent; - public ModuleIOSimTalonFX( SwerveModuleConstants constants) { - this.constants = constants; - driveTalon = new TalonFX(constants.DriveMotorId, SC1.BUS); - turnTalon = new TalonFX(constants.SteerMotorId, SC1.BUS); - cancoder = new CANcoder(constants.EncoderId, SC1.BUS); - - // Configure drive motor - tryUntilOk( - 5, - () -> driveTalon.getConfigurator().apply(DriveConstants.buildDriveConfig(constants), 0.25)); - tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); + super(constants); - // Configure turn motor - tryUntilOk( - 5, - () -> turnTalon.getConfigurator().apply(DriveConstants.buildTurnConfig(constants), 0.25)); - - // Configure CANcoder - cancoder.getConfigurator().refresh(cancoderConfig); - DriveConstants.configureCANcoder(cancoderConfig, constants); - cancoder.getConfigurator().apply(cancoderConfig); - - // Get Phoenix sim states driveSimState = driveTalon.getSimState(); turnSimState = turnTalon.getSimState(); cancoderSimState = cancoder.getSimState(); - // Create physics models driveSim = DriveConstants.createDriveSim(); turnSim = DriveConstants.createTurnSim(); - - // Create status signals - drivePosition = driveTalon.getPosition(); - driveVelocity = driveTalon.getVelocity(); - driveAppliedVolts = driveTalon.getMotorVoltage(); - driveCurrent = driveTalon.getStatorCurrent(); - turnAbsolutePosition = cancoder.getAbsolutePosition(); - turnPosition = turnTalon.getPosition(); - turnVelocity = turnTalon.getVelocity(); - turnAppliedVolts = turnTalon.getMotorVoltage(); - turnCurrent = turnTalon.getStatorCurrent(); } @Override @@ -159,80 +75,15 @@ public void updateInputs(ModuleIOInputs inputs) { cancoderSimState.setRawPosition(Units.radiansToRotations(turnSim.getAngularPosition())); cancoderSimState.setVelocity(Units.radiansToRotations(turnSim.getAngularVelocity())); - // Refresh and read status signals - BaseStatusSignal.refreshAll( - drivePosition, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnPosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); + readSignalInputs(inputs); inputs.driveConnected = true; - inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); - inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); - inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); - inputs.turnConnected = true; inputs.turnEncoderConnected = true; - inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); - inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); - inputs.turnZero = Rotation2d.fromRotations(cancoderConfig.MagnetSensor.MagnetOffset); - inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); - inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); // 50Hz odometry (high-frequency odometry in sim doesn't matter) inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; } - - @Override - public void setDriveOpenLoop(double output) { - driveTalon.setControl( - switch (constants.DriveMotorClosedLoopOutput) { - case Voltage -> voltageRequest.withOutput(output); - case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); - }); - } - - @Override - public void setTurnOpenLoop(double output) { - turnTalon.setControl( - switch (constants.SteerMotorClosedLoopOutput) { - case Voltage -> voltageRequest.withOutput(output); - case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); - }); - } - - @Override - public void setDriveVelocity(double velocityRadPerSec) { - double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec); - driveTalon.setControl( - switch (constants.DriveMotorClosedLoopOutput) { - case Voltage -> velocityVoltageRequest.withVelocity(velocityRotPerSec); - case TorqueCurrentFOC -> velocityTorqueCurrentRequest.withVelocity(velocityRotPerSec); - }); - } - - @Override - public void setTurnPosition(Rotation2d rotation) { - turnTalon.setControl( - switch (constants.SteerMotorClosedLoopOutput) { - case Voltage -> positionVoltageRequest.withPosition(rotation.getRotations()); - case TorqueCurrentFOC -> - positionTorqueCurrentRequest.withPosition(rotation.getRotations()); - }); - } - - @Override - public void setTurnZero(Rotation2d rotation) { - cancoderConfig.MagnetSensor.MagnetOffset = rotation.getRotations(); - cancoder.getConfigurator().apply(cancoderConfig); - } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index e701d8a..3ad7297 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -7,30 +7,14 @@ package frc.robot.subsystems.drive; -import static frc.robot.util.PhoenixUtil.*; - import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.TorqueCurrentFOC; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.ParentDevice; -import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Voltage; -import frc.robot.Constants.CANBusPorts.SC1; import java.util.Queue; /** @@ -39,46 +23,13 @@ * *

Device configuration and other behaviors not exposed by TunerConstants can be customized here. */ -public class ModuleIOTalonFX implements ModuleIO { - private final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - constants; - - // Hardware objects - private final TalonFX driveTalon; - private final TalonFX turnTalon; - private final CANcoder cancoder; - private final CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); - - // Voltage control requests - private final VoltageOut voltageRequest = new VoltageOut(0); - private final PositionVoltage positionVoltageRequest = new PositionVoltage(0.0); - private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); - - // Torque-current control requests - private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0); - private final PositionTorqueCurrentFOC positionTorqueCurrentRequest = - new PositionTorqueCurrentFOC(0.0); - private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = - new VelocityTorqueCurrentFOC(0.0); - +public class ModuleIOTalonFX extends ModuleIOTalonFXBase { // Timestamp inputs from Phoenix thread private final Queue timestampQueue; - // Inputs from drive motor - private final StatusSignal drivePosition; + // High-frequency odometry queues private final Queue drivePositionQueue; - private final StatusSignal driveVelocity; - private final StatusSignal driveAppliedVolts; - private final StatusSignal driveCurrent; - - // Inputs from turn motor - private final StatusSignal turnAbsolutePosition; - private final StatusSignal turnPosition; private final Queue turnPositionQueue; - private final StatusSignal turnVelocity; - private final StatusSignal turnAppliedVolts; - private final StatusSignal turnCurrent; // Connection debouncers private final Debouncer driveConnectedDebounce = @@ -91,46 +42,12 @@ public class ModuleIOTalonFX implements ModuleIO { public ModuleIOTalonFX( SwerveModuleConstants constants) { - this.constants = constants; - driveTalon = new TalonFX(constants.DriveMotorId, SC1.BUS); - turnTalon = new TalonFX(constants.SteerMotorId, SC1.BUS); - cancoder = new CANcoder(constants.EncoderId, SC1.BUS); - - // Configure drive motor - tryUntilOk( - 5, - () -> driveTalon.getConfigurator().apply(DriveConstants.buildDriveConfig(constants), 0.25)); - tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); - - // Configure turn motor - tryUntilOk( - 5, - () -> turnTalon.getConfigurator().apply(DriveConstants.buildTurnConfig(constants), 0.25)); - - // Configure CANcoder - cancoder.getConfigurator().refresh(cancoderConfig); - DriveConstants.configureCANcoder(cancoderConfig, constants); - cancoder.getConfigurator().apply(cancoderConfig); + super(constants); - // Create timestamp queue timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - - // Create drive status signals - drivePosition = driveTalon.getPosition(); drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(drivePosition.clone()); - driveVelocity = driveTalon.getVelocity(); - driveAppliedVolts = driveTalon.getMotorVoltage(); - driveCurrent = driveTalon.getStatorCurrent(); - - // Create turn status signals - turnAbsolutePosition = cancoder.getAbsolutePosition(); - turnPosition = turnTalon.getPosition(); turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(turnPosition.clone()); - turnVelocity = turnTalon.getVelocity(); - turnAppliedVolts = turnTalon.getMotorVoltage(); - turnCurrent = turnTalon.getStatorCurrent(); - // Configure periodic frames BaseStatusSignal.setUpdateFrequencyForAll( Drive.ODOMETRY_FREQUENCY, drivePosition, turnPosition); BaseStatusSignal.setUpdateFrequencyForAll( @@ -147,30 +64,14 @@ public ModuleIOTalonFX( @Override public void updateInputs(ModuleIOInputs inputs) { - BaseStatusSignal.refreshAll( - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnVelocity, - turnAppliedVolts, - turnCurrent, - drivePosition, - turnPosition); + readSignalInputs(inputs); - // Update drive inputs inputs.driveConnected = driveConnectedDebounce.calculate( drivePosition.getStatus().isOK() && driveVelocity.getStatus().isOK() && driveAppliedVolts.getStatus().isOK() && driveCurrent.getStatus().isOK()); - inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); - inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); - inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); - - // Update turn inputs inputs.turnConnected = turnConnectedDebounce.calculate( turnPosition.getStatus().isOK() @@ -179,14 +80,7 @@ public void updateInputs(ModuleIOInputs inputs) { && turnCurrent.getStatus().isOK()); inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnAbsolutePosition.getStatus().isOK()); - inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); - inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); - inputs.turnZero = Rotation2d.fromRotations(cancoderConfig.MagnetSensor.MagnetOffset); - inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); - inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); - // Update odometry inputs inputs.odometryTimestamps = new double[timestampQueue.size()]; for (int i = 0; i < inputs.odometryTimestamps.length; i++) { inputs.odometryTimestamps[i] = timestampQueue.poll(); @@ -200,48 +94,4 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.odometryTurnPositions[i] = Rotation2d.fromRotations(turnPositionQueue.poll()); } } - - @Override - public void setDriveOpenLoop(double output) { - driveTalon.setControl( - switch (constants.DriveMotorClosedLoopOutput) { - case Voltage -> voltageRequest.withOutput(output); - case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); - }); - } - - @Override - public void setTurnOpenLoop(double output) { - turnTalon.setControl( - switch (constants.SteerMotorClosedLoopOutput) { - case Voltage -> voltageRequest.withOutput(output); - case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); - }); - } - - @Override - public void setDriveVelocity(double velocityRadPerSec) { - double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec); - driveTalon.setControl( - switch (constants.DriveMotorClosedLoopOutput) { - case Voltage -> velocityVoltageRequest.withVelocity(velocityRotPerSec); - case TorqueCurrentFOC -> velocityTorqueCurrentRequest.withVelocity(velocityRotPerSec); - }); - } - - @Override - public void setTurnPosition(Rotation2d rotation) { - turnTalon.setControl( - switch (constants.SteerMotorClosedLoopOutput) { - case Voltage -> positionVoltageRequest.withPosition(rotation.getRotations()); - case TorqueCurrentFOC -> - positionTorqueCurrentRequest.withPosition(rotation.getRotations()); - }); - } - - @Override - public void setTurnZero(Rotation2d rotation) { - cancoderConfig.MagnetSensor.MagnetOffset = rotation.getRotations(); - cancoder.getConfigurator().apply(cancoderConfig); - } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java new file mode 100644 index 0000000..26f52cf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java @@ -0,0 +1,165 @@ +// Copyright (c) 2021-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by a BSD +// license that can be found in the LICENSE file +// at the root directory of this project. + +package frc.robot.subsystems.drive; + +import static frc.robot.util.PhoenixUtil.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants.CANBusPorts.SC1; + +/** Shared base for TalonFX-based module IO implementations (hardware and sim). */ +public abstract class ModuleIOTalonFXBase implements ModuleIO { + protected final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + constants; + + protected final TalonFX driveTalon; + protected final TalonFX turnTalon; + protected final CANcoder cancoder; + private final CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); + + // Control requests + private final VoltageOut voltageRequest = new VoltageOut(0); + private final PositionVoltage positionVoltageRequest = new PositionVoltage(0.0); + private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); + private final TorqueCurrentFOC torqueCurrentRequest = new TorqueCurrentFOC(0); + private final PositionTorqueCurrentFOC positionTorqueCurrentRequest = + new PositionTorqueCurrentFOC(0.0); + private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = + new VelocityTorqueCurrentFOC(0.0); + + // Status signals + protected final StatusSignal drivePosition; + protected final StatusSignal driveVelocity; + protected final StatusSignal driveAppliedVolts; + protected final StatusSignal driveCurrent; + protected final StatusSignal turnAbsolutePosition; + protected final StatusSignal turnPosition; + protected final StatusSignal turnVelocity; + protected final StatusSignal turnAppliedVolts; + protected final StatusSignal turnCurrent; + + protected ModuleIOTalonFXBase( + SwerveModuleConstants + constants) { + this.constants = constants; + driveTalon = new TalonFX(constants.DriveMotorId, SC1.BUS); + turnTalon = new TalonFX(constants.SteerMotorId, SC1.BUS); + cancoder = new CANcoder(constants.EncoderId, SC1.BUS); + + tryUntilOk( + 5, + () -> driveTalon.getConfigurator().apply(DriveConstants.buildDriveConfig(constants), 0.25)); + tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); + tryUntilOk( + 5, + () -> turnTalon.getConfigurator().apply(DriveConstants.buildTurnConfig(constants), 0.25)); + + cancoder.getConfigurator().refresh(cancoderConfig); + DriveConstants.configureCANcoder(cancoderConfig, constants); + cancoder.getConfigurator().apply(cancoderConfig); + + drivePosition = driveTalon.getPosition(); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getStatorCurrent(); + turnAbsolutePosition = cancoder.getAbsolutePosition(); + turnPosition = turnTalon.getPosition(); + turnVelocity = turnTalon.getVelocity(); + turnAppliedVolts = turnTalon.getMotorVoltage(); + turnCurrent = turnTalon.getStatorCurrent(); + } + + /** Refreshes all status signals and populates the non-connection, non-odometry inputs. */ + protected void readSignalInputs(ModuleIOInputs inputs) { + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnPosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); + + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); + + inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); + inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()); + inputs.turnZero = Rotation2d.fromRotations(cancoderConfig.MagnetSensor.MagnetOffset); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); + } + + @Override + public void setDriveOpenLoop(double output) { + driveTalon.setControl( + switch (constants.DriveMotorClosedLoopOutput) { + case Voltage -> voltageRequest.withOutput(output); + case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); + }); + } + + @Override + public void setTurnOpenLoop(double output) { + turnTalon.setControl( + switch (constants.SteerMotorClosedLoopOutput) { + case Voltage -> voltageRequest.withOutput(output); + case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); + }); + } + + @Override + public void setDriveVelocity(double velocityRadPerSec) { + double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec); + driveTalon.setControl( + switch (constants.DriveMotorClosedLoopOutput) { + case Voltage -> velocityVoltageRequest.withVelocity(velocityRotPerSec); + case TorqueCurrentFOC -> velocityTorqueCurrentRequest.withVelocity(velocityRotPerSec); + }); + } + + @Override + public void setTurnPosition(Rotation2d rotation) { + turnTalon.setControl( + switch (constants.SteerMotorClosedLoopOutput) { + case Voltage -> positionVoltageRequest.withPosition(rotation.getRotations()); + case TorqueCurrentFOC -> + positionTorqueCurrentRequest.withPosition(rotation.getRotations()); + }); + } + + @Override + public void setTurnZero(Rotation2d rotation) { + cancoderConfig.MagnetSensor.MagnetOffset = rotation.getRotations(); + cancoder.getConfigurator().apply(cancoderConfig); + } +} From 88ae9005e7ab46471edc1175cb36ded445311c33 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 26 Jun 2026 12:07:25 -0400 Subject: [PATCH 06/10] Consolidate TalonFX config into initial config objects Move all non-per-module field mutations (NeutralMode, gains, gear ratios, MotionMagic, FeedbackSensorSource, ContinuousWrap, inversion) from factory methods into DRIVE_INITIAL_CONFIGS and TURN_INITIAL_CONFIGS. Move the remaining per-module mutations (DriveMotorInverted, FeedbackRemoteSensorID, EncoderInverted) directly into the ModuleIOTalonFXBase constructor. Removes buildDriveConfig(), buildTurnConfig(), and configureCANcoder() from DriveConstants entirely. Co-Authored-By: Claude Sonnet 4.6 --- .../subsystems/drive/DriveConstants.java | 102 ++++++------------ .../subsystems/drive/ModuleIOTalonFXBase.java | 23 ++-- 2 files changed, 47 insertions(+), 78 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index ba0a85c..f01ea27 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -10,7 +10,11 @@ import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TorqueCurrentConfigs; @@ -19,7 +23,6 @@ import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.ctre.phoenix6.swerve.SwerveDrivetrain; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; @@ -170,17 +173,21 @@ public class DriveConstants { // TorqueCurrent peak at which the wheels start to slip; used for slip detection in // TorqueCurrentFOC control mode. This needs to be tuned to your individual robot. - static final int SLIP_CURRENT = 120; + private static final int SLIP_CURRENT = 120; // Hardware stator current limit for drive motors - static final int DRIVE_STATOR_CURRENT_LIMIT = KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; + private static final int DRIVE_STATOR_CURRENT_LIMIT = + KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; // Stator current limit for azimuth (steer) motors; lower than drive to reduce brownout risk // since steering requires minimal torque compared to driving. - static final int STEER_STATOR_CURRENT_LIMIT = 60; + private static final int STEER_STATOR_CURRENT_LIMIT = 60; - public static final TalonFXConfiguration DRIVE_INITIAL_CONFIGS = + private static final TalonFXConfiguration DRIVE_INITIAL_CONFIGS = new TalonFXConfiguration() + .withMotorOutput(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake)) + .withSlot0(DRIVE_GAINS) + .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(DRIVE_MOTOR_REDUCTION)) .withTorqueCurrent( new TorqueCurrentConfigs() .withPeakForwardTorqueCurrent(SLIP_CURRENT) @@ -194,8 +201,24 @@ public class DriveConstants { // Azimuth does not require much torque; keep stator limit low to reduce brownout risk // since steering requires minimal torque compared to driving. - public static final TalonFXConfiguration TURN_INITIAL_CONFIGS = + private static final TalonFXConfiguration TURN_INITIAL_CONFIGS = new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withInverted(InvertedValue.CounterClockwise_Positive)) + .withSlot0(STEER_GAINS) + .withFeedback( + new FeedbackConfigs() + .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder) + .withRotorToSensorRatio(TURN_MOTOR_REDUCTION)) + .withMotionMagic( + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(100.0 / TURN_MOTOR_REDUCTION) + .withMotionMagicAcceleration(100.0 / TURN_MOTOR_REDUCTION / 0.100) + .withMotionMagicExpo_kV(0.12 * TURN_MOTOR_REDUCTION) + .withMotionMagicExpo_kA(0.1)) + .withClosedLoopGeneral(new ClosedLoopGeneralConfigs().withContinuousWrap(true)) .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimit(STEER_STATOR_CURRENT_LIMIT) @@ -213,74 +236,20 @@ public class DriveConstants { private static final Voltage STEER_FRICTION_VOLTAGE = Volts.of(0.2); private static final Voltage DRIVE_FRICTION_VOLTAGE = Volts.of(0.2); - public static DCMotorSim createDriveSim() { + static DCMotorSim createDriveSim() { return new DCMotorSim( LinearSystemId.createDCMotorSystem( DRIVE_GEARBOX, DRIVE_INERTIA.in(KilogramSquareMeters), DRIVE_MOTOR_REDUCTION), DRIVE_GEARBOX); } - public static DCMotorSim createTurnSim() { + static DCMotorSim createTurnSim() { return new DCMotorSim( LinearSystemId.createDCMotorSystem( TURN_GEARBOX, STEER_INERTIA.in(KilogramSquareMeters), TURN_MOTOR_REDUCTION), TURN_GEARBOX); } - public static TalonFXConfiguration buildDriveConfig( - SwerveModuleConstants - constants) { - var config = constants.DriveMotorInitialConfigs; - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.Slot0 = constants.DriveMotorGains; - config.Feedback.SensorToMechanismRatio = constants.DriveMotorGearRatio; - config.MotorOutput.Inverted = - constants.DriveMotorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - return config; - } - - public static TalonFXConfiguration buildTurnConfig( - SwerveModuleConstants - constants) { - var config = constants.SteerMotorInitialConfigs; - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.Slot0 = constants.SteerMotorGains; - config.Feedback.FeedbackRemoteSensorID = constants.EncoderId; - config.Feedback.FeedbackSensorSource = - switch (constants.FeedbackSource) { - case RemoteCANcoder -> FeedbackSensorSourceValue.RemoteCANcoder; - case FusedCANcoder -> FeedbackSensorSourceValue.FusedCANcoder; - case SyncCANcoder -> FeedbackSensorSourceValue.SyncCANcoder; - default -> - throw new RuntimeException( - "You are using an unsupported swerve configuration, which this template does not support without manual customization. The 2025 release of Phoenix supports some swerve configurations which were not available during 2025 beta testing, preventing any development and support from the AdvantageKit developers."); - }; - config.Feedback.RotorToSensorRatio = constants.SteerMotorGearRatio; - config.MotionMagic.MotionMagicCruiseVelocity = 100.0 / constants.SteerMotorGearRatio; - config.MotionMagic.MotionMagicAcceleration = - config.MotionMagic.MotionMagicCruiseVelocity / 0.100; - config.MotionMagic.MotionMagicExpo_kV = 0.12 * constants.SteerMotorGearRatio; - config.MotionMagic.MotionMagicExpo_kA = 0.1; - config.ClosedLoopGeneral.ContinuousWrap = true; - config.MotorOutput.Inverted = - constants.SteerMotorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - return config; - } - - public static void configureCANcoder( - CANcoderConfiguration config, - SwerveModuleConstants - constants) { - config.MagnetSensor.SensorDirection = - constants.EncoderInverted - ? SensorDirectionValue.Clockwise_Positive - : SensorDirectionValue.CounterClockwise_Positive; - } - public static final SwerveDrivetrainConstants DRIVETRAIN_CONSTANTS = new SwerveDrivetrainConstants().withCANBusName(SC1.BUS.getName()); @@ -362,15 +331,6 @@ public static void configureCANcoder( TURN_INVERTED, TURN_ENCODER_INVERTED); - /** - * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot - * program,. - */ - // public static CommandSwerveDrivetrain createDrivetrain() { - // return new CommandSwerveDrivetrain( - // DRIVETRAIN_CONSTANTS, FRONT_LEFT, FRONT_RIGHT, BACK_LEFT, BACK_RIGHT); - // } - /** Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ public static class TunerSwerveDrivetrain extends SwerveDrivetrain { /** diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java index 26f52cf..fae5bf0 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java @@ -21,6 +21,8 @@ import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; @@ -70,16 +72,23 @@ protected ModuleIOTalonFXBase( turnTalon = new TalonFX(constants.SteerMotorId, SC1.BUS); cancoder = new CANcoder(constants.EncoderId, SC1.BUS); - tryUntilOk( - 5, - () -> driveTalon.getConfigurator().apply(DriveConstants.buildDriveConfig(constants), 0.25)); + var driveConfig = constants.DriveMotorInitialConfigs; + driveConfig.MotorOutput.Inverted = + constants.DriveMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); - tryUntilOk( - 5, - () -> turnTalon.getConfigurator().apply(DriveConstants.buildTurnConfig(constants), 0.25)); + + var turnConfig = constants.SteerMotorInitialConfigs; + turnConfig.Feedback.FeedbackRemoteSensorID = constants.EncoderId; + tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25)); cancoder.getConfigurator().refresh(cancoderConfig); - DriveConstants.configureCANcoder(cancoderConfig, constants); + cancoderConfig.MagnetSensor.SensorDirection = + constants.EncoderInverted + ? SensorDirectionValue.Clockwise_Positive + : SensorDirectionValue.CounterClockwise_Positive; cancoder.getConfigurator().apply(cancoderConfig); drivePosition = driveTalon.getPosition(); From 19d2b7c23f67f0cc94b3a47e09afc2cdb92e8b6d Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 26 Jun 2026 12:55:17 -0400 Subject: [PATCH 07/10] Inline single-use constants into CONSTANT_CREATOR Remove private static final intermediates that were only referenced once: closed-loop output types, motor/feedback types, stator current limit, friction voltages, and both initial TalonFXConfiguration objects. All config now lives directly in CONSTANT_CREATOR, keeping only named constants that have a meaningful rationale or are used in more than one place (STEER_GAINS, DRIVE_GAINS, SLIP_CURRENT, STEER_STATOR_CURRENT_LIMIT). Co-Authored-By: Claude Sonnet 4.6 --- .../subsystems/drive/DriveConstants.java | 131 +++++++----------- 1 file changed, 51 insertions(+), 80 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index f01ea27..ea33a7d 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -48,7 +48,6 @@ import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Mass; import edu.wpi.first.units.measure.MomentOfInertia; -import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.Constants.CANBusPorts.SC1; import frc.robot.Constants.MotorConstants.KrakenX60Constants; @@ -152,89 +151,20 @@ public class DriveConstants { private static final Slot0Configs DRIVE_GAINS = new Slot0Configs().withKP(10).withKI(0).withKD(0).withKS(0).withKV(0.124); - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType STEER_CLOSED_LOOP_OUTPUT = - ClosedLoopOutputType.TorqueCurrentFOC; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType DRIVE_CLOSED_LOOP_OUTPUT = - ClosedLoopOutputType.TorqueCurrentFOC; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement DRIVE_MOTOR_TYPE = - DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the steer motor - private static final SteerMotorArrangement STEER_MOTOR_TYPE = - SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors - private static final SteerFeedbackType STEER_FEEDBACK_TYPE = SteerFeedbackType.FusedCANcoder; - // TorqueCurrent peak at which the wheels start to slip; used for slip detection in // TorqueCurrentFOC control mode. This needs to be tuned to your individual robot. private static final int SLIP_CURRENT = 120; - // Hardware stator current limit for drive motors - private static final int DRIVE_STATOR_CURRENT_LIMIT = - KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; - // Stator current limit for azimuth (steer) motors; lower than drive to reduce brownout risk // since steering requires minimal torque compared to driving. private static final int STEER_STATOR_CURRENT_LIMIT = 60; - private static final TalonFXConfiguration DRIVE_INITIAL_CONFIGS = - new TalonFXConfiguration() - .withMotorOutput(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake)) - .withSlot0(DRIVE_GAINS) - .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(DRIVE_MOTOR_REDUCTION)) - .withTorqueCurrent( - new TorqueCurrentConfigs() - .withPeakForwardTorqueCurrent(SLIP_CURRENT) - .withPeakReverseTorqueCurrent(-SLIP_CURRENT)) - .withCurrentLimits( - new CurrentLimitsConfigs() - .withStatorCurrentLimit(DRIVE_STATOR_CURRENT_LIMIT) - .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT) - .withSupplyCurrentLimitEnable(true)); - - // Azimuth does not require much torque; keep stator limit low to reduce brownout risk - // since steering requires minimal torque compared to driving. - private static final TalonFXConfiguration TURN_INITIAL_CONFIGS = - new TalonFXConfiguration() - .withMotorOutput( - new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Brake) - .withInverted(InvertedValue.CounterClockwise_Positive)) - .withSlot0(STEER_GAINS) - .withFeedback( - new FeedbackConfigs() - .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder) - .withRotorToSensorRatio(TURN_MOTOR_REDUCTION)) - .withMotionMagic( - new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(100.0 / TURN_MOTOR_REDUCTION) - .withMotionMagicAcceleration(100.0 / TURN_MOTOR_REDUCTION / 0.100) - .withMotionMagicExpo_kV(0.12 * TURN_MOTOR_REDUCTION) - .withMotionMagicExpo_kA(0.1)) - .withClosedLoopGeneral(new ClosedLoopGeneralConfigs().withContinuousWrap(true)) - .withCurrentLimits( - new CurrentLimitsConfigs() - .withStatorCurrentLimit(STEER_STATOR_CURRENT_LIMIT) - .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT) - .withSupplyCurrentLimitEnable(true)); - private static final boolean INVERT_LEFT_SIDE = false; private static final boolean INVERT_RIGHT_SIDE = false; // These are only used for simulation private static final MomentOfInertia STEER_INERTIA = KilogramSquareMeters.of(0.004); private static final MomentOfInertia DRIVE_INERTIA = KilogramSquareMeters.of(0.025); - // Simulated voltage necessary to overcome friction - private static final Voltage STEER_FRICTION_VOLTAGE = Volts.of(0.2); - private static final Voltage DRIVE_FRICTION_VOLTAGE = Volts.of(0.2); static DCMotorSim createDriveSim() { return new DCMotorSim( @@ -264,19 +194,60 @@ static DCMotorSim createTurnSim() { .withWheelRadius(WHEEL_RADIUS) .withSteerMotorGains(STEER_GAINS) .withDriveMotorGains(DRIVE_GAINS) - .withSteerMotorClosedLoopOutput(STEER_CLOSED_LOOP_OUTPUT) - .withDriveMotorClosedLoopOutput(DRIVE_CLOSED_LOOP_OUTPUT) + .withSteerMotorClosedLoopOutput(ClosedLoopOutputType.TorqueCurrentFOC) + .withDriveMotorClosedLoopOutput(ClosedLoopOutputType.TorqueCurrentFOC) .withSlipCurrent(Amps.of(SLIP_CURRENT)) .withSpeedAt12Volts(DRIVETRAIN_SPEED_LIMIT) - .withDriveMotorType(DRIVE_MOTOR_TYPE) - .withSteerMotorType(STEER_MOTOR_TYPE) - .withFeedbackSource(STEER_FEEDBACK_TYPE) - .withDriveMotorInitialConfigs(DRIVE_INITIAL_CONFIGS) - .withSteerMotorInitialConfigs(TURN_INITIAL_CONFIGS) + .withDriveMotorType(DriveMotorArrangement.TalonFX_Integrated) + .withSteerMotorType(SteerMotorArrangement.TalonFX_Integrated) + .withFeedbackSource(SteerFeedbackType.FusedCANcoder) + .withDriveMotorInitialConfigs( + new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake)) + .withSlot0(DRIVE_GAINS) + .withFeedback( + new FeedbackConfigs().withSensorToMechanismRatio(DRIVE_MOTOR_REDUCTION)) + .withTorqueCurrent( + new TorqueCurrentConfigs() + .withPeakForwardTorqueCurrent(SLIP_CURRENT) + .withPeakReverseTorqueCurrent(-SLIP_CURRENT)) + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit( + KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit( + KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .withSupplyCurrentLimitEnable(true))) + .withSteerMotorInitialConfigs( + new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withInverted(InvertedValue.CounterClockwise_Positive)) + .withSlot0(STEER_GAINS) + .withFeedback( + new FeedbackConfigs() + .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder) + .withRotorToSensorRatio(TURN_MOTOR_REDUCTION)) + .withMotionMagic( + new MotionMagicConfigs() + .withMotionMagicCruiseVelocity(100.0 / TURN_MOTOR_REDUCTION) + .withMotionMagicAcceleration(100.0 / TURN_MOTOR_REDUCTION / 0.100) + .withMotionMagicExpo_kV(0.12 * TURN_MOTOR_REDUCTION) + .withMotionMagicExpo_kA(0.1)) + .withClosedLoopGeneral( + new ClosedLoopGeneralConfigs().withContinuousWrap(true)) + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(STEER_STATOR_CURRENT_LIMIT) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit( + KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .withSupplyCurrentLimitEnable(true))) .withSteerInertia(STEER_INERTIA) - .withDriveInertia(DRIVE_INERTIA) - .withSteerFrictionVoltage(STEER_FRICTION_VOLTAGE) - .withDriveFrictionVoltage(DRIVE_FRICTION_VOLTAGE); + .withDriveInertia(DRIVE_INERTIA); public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> From 5a6af86b3d8b806baf74e0d43900a4ec74967710 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 26 Jun 2026 12:59:28 -0400 Subject: [PATCH 08/10] Fix DRIVE_MOTOR_REDUCTION references after merge from main Replace two remaining DRIVE_MOTOR_REDUCTION usages (createDriveSim and withSensorToMechanismRatio) with SELECTED_RATIO.getDriveMotorReduction() following the DriveGearRatio enum introduced in main. Co-Authored-By: Claude Sonnet 4.6 --- .../java/frc/robot/subsystems/drive/DriveConstants.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 49985b2..1b80e7e 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -206,7 +206,9 @@ public double getCoupleRatio() { static DCMotorSim createDriveSim() { return new DCMotorSim( LinearSystemId.createDCMotorSystem( - DRIVE_GEARBOX, DRIVE_INERTIA.in(KilogramSquareMeters), DRIVE_MOTOR_REDUCTION), + DRIVE_GEARBOX, + DRIVE_INERTIA.in(KilogramSquareMeters), + SELECTED_RATIO.getDriveMotorReduction()), DRIVE_GEARBOX); } @@ -244,7 +246,8 @@ static DCMotorSim createTurnSim() { new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake)) .withSlot0(DRIVE_GAINS) .withFeedback( - new FeedbackConfigs().withSensorToMechanismRatio(DRIVE_MOTOR_REDUCTION)) + new FeedbackConfigs() + .withSensorToMechanismRatio(SELECTED_RATIO.getDriveMotorReduction())) .withTorqueCurrent( new TorqueCurrentConfigs() .withPeakForwardTorqueCurrent(SLIP_CURRENT) From ad81f1f9ce30a3f361998a84791eb6660c33e8ef Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 26 Jun 2026 13:19:16 -0400 Subject: [PATCH 09/10] Update copyright statements --- .../java/frc/robot/subsystems/drive/ModuleIOSimTalonFX.java | 3 +++ src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java | 3 +++ src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java | 3 +++ .../java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java | 3 +++ 4 files changed, 12 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimTalonFX.java index 609bcde..bd3c2eb 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSimTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimTalonFX.java @@ -1,6 +1,9 @@ // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage // +// Modified work Copyright (c) 2026 Triple Helix Robotics, FRC Team 2363 +// https://github.com/TripleHelixProgramming +// // Use of this source code is governed by a BSD // license that can be found in the LICENSE file // at the root directory of this project. diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java index 10984e3..eb2f576 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java @@ -1,6 +1,9 @@ // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage // +// Modified work Copyright (c) 2026 Triple Helix Robotics, FRC Team 2363 +// https://github.com/TripleHelixProgramming +// // Use of this source code is governed by a BSD // license that can be found in the LICENSE file // at the root directory of this project. diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 3ad7297..7f4bc71 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -1,6 +1,9 @@ // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage // +// Modified work Copyright (c) 2026 Triple Helix Robotics, FRC Team 2363 +// https://github.com/TripleHelixProgramming +// // Use of this source code is governed by a BSD // license that can be found in the LICENSE file // at the root directory of this project. diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java index fae5bf0..cddeab4 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java @@ -1,6 +1,9 @@ // Copyright (c) 2021-2026 Littleton Robotics // http://github.com/Mechanical-Advantage // +// Modified work Copyright (c) 2026 Triple Helix Robotics, FRC Team 2363 +// https://github.com/TripleHelixProgramming +// // Use of this source code is governed by a BSD // license that can be found in the LICENSE file // at the root directory of this project. From 60531a54fd67e3ac2a0e85442d03ffd5eb55427d Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 26 Jun 2026 13:23:12 -0400 Subject: [PATCH 10/10] Move per-module config reconciliation into DriveConstants Add configureModule() to bake DriveMotorInverted into DriveMotorInitialConfigs.MotorOutput.Inverted and EncoderId into SteerMotorInitialConfigs.Feedback.FeedbackRemoteSensorID immediately after createModuleConstants(). ModuleIOTalonFXBase can now apply constants.DriveMotorInitialConfigs and constants.SteerMotorInitialConfigs directly without mutation, keeping all configuration logic in DriveConstants. Co-Authored-By: Claude Sonnet 4.6 --- .../subsystems/drive/DriveConstants.java | 97 +++++++++++-------- .../subsystems/drive/ModuleIOTalonFXBase.java | 15 +-- 2 files changed, 61 insertions(+), 51 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 1b80e7e..2f72562 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -219,6 +219,19 @@ static DCMotorSim createTurnSim() { TURN_GEARBOX); } + private static SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + configureModule( + SwerveModuleConstants + constants) { + constants.DriveMotorInitialConfigs.MotorOutput.Inverted = + constants.DriveMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + constants.SteerMotorInitialConfigs.Feedback.FeedbackRemoteSensorID = constants.EncoderId; + return constants; + } + public static final SwerveDrivetrainConstants DRIVETRAIN_CONSTANTS = new SwerveDrivetrainConstants().withCANBusName(SC1.BUS.getName()); @@ -292,55 +305,59 @@ static DCMotorSim createTurnSim() { public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FRONT_LEFT = - CONSTANT_CREATOR.createModuleConstants( - SC1.FRONT_LEFT_TURN, - SC1.FRONT_LEFT_DRIVE, - SC1.FRONT_LEFT_TURN_ABS_ENC, - Rotations.of(0), - WHEEL_BASE.div(2.0), - TRACK_WIDTH.div(2.0), - INVERT_LEFT_SIDE, - TURN_INVERTED, - TURN_ENCODER_INVERTED); + configureModule( + CONSTANT_CREATOR.createModuleConstants( + SC1.FRONT_LEFT_TURN, + SC1.FRONT_LEFT_DRIVE, + SC1.FRONT_LEFT_TURN_ABS_ENC, + Rotations.of(0), + WHEEL_BASE.div(2.0), + TRACK_WIDTH.div(2.0), + INVERT_LEFT_SIDE, + TURN_INVERTED, + TURN_ENCODER_INVERTED)); public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FRONT_RIGHT = - CONSTANT_CREATOR.createModuleConstants( - SC1.FRONT_RIGHT_TURN, - SC1.FRONT_RIGHT_DRIVE, - SC1.FRONT_RIGHT_TURN_ABS_ENC, - Rotations.of(0), - WHEEL_BASE.div(2.0), - TRACK_WIDTH.div(-2.0), - INVERT_RIGHT_SIDE, - TURN_INVERTED, - TURN_ENCODER_INVERTED); + configureModule( + CONSTANT_CREATOR.createModuleConstants( + SC1.FRONT_RIGHT_TURN, + SC1.FRONT_RIGHT_DRIVE, + SC1.FRONT_RIGHT_TURN_ABS_ENC, + Rotations.of(0), + WHEEL_BASE.div(2.0), + TRACK_WIDTH.div(-2.0), + INVERT_RIGHT_SIDE, + TURN_INVERTED, + TURN_ENCODER_INVERTED)); public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BACK_LEFT = - CONSTANT_CREATOR.createModuleConstants( - SC1.BACK_LEFT_TURN, - SC1.BACK_LEFT_DRIVE, - SC1.BACK_LEFT_TURN_ABS_ENC, - Rotations.of(0), - WHEEL_BASE.div(-2.0), - TRACK_WIDTH.div(2.0), - INVERT_LEFT_SIDE, - TURN_INVERTED, - TURN_ENCODER_INVERTED); + configureModule( + CONSTANT_CREATOR.createModuleConstants( + SC1.BACK_LEFT_TURN, + SC1.BACK_LEFT_DRIVE, + SC1.BACK_LEFT_TURN_ABS_ENC, + Rotations.of(0), + WHEEL_BASE.div(-2.0), + TRACK_WIDTH.div(2.0), + INVERT_LEFT_SIDE, + TURN_INVERTED, + TURN_ENCODER_INVERTED)); public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BACK_RIGHT = - CONSTANT_CREATOR.createModuleConstants( - SC1.BACK_RIGHT_TURN, - SC1.BACK_RIGHT_DRIVE, - SC1.BACK_RIGHT_TURN_ABS_ENC, - Rotations.of(0), - WHEEL_BASE.div(-2.0), - TRACK_WIDTH.div(-2.0), - INVERT_RIGHT_SIDE, - TURN_INVERTED, - TURN_ENCODER_INVERTED); + configureModule( + CONSTANT_CREATOR.createModuleConstants( + SC1.BACK_RIGHT_TURN, + SC1.BACK_RIGHT_DRIVE, + SC1.BACK_RIGHT_TURN_ABS_ENC, + Rotations.of(0), + WHEEL_BASE.div(-2.0), + TRACK_WIDTH.div(-2.0), + INVERT_RIGHT_SIDE, + TURN_INVERTED, + TURN_ENCODER_INVERTED)); /** Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ public static class TunerSwerveDrivetrain extends SwerveDrivetrain { diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java index cddeab4..3e80317 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXBase.java @@ -24,7 +24,6 @@ import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import edu.wpi.first.math.geometry.Rotation2d; @@ -75,17 +74,11 @@ protected ModuleIOTalonFXBase( turnTalon = new TalonFX(constants.SteerMotorId, SC1.BUS); cancoder = new CANcoder(constants.EncoderId, SC1.BUS); - var driveConfig = constants.DriveMotorInitialConfigs; - driveConfig.MotorOutput.Inverted = - constants.DriveMotorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - tryUntilOk(5, () -> driveTalon.getConfigurator().apply(driveConfig, 0.25)); + tryUntilOk( + 5, () -> driveTalon.getConfigurator().apply(constants.DriveMotorInitialConfigs, 0.25)); tryUntilOk(5, () -> driveTalon.setPosition(0.0, 0.25)); - - var turnConfig = constants.SteerMotorInitialConfigs; - turnConfig.Feedback.FeedbackRemoteSensorID = constants.EncoderId; - tryUntilOk(5, () -> turnTalon.getConfigurator().apply(turnConfig, 0.25)); + tryUntilOk( + 5, () -> turnTalon.getConfigurator().apply(constants.SteerMotorInitialConfigs, 0.25)); cancoder.getConfigurator().refresh(cancoderConfig); cancoderConfig.MagnetSensor.SensorDirection =