Task/2025 refactor sim

src/main/java/frc/robot/Constants.java

@@ -25,6 +25,7 @@ public static final class DriverLabels {
     }
 
     public static class Drive {
+        // (x, y) position of each module relative to the robot center (center of rotation)
         public static final Translation2d SMFrontRightLocation = new Translation2d(0.285, -0.285);
         public static final Translation2d SMFrontLeftLocation = new Translation2d(0.285, 0.285);
         public static final Translation2d SMBackLeftLocation = new Translation2d(-0.285, 0.285);
@@ -88,23 +89,23 @@ public static class AprilTagID {
     }
 
     public static class Port {
-        public static final int blSteerMtrC = 1;
-        public static final int blDriveMtrC = 2;
-        public static final int flDriveMtrC = 3;
-        public static final int flSteerMtrC = 4;
-        public static final int frSteerMtrC = 5;
-        public static final int frDriveMtrC = 6;
-        public static final int brDriveMtrC = 7;
-        public static final int brSteerMtrC = 8;
-        public static final int elevatorMotorChannel = 11;
-        public static final int climbMotorChannel = 12;
-        public static final int coralMotorRightChannel = 13;
-        public static final int coralMotorLeftChannel = 14;
-        public static final int elevatorFollowerMotorChannel = 15;
-        public static final int blTurnEncoderDIOC = DIOChannel.ZERO.getChannel();
-        public static final int flTurnEncoderDIOC = DIOChannel.ONE.getChannel();
-        public static final int frTurnEncoderDIOC = DIOChannel.TWO.getChannel();
-        public static final int brTurnEncoderDIOC = DIOChannel.THREE.getChannel();
+        public static final int REAR_LEFT_TURN_CHANNEL = 1;
+        public static final int REAR_LEFT_DRIVE_CHANNEL = 2;
+        public static final int FRONT_LEFT_DRIVE_CHANNEL = 3;
+        public static final int FRONT_LEFT_STEER_CHANNEL = 4;
+        public static final int FRONT_RIGHT_STEER_CHANNEL = 5;
+        public static final int FRONT_RIGHT_DRIVE_CHANNEL = 6;
+        public static final int REAR_RIGHT_DRIVE_CHANNEL = 7;
+        public static final int REAR_RIGHT_STEER_CHANNEL = 8;
+        public static final int ELEVATOR_DRIVE_CHANNEL = 11;
+        public static final int CLIMB_DRIVE_CHANNEL = 12;
+        public static final int RIGHT_CORAL_DRIVE_CHANNEL = 13;
+        public static final int LEFT_CORAL_DRIVE_CHANNEL = 14;
+        public static final int ELEVATOR_FOLLOWER_DRIVE_CHANNEL = 15;
+        public static final int REAR_LEFT_TURN_ENCODER_DIO_CHANNEL = DIOChannel.ZERO.getChannel();
+        public static final int FRONT_LEFT_TURN_ENCODER_DIO_CHANNEL = DIOChannel.ONE.getChannel();
+        public static final int FRONT_RIGHT_TURN_ENCODER_DIO_CHANNEL = DIOChannel.TWO.getChannel();
+        public static final int REAR_RIGHT_TURN_ENCODER_DIO_CHANNEL = DIOChannel.THREE.getChannel();
         public static final int climbMagSwitchDIOC = DIOChannel.FOUR.getChannel();
         ;
         public static final int hangerLeftMagSwitchDIOC = DIOChannel.SEVEN.getChannel();
@@ -129,15 +130,15 @@ public abstract class RobotVersionConstants {
     public class RobotVersion2025 extends RobotVersionConstants {
         public static final double flTurnEncoderOffset = 3.84 - .04 + Math.PI;
         public static final double frTurnEncoderOffset = 1.7 + Math.PI - .03 + Math.PI;
-        public static final double blTurnEncoderOffset = 3.284 + Math.PI;
-        public static final double brTurnEncoderOffset = 4.49 + Math.PI;
+        public static final double rlTurnEncoderOffset = 3.284 + Math.PI;
+        public static final double rrTurnEncoderOffset = 4.49 + Math.PI;
     }
 
     public class RobotVersion2023 extends RobotVersionConstants {
         public static final double flTurnEncoderOffset = 5.3038;
         public static final double frTurnEncoderOffset = Math.PI / 2 - 0.1242 - .05759;
-        public static final double blTurnEncoderOffset = 4.2 + 0.0385;
-        public static final double brTurnEncoderOffset = 2.736 - .06098;
+        public static final double rlTurnEncoderOffset = 4.2 + 0.0385;
+        public static final double rrTurnEncoderOffset = 2.736 - .06098;
     }
 
     public static final class Poses {

src/main/java/frc/robot/RobotContainer.java

@@ -148,6 +148,7 @@ public class RobotContainer {
 
     private final CommandXboxController manipController =
             new CommandXboxController(Constants.Controller.MANIPULATION_CONTROLLER_CHANNEL);
+
     private final CommandXboxController debugController =
             new CommandXboxController(Constants.Controller.DEBUG_CONTROLLER_CHANNEL);
 

src/main/java/frc/robot/commands/swervedrive/ControllerDelegate.java

@@ -41,6 +41,13 @@ public ControllerDelegate(
         this.driver = driver;
     }
 
+    public void logRawAxes() {
+        for (int i = 0; i < 10; i++) {
+            double value = edu.wpi.first.wpilibj.DriverStation.getStickAxis(0, i);
+            System.out.println("Axis[" + i + "]: " + value);
+        }
+    }
+
     public double getLeftX() {
         return this.leftXSupplier.getAsDouble();
     }

src/main/java/frc/robot/commands/swervedrive/SwerveDriveCommand.java

@@ -63,17 +63,6 @@ public void execute() {
             ySpeed = x * this.driveMaxSpeed * acceleration * elevatorDecelerationRatio;
         }
 
-        // TODO: x and y are assigned to and never used again. I'm assuming this is just an oversight in the code.
-        // To be clear, this is an issue because x and y and on the local scope of this method... did the author intend
-        // for their values to be normalized and retained across executions... because they're not, as they are
-        // stack-level
-        // variables so commenting this out...
-        // double normalizingFactor = Math.hypot(x, y);
-        // if (normalizingFactor > 0) {
-        // x /= normalizingFactor;
-        // y /= normalizingFactor;
-        // }
-
         if (this.controllerDelegate.isHalfSpeed()) {
             xSpeed /= 2;
             ySpeed /= 2;

src/main/java/frc/robot/sim/SwerveModuleSim.java

@@ -41,7 +41,7 @@ public void update(double dtSeconds) {
         this.driveDistanceMeters += wheelSpeed * dtSeconds;
 
         double turnRotations = turnMotorSim.getAngularPositionRotations();
-        this.turnAngle = Rotation2d.fromRotations(turnRotations % 1.0);
+        this.turnAngle = Rotation2d.fromRotations(turnRotations);
     }
 
     public void setDriveVoltage(double volts) {

src/main/java/frc/robot/subsystems/LedStrand.java

@@ -31,7 +31,6 @@ public void stopLed() {
     }
 
     public void changeLed(int r, int g, int b) {
-        System.out.println("Here");
         sendSerialCommand("R" + r + "G" + g + "B" + b);
     }
 

src/main/java/frc/robot/subsystems/climb/ClimbMechanism.java

@@ -1,10 +1,13 @@
 package frc.robot.subsystems.climb;
 
+import static edu.wpi.first.wpilibj.RobotBase.isSimulation;
 import static java.util.Objects.requireNonNull;
 
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.config.AlternateEncoderConfig;
+import com.revrobotics.spark.config.ClosedLoopConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import edu.wpi.first.util.sendable.Sendable;
@@ -13,6 +16,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.support.PIDSettings;
 import frc.robot.support.sparkmax.TeamSparkMax;
 
 /**
@@ -36,8 +40,7 @@ public ClimbMechanism() {
     /**
      * Instantiates a new ClimbMechanism subsystem with the specified settings
      *
-     * @param context
-     *            The ClimbMechanismSettings to apply to this instance
+     * @param context The ClimbMechanismSettings to apply to this instance
      */
     public ClimbMechanism(final ClimbMechanismContext context) {
         requireNonNull(context, "ClimbMechanismContext cannot be null");
@@ -59,10 +62,24 @@ private SparkMaxConfig assembleClimbMotorConfig() {
         config.encoder
                 .positionConversionFactor(this.context.getClimbPositionConversionFactor())
                 .velocityConversionFactor(this.context.getClimbVelocityConversionFactor());
-        // PIDSettings pidSettings = this.context.getClimbControllerPIDSettings();
-        // config
-        // .closedLoop.feedbackSensor(FeedbackSensor.kAlternateOrExternalEncoder)
-        // .pid(pidSettings.p(), pidSettings.i(), pidSettings.d());
+        PIDSettings pidSettings = this.context.getClimbControllerPIDSettings();
+
+        if (isSimulation()) {
+            //  Eliminate data-port dependency in sim
+            config.closedLoop
+                    .feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder)
+                    .pid(pidSettings.p(), pidSettings.i(), pidSettings.d());
+        } else {
+            // Use the alternate encoder in real
+            AlternateEncoderConfig altConfig = new AlternateEncoderConfig()
+                    .setSparkMaxDataPortConfig()
+                    .countsPerRevolution(context.getClimbAltEncoderCPR());
+
+            config.apply(altConfig);
+            config.closedLoop
+                    .feedbackSensor(ClosedLoopConfig.FeedbackSensor.kAlternateOrExternalEncoder)
+                    .pid(pidSettings.p(), pidSettings.i(), pidSettings.d());
+        }
         return config;
     }
 
@@ -152,8 +169,7 @@ private double getCurrentClimbPosition() {
     /**
      * {@link Sendable#initSendable(SendableBuilder)} implementation
      *
-     * @param builder
-     *            The sendable builder
+     * @param builder The sendable builder
      */
     @Override
     public void initSendable(SendableBuilder builder) {

src/main/java/frc/robot/subsystems/climb/ClimbMechanismContext.java

@@ -24,14 +24,14 @@ public static ClimbMechanismContext defaults() {
                 .climbMotor(
                         (Robot.isReal())
                                 ? new TeamSparkMaxImpl(
-                                        Constants.Port.climbMotorChannel, SparkLowLevel.MotorType.kBrushless)
+                                        Constants.Port.CLIMB_DRIVE_CHANNEL, SparkLowLevel.MotorType.kBrushless)
                                 : new TeamSparkMaxSimImpl(
-                                        Constants.Port.climbMotorChannel, SparkLowLevel.MotorType.kBrushless))
+                                        Constants.Port.CLIMB_DRIVE_CHANNEL, SparkLowLevel.MotorType.kBrushless))
                 .build();
     }
 
     @Builder.Default
-    private int climbMotorChannel = Constants.Port.climbMotorChannel;
+    private int climbMotorChannel = Constants.Port.CLIMB_DRIVE_CHANNEL;
 
     @Builder.Default
     private int climbMagSwitchChannel = Constants.Port.climbMagSwitchDIOC;
@@ -51,5 +51,11 @@ public static ClimbMechanismContext defaults() {
     @Builder.Default
     private double reverseIncrement = -.75;
 
+    @Builder.Default
+    // TODO: Look up the datasheet for the exact encoder you installed on the climb mechanism.
+    //  If we're not using a quadrature encoder at all, then don’t pick kAlternateOrExternalEncoder
+    //  Instead use the primary encoder or the appropriate sensor type.
+    private int climbAltEncoderCPR = (int) Constants.Conversion.MagEncoderCountsPerRev;
+
     private TeamSparkMax climbMotor;
 }

src/main/java/frc/robot/subsystems/coral/CoralMechanism.java

@@ -43,8 +43,8 @@ public CoralMechanism() {
     public CoralMechanism(final CoralMechanismContext context) {
         requireNonNull(context, "CoralMechanismContext cannot be null");
         this.context = context;
-        this.coralMotorRight = new TalonSRX(Constants.Port.coralMotorRightChannel);
-        this.coralMotorLeft = new TalonSRX(Constants.Port.coralMotorLeftChannel);
+        this.coralMotorRight = new TalonSRX(Constants.Port.RIGHT_CORAL_DRIVE_CHANNEL);
+        this.coralMotorLeft = new TalonSRX(Constants.Port.LEFT_CORAL_DRIVE_CHANNEL);
         this.rearSensor = new AnalogInput(this.context.getRearSensorChannel());
         this.frontSensor = new AnalogInput(this.context.getFrontSensorChannel());
     }