Add shift tracking

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

@@ -9,8 +9,10 @@
 import static edu.wpi.first.units.Units.Meters;
 import static edu.wpi.first.units.Units.MetersPerSecond;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
 
 import java.util.Arrays;
+import java.util.Optional;
 
 import org.ironmaple.simulation.drivesims.COTS;
 import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig;
@@ -22,6 +24,7 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -63,6 +66,7 @@
 import frc.robot.subsystems.vision.CameraIOLimelight4;
 import frc.robot.subsystems.vision.CameraIOPhotonSim;
 import frc.robot.subsystems.vision.VisionConstants;
+import frc.robot.util.HubShiftUtil;
 import frc.robot.util.MapleSimUtil;
 import frc.robot.util.Mechanism3d;
 
@@ -264,6 +268,14 @@ public RobotContainer() {
         testBindings_.addOption("Hood Calibration", shooter_.hoodCalibration());
         testBindings_.addOption("Startup Sequence Test", new StartupCmd(intake_, hopper_, shooter_)) ;
 
+        // Auto Win Override
+        LoggedNetworkBoolean winAutoPractice = new LoggedNetworkBoolean("Win Auto (Practice)", true);
+
+        HubShiftUtil.setAllianceWinOverride(() -> {
+            if (DriverStation.isFMSAttached()) return Optional.empty();
+            return Optional.of(winAutoPractice.get());
+        });
+
         // Sets the selected test binding to be triggered when the A button is pressed in test mode.
         RobotModeTriggers.test().and(gamepad_.a()).toggleOnTrue(Commands.deferredProxy(testBindings_::get));
 
@@ -291,7 +303,10 @@ private void configureBindings() {
             .whileTrue(RobotCommands.shoot(shooter_, hopper_, intake_, drivebase_));
 
         // When the shooter isnt shooting, get it ready to shoot.
-        shooter_.setDefaultCommand(shooter_.idleCommand());
+        shooter_.setDefaultCommand(shooter_.spinUpVelocityCommand(() -> {
+            var shift = HubShiftUtil.getOfficialShiftInfo();
+            return shift.active() ? RotationsPerSecond.of(50) : RotationsPerSecond.zero();
+        }));
 
         //While the X button is held, the intake will run the eject sequence. If it the intake is stowed, it will also deploy it.
 

src/main/java/frc/robot/RobotState.java

@@ -14,6 +14,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import frc.robot.subsystems.shooter.ShooterConstants;
+import frc.robot.util.HubShiftUtil;
 import frc.robot.util.LoggedTracer;
 
 /**
@@ -42,6 +43,8 @@ public static void initialize(Supplier<Pose2d> poseSupplier) {
      */
     public static void periodic() {
         LoggedTracer.reset();
+
+        // Calculations
 
         Pose2d currentPose = pose.get();
 
@@ -66,6 +69,10 @@ public static void periodic() {
 
         inAllianceZone = currentPose.getMeasureX().minus(allianceWall).abs(Meters) < ShooterConstants.Positions.spinUpZone.in(Meters);
 
+        // Other logging
+
+        Logger.recordOutput("ShiftInfo", HubShiftUtil.getOfficialShiftInfo());
+
         LoggedTracer.record("RobotState");
     }
 

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -15,9 +15,7 @@
 
 import static edu.wpi.first.units.Units.Degrees;
 import static edu.wpi.first.units.Units.MetersPerSecond;
-import static edu.wpi.first.units.Units.Radians;
 import static edu.wpi.first.units.Units.Volts;
-import static edu.wpi.first.units.Units.derive;
 
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
@@ -54,7 +52,6 @@
 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.units.AngleUnit;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.LinearVelocity;
@@ -63,7 +60,6 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;

src/main/java/frc/robot/subsystems/shooter/Shooter.java

@@ -327,15 +327,12 @@ public Command ejectUp() {
         return startEnd(() -> setShooterVelocity(ShooterConstants.ejectVelocity), this::stopShooter);
     }
 
-    /**
-     * Idles the shooter at 0 velocity and the hood at the parked position.
-     * @return
-     */
-    public Command idleCommand() {
-        return runToSetpointsCmd(
-            RotationsPerSecond.of(0.0),
-            ShooterConstants.hoodParkedAngle
-        );
+    public Command spinUpSetpointsCommand(Supplier<AngularVelocity> velocity, Angle hoodAngle) {
+        return run(() -> setSetpoints(velocity.get(), hoodAngle));
+    }
+
+    public Command spinUpVelocityCommand(Supplier<AngularVelocity> velocity) {
+        return run(() -> setSetpoints(velocity.get(), ShooterConstants.hoodParkedAngle));
     }
 
     /**

src/main/java/frc/robot/util/HubShiftUtil.java

@@ -0,0 +1,231 @@
+// Copyright (c) 2025-2026 Littleton Robotics
+// http://github.com/Mechanical-Advantage
+//
+
+// MIT License
+
+// Copyright (c) 2025-2026 Littleton Robotics
+
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+
+package frc.robot.util;
+
+import java.util.Optional;
+import java.util.function.Supplier;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.Timer;
+
+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;
+
+  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 = 1.0;
+  private static double shiftTimerOffset = 0.0;
+  private static Supplier<Optional<Boolean>> allianceWinOverride = () -> Optional.empty();
+
+  public static Optional<Boolean> getAllianceWinOverride() {
+    return allianceWinOverride.get();
+  }
+
+  public static void setAllianceWinOverride(Supplier<Optional<Boolean>> win) {
+    allianceWinOverride = win;
+  }
+
+  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;
+  }
+
+  @AutoLogOutput
+  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;
+
+        shiftTimerOffset = timerValue - fieldTeleopTime;
+        currentTime = fieldTeleopTime;
+      }
+      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);
+  //   // }
+  // }
+}