diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de8b01b..87bb2bd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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. diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 07acfe6..3aa69f8 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,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"); } 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; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 6acbf1e..6c11ebc 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 spinUpVelocityCommand(Supplier velocity) { + return run(() -> setSetpoints(velocity.get(), ShooterConstants.hoodParkedAngle)); } /** 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