From dcc226aca4b641daee4d857a3b9fe56ad7002152 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Wed, 11 Mar 2026 19:26:21 +0000 Subject: [PATCH 1/3] Initial plan From ed6c04c28b8ba1c32e59207e4c779243bb86041e Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Wed, 11 Mar 2026 19:31:52 +0000 Subject: [PATCH 2/3] Add alliance zone return warning Co-authored-by: GrumpyBud <109627078+GrumpyBud@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 33 +++++++++++ .../java/frc/robot/util/HubShiftUtil.java | 55 +++++++++++++++++++ 2 files changed, 88 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6a60369..38ff29e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -58,6 +59,7 @@ import frc.robot.util.LoggedTunableNumber; import frc.robot.util.TriggerUtil; import lombok.experimental.ExtensionMethod; +import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -97,6 +99,11 @@ public class RobotContainer { private static final LoggedTunableNumber manualHoodAngle = new LoggedTunableNumber("Shooter/ManualHoodAngleDeg", 0.1); private static final double flywheelSpeedOffsetPercentStep = 0.05; + private static final String returnWarningKey = "HubShift/ReturnToAllianceZoneWarning"; + private static final String estimatedReturnTimeKey = "HubShift/EstimatedReturnTimeSec"; + private static final String hubTimeRemainingKey = "HubShift/TimeUntilHubActiveSec"; + private static final double returnWarningRumbleStrength = 1.0; + private static final double returnWarningRumblePulsePeriodSec = 0.5; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -376,6 +383,32 @@ public void updateDashboardOutputs() { // Publish match time SmartDashboard.putNumber("Match Time", DriverStation.getMatchTime()); + var shiftInfo = HubShiftUtil.getShiftedShiftInfo(); + var robotPose = drive.getPose(); + double maxDriveSpeedMetersPerSec = drive.getMaxLinearSpeedMetersPerSec(); + double estimatedReturnTime = + HubShiftUtil.getEstimatedReturnToAllianceZoneTime(robotPose, maxDriveSpeedMetersPerSec); + boolean returnWarningActive = + HubShiftUtil.shouldWarnReturnToAllianceZone(robotPose, maxDriveSpeedMetersPerSec); + double timeUntilHubActive = shiftInfo.active() ? 0.0 : shiftInfo.remainingTime(); + + SmartDashboard.putBoolean(returnWarningKey, returnWarningActive); + SmartDashboard.putNumber(estimatedReturnTimeKey, estimatedReturnTime); + SmartDashboard.putNumber(hubTimeRemainingKey, timeUntilHubActive); + Logger.recordOutput(returnWarningKey, returnWarningActive); + Logger.recordOutput(estimatedReturnTimeKey, estimatedReturnTime); + Logger.recordOutput(hubTimeRemainingKey, timeUntilHubActive); + + boolean rumbleEnabled = + returnWarningActive + && (Timer.getTimestamp() % returnWarningRumblePulsePeriodSec) + < (returnWarningRumblePulsePeriodSec / 2.0); + controller + .getHID() + .setRumble( + GenericHID.RumbleType.kBothRumble, + rumbleEnabled ? returnWarningRumbleStrength : 0.0); + // Controller disconnected alerts controllerDisconnected.set(!DriverStation.isJoystickConnected(controller.getHID().getPort())); } diff --git a/src/main/java/frc/robot/util/HubShiftUtil.java b/src/main/java/frc/robot/util/HubShiftUtil.java index 70cd9c1..bc0796b 100644 --- a/src/main/java/frc/robot/util/HubShiftUtil.java +++ b/src/main/java/frc/robot/util/HubShiftUtil.java @@ -3,9 +3,12 @@ package frc.robot.util; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; +import frc.robot.FieldConstants; import frc.robot.subsystems.shooter.ShotCalculator; import java.util.Optional; import java.util.function.Supplier; @@ -40,6 +43,21 @@ public record ShiftInfo( private static final double approachingActiveFudge = -1 * (minTimeOfFlight + minFuelCountDelay); private static final double endingActiveFudge = shiftEndFuelCountExtension + -1 * (maxTimeOfFlight + maxFuelCountDelay); + private static final double returnDriveSpeedFraction = 0.6; + private static final double returnDriveFudgeSeconds = 1.0; + private static final double minimumReturnDriveSpeedMetersPerSec = 0.1; + private static final Translation2d[] returnTargets = { + new Translation2d( + FieldConstants.LinesVertical.allianceZone, + (FieldConstants.LinesHorizontal.rightTrenchOpenStart + + FieldConstants.LinesHorizontal.rightTrenchOpenEnd) + / 2.0), + new Translation2d( + FieldConstants.LinesVertical.allianceZone, + (FieldConstants.LinesHorizontal.leftTrenchOpenStart + + FieldConstants.LinesHorizontal.leftTrenchOpenEnd) + / 2.0) + }; public static final double autoEndTime = 20.0; public static final double teleopDuration = 140.0; @@ -189,4 +207,41 @@ public static ShiftInfo getShiftedShiftInfo() { return getShiftInfo(shiftSchedule, shiftedShiftStartTimes, shiftedShiftEndTimes); // } } + + public static double getEstimatedReturnToAllianceZoneTime( + Pose2d robotPose, double maxDriveSpeedMetersPerSec) { + Pose2d allianceRelativePose = AllianceFlipUtil.apply(robotPose); + if (allianceRelativePose.getX() <= FieldConstants.LinesVertical.allianceZone) { + return 0.0; + } + + double shortestReturnDistance = Double.POSITIVE_INFINITY; + for (Translation2d returnTarget : returnTargets) { + double returnDistance = + Math.abs(allianceRelativePose.getY() - returnTarget.getY()) + + Math.max(0.0, allianceRelativePose.getX() - returnTarget.getX()); + shortestReturnDistance = Math.min(shortestReturnDistance, returnDistance); + } + + double returnDriveSpeedMetersPerSec = + Math.max( + minimumReturnDriveSpeedMetersPerSec, + maxDriveSpeedMetersPerSec * returnDriveSpeedFraction); + return shortestReturnDistance / returnDriveSpeedMetersPerSec + returnDriveFudgeSeconds; + } + + public static boolean shouldWarnReturnToAllianceZone( + Pose2d robotPose, double maxDriveSpeedMetersPerSec) { + if (!DriverStation.isTeleopEnabled()) { + return false; + } + + ShiftInfo shiftInfo = getShiftedShiftInfo(); + if (shiftInfo.active()) { + return false; + } + + return shiftInfo.remainingTime() + <= getEstimatedReturnToAllianceZoneTime(robotPose, maxDriveSpeedMetersPerSec); + } } From b0f984e4d4a981e2ae933f5b9f76267e6cad9818 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Wed, 11 Mar 2026 19:35:53 +0000 Subject: [PATCH 3/3] Polish return warning implementation Co-authored-by: GrumpyBud <109627078+GrumpyBud@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 8 +++++--- src/main/java/frc/robot/util/HubShiftUtil.java | 8 ++++++++ 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 38ff29e..6c4d606 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -104,6 +104,7 @@ public class RobotContainer { private static final String hubTimeRemainingKey = "HubShift/TimeUntilHubActiveSec"; private static final double returnWarningRumbleStrength = 1.0; private static final double returnWarningRumblePulsePeriodSec = 0.5; + private static final long returnWarningRumblePulseCyclePhases = 2; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -399,10 +400,11 @@ public void updateDashboardOutputs() { Logger.recordOutput(estimatedReturnTimeKey, estimatedReturnTime); Logger.recordOutput(hubTimeRemainingKey, timeUntilHubActive); + long currentPulsePhase = + (long) (Timer.getFPGATimestamp() / returnWarningRumblePulsePeriodSec); + // Create a simple 0.5s on / 0.5s off buzz once it is time to head back. boolean rumbleEnabled = - returnWarningActive - && (Timer.getTimestamp() % returnWarningRumblePulsePeriodSec) - < (returnWarningRumblePulsePeriodSec / 2.0); + returnWarningActive && currentPulsePhase % returnWarningRumblePulseCyclePhases == 0; controller .getHID() .setRumble( diff --git a/src/main/java/frc/robot/util/HubShiftUtil.java b/src/main/java/frc/robot/util/HubShiftUtil.java index bc0796b..2951188 100644 --- a/src/main/java/frc/robot/util/HubShiftUtil.java +++ b/src/main/java/frc/robot/util/HubShiftUtil.java @@ -43,9 +43,15 @@ public record ShiftInfo( private static final double approachingActiveFudge = -1 * (minTimeOfFlight + minFuelCountDelay); private static final double endingActiveFudge = shiftEndFuelCountExtension + -1 * (maxTimeOfFlight + maxFuelCountDelay); + // Conservative estimate: drivers generally cannot hold top speed while manually routing back + // around the hub, so plan around roughly 60% of max speed plus extra slack. private static final double returnDriveSpeedFraction = 0.6; + // Extra slack for imperfect driving lines and the final settle-in before reaching the scoring + // side of the alliance zone. private static final double returnDriveFudgeSeconds = 1.0; private static final double minimumReturnDriveSpeedMetersPerSec = 0.1; + // Use the midpoint of the open trench-side lanes so the estimate does not assume the robot can + // pass through the hub or bump geometry. private static final Translation2d[] returnTargets = { new Translation2d( FieldConstants.LinesVertical.allianceZone, @@ -217,6 +223,8 @@ public static double getEstimatedReturnToAllianceZoneTime( double shortestReturnDistance = Double.POSITIVE_INFINITY; for (Translation2d returnTarget : returnTargets) { + // Approximate the manual path as a lateral move into the nearest open lane followed by a + // straight drive back across the alliance-zone line. double returnDistance = Math.abs(allianceRelativePose.getY() - returnTarget.getY()) + Math.max(0.0, allianceRelativePose.getX() - returnTarget.getX());