From d3bba568ce54a783214bfbfcc9852863e670b93c Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Thu, 2 Apr 2026 11:03:28 -0700 Subject: [PATCH 1/5] Add shift tracking --- src/main/java/frc/robot/Robot.java | 2 + src/main/java/frc/robot/RobotContainer.java | 12 + src/main/java/frc/robot/RobotState.java | 6 + .../java/frc/robot/util/HubShiftUtil.java | 231 ++++++++++++++++++ 4 files changed, 251 insertions(+) create mode 100644 src/main/java/frc/robot/util/HubShiftUtil.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a263161..d22db38 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,7 @@ package frc.robot; +import org.littletonrobotics.junction.AutoLogOutputManager; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedPowerDistribution; import org.littletonrobotics.junction.LoggedRobot; @@ -21,6 +22,7 @@ import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.util.HubShiftUtil; public class Robot extends LoggedRobot { private Command m_autonomousCommand; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de8b01b..18b8172 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,7 +10,9 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; +import java.lang.StackWalker.Option; 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)); diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 07acfe6..2b8fef2 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/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 poseSupplier) { */ public static void periodic() { LoggedTracer.reset(); + + // Calculations!!! Pose2d currentPose = pose.get(); @@ -66,6 +69,9 @@ 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"); } diff --git a/src/main/java/frc/robot/util/HubShiftUtil.java b/src/main/java/frc/robot/util/HubShiftUtil.java new file mode 100644 index 0000000..7fffd63 --- /dev/null +++ b/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> allianceWinOverride = () -> Optional.empty(); + + public static Optional getAllianceWinOverride() { + return allianceWinOverride.get(); + } + + public static void setAllianceWinOverride(Supplier> 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); + // // } + // } +} \ No newline at end of file From 098057197b39db9b268051eea02dc47f3f407089 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Thu, 2 Apr 2026 11:27:54 -0700 Subject: [PATCH 2/5] Nominal velocity when active --- src/main/java/frc/robot/RobotContainer.java | 6 +++++- .../frc/robot/subsystems/shooter/Shooter.java | 15 ++++++--------- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 18b8172..307b0a8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,7 @@ 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.lang.StackWalker.Option; import java.util.Arrays; @@ -303,7 +304,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. diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 6acbf1e..d49e82c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/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 velocity, Angle hoodAngle) { + return run(() -> setSetpoints(velocity.get(), hoodAngle)); + } + + public Command spinUpSetpointsCommand(Supplier velocity) { + return run(() -> setSetpoints(velocity.get(), ShooterConstants.hoodParkedAngle)); } /** From df601e13b6b20bd805be48ad8537cc3ac0d24acc Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Thu, 2 Apr 2026 18:42:49 -0700 Subject: [PATCH 3/5] Fix --- src/main/java/frc/robot/subsystems/shooter/Shooter.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index d49e82c..6c11ebc 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -331,7 +331,7 @@ public Command spinUpSetpointsCommand(Supplier velocity, Angle return run(() -> setSetpoints(velocity.get(), hoodAngle)); } - public Command spinUpSetpointsCommand(Supplier velocity) { + public Command spinUpVelocityCommand(Supplier velocity) { return run(() -> setSetpoints(velocity.get(), ShooterConstants.hoodParkedAngle)); } From 746240327c94286fb131b07227c3593c1768085b Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Thu, 2 Apr 2026 18:44:44 -0700 Subject: [PATCH 4/5] Cleanup warnings --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 1 - src/main/java/frc/robot/subsystems/drive/Drive.java | 4 ---- 3 files changed, 1 insertion(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d22db38..8110921 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -34,7 +34,7 @@ public Robot() { Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); - Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitDate", BuildOConstants.GIT_DATE); Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); Logger.recordMetadata("Robot", Constants.getRobot().toString()); Logger.recordMetadata("Mode", Constants.getMode().toString()); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 307b0a8..87bb2bd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,7 +11,6 @@ import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; -import java.lang.StackWalker.Option; import java.util.Arrays; import java.util.Optional; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 4e97b65..248440a 100755 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/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; From 547b3e6626a4b79ea835ec8192a0da618779375f Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Thu, 2 Apr 2026 18:49:30 -0700 Subject: [PATCH 5/5] Fix again sorry --- src/main/java/frc/robot/Robot.java | 4 +--- src/main/java/frc/robot/RobotState.java | 3 ++- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8110921..a263161 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,7 +4,6 @@ package frc.robot; -import org.littletonrobotics.junction.AutoLogOutputManager; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedPowerDistribution; import org.littletonrobotics.junction.LoggedRobot; @@ -22,7 +21,6 @@ import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.util.HubShiftUtil; public class Robot extends LoggedRobot { private Command m_autonomousCommand; @@ -34,7 +32,7 @@ public Robot() { Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); - Logger.recordMetadata("GitDate", BuildOConstants.GIT_DATE); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); Logger.recordMetadata("Robot", Constants.getRobot().toString()); Logger.recordMetadata("Mode", Constants.getMode().toString()); diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 2b8fef2..3aa69f8 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -44,7 +44,7 @@ public static void initialize(Supplier poseSupplier) { public static void periodic() { LoggedTracer.reset(); - // Calculations!!! + // Calculations Pose2d currentPose = pose.get(); @@ -70,6 +70,7 @@ 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");