Fix turret setpoint wraparound for asymmetric operating ranges

src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java

@@ -76,10 +76,27 @@ public static final class TurretConstants {
     // Geometry
     public static final Transform3d CHASSIS_TO_TURRET_BASE =
         new Transform3d(Inches.of(-4.000), Inches.of(6.500), Inches.of(16.331), Rotation3d.kZero);
-    public static final Rotation2d ABS_ENCODER_OFFSET = new Rotation2d(5.157);
-    public static final Rotation2d MECHANISM_OFFSET = Rotation2d.kZero;
-    public static final double UPPER_LIMIT_RAD = Units.degreesToRadians(270);
-    public static final double LOWER_LIMIT_RAD = Units.degreesToRadians(45);
+    // Turret angle convention: 0 / 2*pi rad = forward, increasing CCW (viewed from above),
+    // matching standard Rotation2d/WPILib handedness. All constants below are w/r/t forward.
+
+    // Calibration value chosen so the absolute encoder reads 0 / 2*pi when the turret is
+    // physically facing forward. To reclock after a mechanical rebuild or encoder reseat: point
+    // the turret forward by hand, read the raw absolute encoder value (with this offset backed
+    // out, i.e. temporarily zeroed), and set ABS_ENCODER_OFFSET to that raw reading.
+    public static final Rotation2d ABS_ENCODER_OFFSET = new Rotation2d(5.157 + 1.267);
+
+    // Soft limits of the mechanical range, measured CCW from forward (e.g. LOWER=-280 means the
+    // turret can travel 280 deg clockwise of forward). UPPER_LIMIT_RAD - LOWER_LIMIT_RAD must
+    // stay under 360 deg, or the wrap math below (CENTER_RAD, and its use in TurretIOSpark /
+    // TurretIOSimSpark) can no longer place every reachable angle in a single unambiguous branch.
+    public static final double UPPER_LIMIT_RAD = Units.degreesToRadians(30);
+    public static final double LOWER_LIMIT_RAD = Units.degreesToRadians(-280);
+
+    // Center of the operating range. Used to pick the modulus window ([CENTER-pi, CENTER+pi))
+    // that setpoints and the relative-encoder seed get wrapped into, so the branch cut always
+    // falls in the unreachable gap directly opposite the range rather than inside it. Derived
+    // automatically from the limits above - do not hand-edit when reclocking.
+    public static final double CENTER_RAD = (LOWER_LIMIT_RAD + UPPER_LIMIT_RAD) / 2.0;
     public static final double MARGIN_RAD = Units.degreesToRadians(5);
 
     // Position controller

src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java

@@ -78,7 +78,9 @@ public TurretIOSimSpark() {
             LinearSystemId.createDCMotorSystem(GEARBOX, TURRET_MOI_KG_M2, MOTOR_REDUCTION),
             GEARBOX);
 
-    turnSim.setState(2.0 * Math.PI - MECHANISM_OFFSET.getRadians(), 0);
+    double seedPosition =
+        MathUtil.inputModulus(2.0 * Math.PI, CENTER_RAD - Math.PI, CENTER_RAD + Math.PI);
+    turnSim.setState(seedPosition, 0);
     turnSparkSim.setPosition(turnSim.getAngularPositionRad());
   }
 
@@ -93,13 +95,13 @@ public void updateInputs(TurretIOInputs inputs) {
 
     // Update inputs
     inputs.motorControllerConnected = true;
-    inputs.relativePosition = new Rotation2d(turnSparkSim.getPosition()).plus(MECHANISM_OFFSET);
+    inputs.relativePosition = new Rotation2d(turnSparkSim.getPosition());
     inputs.velocityRadPerSec = turnSparkSim.getVelocity();
     inputs.appliedVolts = turnSparkSim.getAppliedOutput() * turnSparkSim.getBusVoltage();
     inputs.currentAmps = Math.abs(turnSparkSim.getMotorCurrent());
 
     inputs.absoluteEncoderConnected = true;
-    inputs.absolutePosition = new Rotation2d(turnSparkSim.getPosition()).plus(MECHANISM_OFFSET);
+    inputs.absolutePosition = new Rotation2d(turnSparkSim.getPosition());
 
     inputs.oversaturation = oversaturation;
     inputs.oversaturationLessMargin = oversaturationLessMargin;
@@ -115,8 +117,7 @@ public void setOpenLoop(Voltage volts) {
   @Override
   public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) {
     double setpoint =
-        MathUtil.inputModulus(
-            rotation.getRadians() - MECHANISM_OFFSET.getRadians(), 0.0, 2.0 * Math.PI);
+        MathUtil.inputModulus(rotation.getRadians(), CENTER_RAD - Math.PI, CENTER_RAD + Math.PI);
     double clampedSetpoint = MathUtil.clamp(setpoint, LOWER_LIMIT_RAD, UPPER_LIMIT_RAD);
     double clampedSetpointWithMargin =
         MathUtil.clamp(setpoint, LOWER_LIMIT_RAD + MARGIN_RAD, UPPER_LIMIT_RAD - MARGIN_RAD);

src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java

@@ -54,7 +54,7 @@ public TurretIOSpark() {
         new DutyCycleEncoder(
             new DigitalInput(DIOPorts.TURRET_ABS_ENCODER),
             2 * Math.PI,
-            ABS_ENCODER_OFFSET.getRadians() + MECHANISM_OFFSET.getRadians());
+            ABS_ENCODER_OFFSET.getRadians());
 
     var turnConfig = new SparkMaxConfig();
 
@@ -98,12 +98,14 @@ public TurretIOSpark() {
   @Override
   public void updateInputs(TurretIOInputs inputs) {
     if (!relativeEncoderSeeded && inputs.absoluteEncoderConnected) {
-      turnSparkEncoder.setPosition(absoluteEncoder.get());
+      double seedPosition =
+          MathUtil.inputModulus(absoluteEncoder.get(), CENTER_RAD - Math.PI, CENTER_RAD + Math.PI);
+      turnSparkEncoder.setPosition(seedPosition);
       relativeEncoderSeeded = true;
     }
 
     // Read from cached values (non-blocking) - updated by SparkOdometryThread
-    inputs.relativePosition = new Rotation2d(sparkInputs.getPosition()).plus(MECHANISM_OFFSET);
+    inputs.relativePosition = new Rotation2d(sparkInputs.getPosition());
     inputs.velocityRadPerSec = sparkInputs.getVelocity();
     inputs.appliedVolts = sparkInputs.getAppliedVolts();
     inputs.currentAmps = sparkInputs.getOutputCurrent();
@@ -129,8 +131,7 @@ public void setOpenLoop(Voltage volts) {
   @Override
   public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) {
     double setpoint =
-        MathUtil.inputModulus(
-            rotation.getRadians() - MECHANISM_OFFSET.getRadians(), 0.0, 2 * Math.PI);
+        MathUtil.inputModulus(rotation.getRadians(), CENTER_RAD - Math.PI, CENTER_RAD + Math.PI);
     double clampedSetpoint = MathUtil.clamp(setpoint, LOWER_LIMIT_RAD, UPPER_LIMIT_RAD);
     double clampedSetpointWithMargin =
         MathUtil.clamp(setpoint, LOWER_LIMIT_RAD + MARGIN_RAD, UPPER_LIMIT_RAD - MARGIN_RAD);