Add hub reactivation return warning to driver feedback

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,12 @@ 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;
+  private static final long returnWarningRumblePulseCyclePhases = 2;
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
@@ -376,6 +384,33 @@ 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);
+
+    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 && currentPulsePhase % returnWarningRumblePulseCyclePhases == 0;
+    controller
+        .getHID()
+        .setRumble(
+            GenericHID.RumbleType.kBothRumble,
+            rumbleEnabled ? returnWarningRumbleStrength : 0.0);
+
     // Controller disconnected alerts
     controllerDisconnected.set(!DriverStation.isJoystickConnected(controller.getHID().getPort()));
   }

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,27 @@ 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,
+        (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 +213,43 @@ 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) {
+      // 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());
+      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);
+  }
 }