Added barge buffer zone, and reservsal mode for loop overruns

Author: Huck-Richardson

src/main/java/frc/robot/constants/BargeAlignConstants.java

@@ -7,8 +7,10 @@ public class BargeAlignConstants {
 
   public static final double kBlueEjectAlgaeX = 7.67; // 7.72
   public static final double kRedEjectAlgaeX = 9.86; // 9.81
-  public static final double kBlueRaiseElevatorX = 7.02; // 7.22
-  public static final double kRedRaiseElevatorX = 10.51; // 10.31
+  public static final double kBlueRaiseElevatorX = 7.22; // 7.22
+  public static final double kRedRaiseElevatorX = 10.71; // 10.31
+  public static final double kBlueUnsafeX = 7.02;
+  public static final double kRedUnsafeX = 10.51;
 
   public static final Rotation2d kBlueDesiredYaw = Rotation2d.fromDegrees(0.0);
   public static final Rotation2d kRedDesiredYaw = Rotation2d.fromDegrees(180.0);

src/main/java/frc/robot/constants/RobotStateConstants.java

@@ -8,8 +8,9 @@ public class RobotStateConstants {
   public static final double[] kNodeAngles = {0.0, 60.0, 120.0, 180.0, -120.0, -60.0};
   public static final double kAlgaeRetreatDistance = 0;
 
-  public static final double kBlueBargeSafeX = 7; // 7.6
-  public static final double kRedBargeSafeX = DriveConstants.kFieldMaxX - kBlueBargeSafeX;
+  // Use Barge align constants
+  // public static final double kBlueBargeSafeX = 7; // 7.6
+  // public static final double kRedBargeSafeX = DriveConstants.kFieldMaxX - kBlueBargeSafeX;
 
   public static final double kCoralEjectTimer = 0.25;
   public static final double kAlgaeEjectTimer = 0.5;

src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Timer;
+import frc.robot.constants.BargeAlignConstants;
 import frc.robot.constants.DriveConstants;
 import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.RobotConstants;
@@ -672,8 +673,8 @@ public void toScoreAlgae() {
         }
         double poseX = driveSubsystem.getPoseMeters().getX();
         isBargeSafe =
-            poseX > RobotStateConstants.kRedBargeSafeX
-                || poseX < RobotStateConstants.kBlueBargeSafeX;
+            poseX > BargeAlignConstants.kRedRaiseElevatorX
+                || poseX < BargeAlignConstants.kBlueRaiseElevatorX;
         if (isBargeSafe) {
           driveSubsystem.setDriveMultiplier(DriveConstants.kBargeScoreStickMultiplier);
           if (isAutoPlacing) {

src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java

@@ -5,13 +5,13 @@
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import frc.robot.constants.BargeAlignConstants;
 import frc.robot.constants.DriveConstants;
-import frc.robot.constants.RobotStateConstants;
 import frc.robot.controllers.FlyskyJoystick;
 import frc.robot.subsystems.drive.DriveSubsystem;
 import java.util.Set;
 import org.littletonrobotics.junction.Logger;
 import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
 import org.strykeforce.telemetry.measurable.Measure;
+// TODO Make final drive faster. Reverse if we are too close to the barge.
 
 public class BargeAlignSubsystem extends MeasurableSubsystem {
 
@@ -76,15 +76,24 @@ private void setupBargeAlign() {
   private boolean isSafe() {
     double poseX = driveSubsystem.getPoseMeters().getX();
 
-    return poseX > RobotStateConstants.kRedBargeSafeX
-        || poseX < RobotStateConstants.kBlueBargeSafeX;
+    return poseX > BargeAlignConstants.kRedRaiseElevatorX
+        || poseX < BargeAlignConstants.kBlueRaiseElevatorX;
   }
 
   private boolean shouldRaiseElevator() {
     double poseX = driveSubsystem.getPoseMeters().getX();
     return isOnBlueSide
         ? poseX > BargeAlignConstants.kBlueRaiseElevatorX
-        : poseX < BargeAlignConstants.kRedRaiseElevatorX;
+            && poseX <= BargeAlignConstants.kBlueUnsafeX
+        : poseX < BargeAlignConstants.kRedRaiseElevatorX
+            && poseX >= BargeAlignConstants.kRedUnsafeX;
+  }
+
+  private boolean shouldDriveBackwards() {
+    double poseX = driveSubsystem.getPoseMeters().getX();
+    return isOnBlueSide
+        ? poseX > BargeAlignConstants.kBlueUnsafeX
+        : poseX < BargeAlignConstants.kRedUnsafeX;
   }
 
   private boolean shouldEjectAlgae() {
@@ -131,6 +140,8 @@ public void periodic() {
         Logger.recordOutput("BargeAlign/Vomega", vOmega);
         if (shouldRaiseElevator()) {
           setState(BargeAlignStates.RAISE_ELEV);
+        } else if (shouldDriveBackwards()) {
+          setState(BargeAlignStates.REVERSE);
         }
       }
       case RAISE_ELEV -> {
@@ -145,6 +156,14 @@ public void periodic() {
           terminate();
         }
       }
+      case REVERSE -> {
+        if (isOnBlueSide) {
+          driveSubsystem.move(
+              -vX, getYStickReading(), BargeAlignConstants.kBlueRaiseElevatorX, true);
+        } else {
+          driveSubsystem.move(vX, getYStickReading(), BargeAlignConstants.kRedRaiseElevatorX, true);
+        }
+      }
       case FINISHED -> {}
     }
   }
@@ -158,6 +177,7 @@ public enum BargeAlignStates {
     // INIT,
     DRIVE,
     RAISE_ELEV,
+    REVERSE,
     FINISHED
   }
 }