diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6a60369..6c4d606 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,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())); } diff --git a/src/main/java/frc/robot/util/HubShiftUtil.java b/src/main/java/frc/robot/util/HubShiftUtil.java index 70cd9c1..2951188 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,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); + } }