sim working and merged with main, but constants are sus

simgui-ds.json

@@ -1,9 +1,4 @@
 {
-  "System Joysticks": {
-    "window": {
-      "enabled": false
-    }
-  },
   "keyboardJoysticks": [
     {
       "axisConfig": [
@@ -93,5 +88,11 @@
       "buttonCount": 0,
       "povCount": 0
     }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
+    }
   ]
 }

src/main/java/frc/lib/Rebuilt2026/HubShiftUtil.java

@@ -0,0 +1,206 @@
+package frc.lib.Rebuilt2026;
+
+// Copyright (c) 2025-2026 Littleton Robotics
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.Timer;
+import java.util.Optional;
+import java.util.function.Supplier;
+import lombok.Setter;
+
+public class HubShiftUtil {
+  public enum ShiftEnum {
+    TRANSITION,
+    SHIFT1,
+    SHIFT2,
+    SHIFT3,
+    SHIFT4,
+    ENDGAME,
+    AUTO,
+    DISABLED;
+  }
+
+  public record ShiftInfo(
+      ShiftEnum currentShift, double elapsedTime, double remainingTime, boolean active) {}
+
+  private static Timer shiftTimer = new Timer();
+  private static final ShiftEnum[] shiftsEnums = ShiftEnum.values();
+
+  private static final double[] shiftStartTimes = {0.0, 10.0, 35.0, 60.0, 85.0, 110.0};
+  private static final double[] shiftEndTimes = {10.0, 35.0, 60.0, 85.0, 110.0, 140.0};
+
+  private static final double minFuelCountDelay = 1.0;
+  private static final double maxFuelCountDelay = 2.0;
+  private static final double shiftEndFuelCountExtension = 3.0;
+  private static final double minTimeOfFlight = 0;
+  private static final double maxTimeOfFlight = 0;
+  private static final double approachingActiveFudge = -1 * (minTimeOfFlight + minFuelCountDelay);
+  private static final double endingActiveFudge =
+      shiftEndFuelCountExtension + -1 * (maxTimeOfFlight + maxFuelCountDelay);
+
+  public static final double autoEndTime = 20.0;
+  public static final double teleopDuration = 140.0;
+  private static final boolean[] activeSchedule = {true, true, false, true, false, true};
+  private static final boolean[] inactiveSchedule = {true, false, true, false, true, true};
+  private static final double timeResetThreshold = 3.0;
+  private static double shiftTimerOffset = 0.0;
+  @Setter private static Supplier<Optional<Boolean>> allianceWinOverride = () -> Optional.empty();
+
+  public static Optional<Boolean> getAllianceWinOverride() {
+    return allianceWinOverride.get();
+  }
+
+  public static Alliance getFirstActiveAlliance() {
+    var alliance = DriverStation.getAlliance().orElse(Alliance.Blue);
+
+    // Return override value
+    var winOverride = getAllianceWinOverride();
+    if (!winOverride.isEmpty()) {
+      return winOverride.get()
+          ? (alliance == Alliance.Blue ? Alliance.Red : Alliance.Blue)
+          : (alliance == Alliance.Blue ? Alliance.Blue : Alliance.Red);
+    }
+
+    // Return FMS value
+    String message = DriverStation.getGameSpecificMessage();
+    if (message.length() > 0) {
+      char character = message.charAt(0);
+      if (character == 'R') {
+        return Alliance.Blue;
+      } else if (character == 'B') {
+        return Alliance.Red;
+      }
+    }
+
+    // Return default value
+    return alliance == Alliance.Blue ? Alliance.Red : Alliance.Blue;
+  }
+
+  /** Starts the timer at the begining of teleop. */
+  public static void initialize() {
+    shiftTimerOffset = 0;
+    shiftTimer.restart();
+  }
+
+  private static boolean[] getSchedule() {
+    boolean[] currentSchedule;
+    Alliance startAlliance = getFirstActiveAlliance();
+    currentSchedule =
+        startAlliance == DriverStation.getAlliance().orElse(Alliance.Blue)
+            ? activeSchedule
+            : inactiveSchedule;
+    return currentSchedule;
+  }
+
+  private static ShiftInfo getShiftInfo(
+      boolean[] currentSchedule, double[] shiftStartTimes, double[] shiftEndTimes) {
+    double timerValue = shiftTimer.get();
+    double currentTime = timerValue - shiftTimerOffset;
+    double stateTimeElapsed = currentTime;
+    double stateTimeRemaining = 0.0;
+    boolean active = false;
+    ShiftEnum currentShift = ShiftEnum.DISABLED;
+    double fieldTeleopTime = 140.0 - DriverStation.getMatchTime();
+
+    if (DriverStation.isAutonomousEnabled()) {
+      stateTimeElapsed = currentTime;
+      stateTimeRemaining = autoEndTime - currentTime;
+      active = true;
+      currentShift = ShiftEnum.AUTO;
+    } else if (DriverStation.isEnabled()) {
+      // Adjust the current offset if the time difference above the theshold
+      if (Math.abs(fieldTeleopTime - currentTime) >= timeResetThreshold
+          && fieldTeleopTime <= 135
+          && DriverStation.isFMSAttached()) {
+        shiftTimerOffset += currentTime - fieldTeleopTime;
+        currentTime = timerValue + shiftTimerOffset;
+      }
+      int currentShiftIndex = -1;
+      for (int i = 0; i < shiftStartTimes.length; i++) {
+        if (currentTime >= shiftStartTimes[i] && currentTime < shiftEndTimes[i]) {
+          currentShiftIndex = i;
+          break;
+        }
+      }
+      if (currentShiftIndex < 0) {
+        // After last shift, so assume endgame
+        currentShiftIndex = shiftStartTimes.length - 1;
+      }
+
+      // Calculate elapsed and remaining time in the current shift, ignoring combined shifts
+      stateTimeElapsed = currentTime - shiftStartTimes[currentShiftIndex];
+      stateTimeRemaining = shiftEndTimes[currentShiftIndex] - currentTime;
+
+      // If the state is the same as the last shift, combine the elapsed time
+      if (currentShiftIndex > 0) {
+        if (currentSchedule[currentShiftIndex] == currentSchedule[currentShiftIndex - 1]) {
+          stateTimeElapsed = currentTime - shiftStartTimes[currentShiftIndex - 1];
+        }
+      }
+
+      // If the state is the same as the next shift, combine the remaining time
+      if (currentShiftIndex < shiftEndTimes.length - 1) {
+        if (currentSchedule[currentShiftIndex] == currentSchedule[currentShiftIndex + 1]) {
+          stateTimeRemaining = shiftEndTimes[currentShiftIndex + 1] - currentTime;
+        }
+      }
+
+      active = currentSchedule[currentShiftIndex];
+      currentShift = shiftsEnums[currentShiftIndex];
+    }
+    ShiftInfo shiftInfo = new ShiftInfo(currentShift, stateTimeElapsed, stateTimeRemaining, active);
+    return shiftInfo;
+  }
+
+  public static ShiftInfo getOfficialShiftInfo() {
+    return getShiftInfo(getSchedule(), shiftStartTimes, shiftEndTimes);
+  }
+
+  public static ShiftInfo getShiftedShiftInfo() {
+    boolean[] shiftSchedule = getSchedule();
+    // Starting active
+    if (shiftSchedule[1] == true) {
+      double[] shiftedShiftStartTimes = {
+        0.0,
+        10.0,
+        35.0 + endingActiveFudge,
+        60.0 + approachingActiveFudge,
+        85.0 + endingActiveFudge,
+        110.0 + approachingActiveFudge
+      };
+      double[] shiftedShiftEndTimes = {
+        10.0,
+        35.0 + endingActiveFudge,
+        60.0 + approachingActiveFudge,
+        85.0 + endingActiveFudge,
+        110.0 + approachingActiveFudge,
+        140.0
+      };
+      return getShiftInfo(shiftSchedule, shiftedShiftStartTimes, shiftedShiftEndTimes);
+    }
+    double[] shiftedShiftStartTimes = {
+      0.0,
+      10.0 + endingActiveFudge,
+      35.0 + approachingActiveFudge,
+      60.0 + endingActiveFudge,
+      85.0 + approachingActiveFudge,
+      110.0
+    };
+    double[] shiftedShiftEndTimes = {
+      10.0 + endingActiveFudge,
+      35.0 + approachingActiveFudge,
+      60.0 + endingActiveFudge,
+      85.0 + approachingActiveFudge,
+      110.0,
+      140.0
+    };
+    return getShiftInfo(shiftSchedule, shiftedShiftStartTimes, shiftedShiftEndTimes);
+    // }
+  }
+}

src/main/java/frc/lib/W8/io/motor/MotorIOTalonFXSim.java

@@ -17,26 +17,51 @@
 
 import static edu.wpi.first.units.Units.Rotations;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
+import static edu.wpi.first.units.Units.Volts;
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.sim.TalonFXSimState;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.units.AngularAccelerationUnit;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularAcceleration;
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Velocity;
 import edu.wpi.first.wpilibj.RobotController;
 import frc.lib.W8.util.Device.CAN;
 
 /**
  * Abstraction for a simulated CTRE TalonFX motor implementing the {@link MotorIOSim} interface.
  * Wraps motor setup, control modes, telemetry polling, and error handling.
+ *
+ * <p>Because Phoenix TalonFX simulation does not simulate closed-loop PID control, this class
+ * implements manual PID controllers for position and velocity modes. When runPosition() or
+ * runVelocity() are called, the appropriate PID controller is enabled and its output voltage is
+ * applied to the simulated motor via setInputVoltage().
  */
 public class MotorIOTalonFXSim extends MotorIOTalonFX implements MotorIOSim {
 
+  // Simulation configuration
+  private static final double POSITION_KP = 50.0; // Position control proportional gain
+  private static final double POSITION_KD = 5.0; // Position control derivative gain
+  private static final double VELOCITY_KP = 0.1; // Velocity control proportional gain
+  private static final double VELOCITY_KD = 0.0; // Velocity control derivative gain
+
   private double rotorToSensorRatio;
   private double sensorToMechanismRatio;
   private TalonFXSimState simState;
 
+  // Manual PID control for simulation
+  private final PIDController positionController = new PIDController(POSITION_KP, 0, POSITION_KD);
+  private final PIDController velocityController = new PIDController(VELOCITY_KP, 0, VELOCITY_KD);
+
+  // Track which control mode is active for manual PID
+  private boolean positionClosedLoop = false;
+  private boolean velocityClosedLoop = false;
+  private double appliedVoltage = 0.0;
+
   /**
    * Constructs and initializes a TalonFX motor simulation.
    *
@@ -84,8 +109,68 @@ public void setEncoderPosition(Angle position) {
     super.setEncoderPosition(position.times(rotorToSensorRatio * sensorToMechanismRatio));
   }
 
+  /**
+   * Runs the motor to a target position using manual PID control (since Phoenix sim doesn't
+   * simulate closed-loop control).
+   */
+  @Override
+  public void runPosition(
+      Angle position,
+      AngularVelocity cruiseVelocity,
+      AngularAcceleration acceleration,
+      Velocity<AngularAccelerationUnit> maxJerk,
+      PIDSlot slot) {
+    this.goalPosition = position;
+    double newSetpoint = position.in(Rotations);
+
+    // Always reset PID controller when a new position command is issued
+    // This ensures fresh control for each new setpoint
+    positionController.reset();
+
+    positionClosedLoop = true;
+    velocityClosedLoop = false;
+    positionController.setSetpoint(newSetpoint);
+  }
+
+  /**
+   * Runs the motor at a target velocity using manual PID control (since Phoenix sim doesn't
+   * simulate closed-loop control).
+   */
+  @Override
+  public void runVelocity(
+      AngularVelocity velocity, AngularAcceleration acceleration, PIDSlot slot) {
+    double newSetpoint = velocity.in(RotationsPerSecond);
+
+    // Always reset PID controller when a new velocity command is issued
+    // This ensures fresh control for each new setpoint
+    velocityController.reset();
+
+    velocityClosedLoop = true;
+    positionClosedLoop = false;
+    velocityController.setSetpoint(newSetpoint);
+  }
+
   @Override
   public void updateInputs(MotorInputs inputs) {
+    // Run manual PID control for closed-loop modes since Phoenix sim doesn't simulate firmware
+    if (positionClosedLoop) {
+      double currentPosition = super.position.getValue().in(Rotations);
+      double pidOutput = positionController.calculate(currentPosition);
+      appliedVoltage = MathUtil.clamp(pidOutput, -12.0, 12.0);
+      // Apply calculated voltage via VoltageOut control
+      super.motor.setControl(super.voltageControl.withOutput(appliedVoltage));
+    } else if (velocityClosedLoop) {
+      double currentVelocity = super.velocity.getValue().in(RotationsPerSecond);
+      double pidOutput = velocityController.calculate(currentVelocity);
+      appliedVoltage = MathUtil.clamp(pidOutput, -12.0, 12.0);
+      // Apply calculated voltage via VoltageOut control
+      super.motor.setControl(super.voltageControl.withOutput(appliedVoltage));
+    } else {
+      appliedVoltage = 0.0;
+      positionController.reset();
+      velocityController.reset();
+    }
+
     inputs.connected =
         BaseStatusSignal.refreshAll(
                 super.position,
@@ -103,7 +188,8 @@ public void updateInputs(MotorInputs inputs) {
 
     inputs.position = super.position.getValue();
     inputs.velocity = super.velocity.getValue();
-    inputs.appliedVoltage = simState.getMotorVoltageMeasure();
+    // Use the voltage we calculated, not what the simState reports (which may not have updated yet)
+    inputs.appliedVoltage = Volts.of(appliedVoltage);
     inputs.supplyCurrent = simState.getSupplyCurrentMeasure();
     inputs.torqueCurrent = simState.getTorqueCurrentMeasure();
     inputs.temperature = super.temperature.getValue();

src/main/java/frc/lib/W8/mechanisms/rotary/RotaryMechanismSim.java

@@ -76,6 +76,7 @@ public RotaryMechanismSim(
   @Override
   public void periodic() {
     super.periodic();
+    io.updateInputs(inputs);
 
     Time currentTime = Seconds.of(Timer.getTimestamp());
     double deltaTime = currentTime.minus(lastTime).in(Seconds);
@@ -103,7 +104,6 @@ public void periodic() {
           Logger.processInputs(encoderSim.getName(), absoluteEncoderInputs);
         });
 
-    io.updateInputs(inputs);
     Logger.processInputs(io.getName(), inputs);
   }