From 7b771086a5b90ed3c9ba77610267e9815ec1c01e Mon Sep 17 00:00:00 2001
From: David Shen <dshen1248@gmail.com>
Date: Thu, 29 May 2025 20:11:58 -0400
Subject: [PATCH] reverse in danger zone

---
 .../robotState/ForceBargeCommand.java         |  2 +-
 .../robot/constants/BargeAlignConstants.java  |  4 +++
 .../robotState/RobotStateSubsystem.java       | 22 +++++++--------
 .../tagAlign/BargeAlignSubsystem.java         | 27 ++++++++++++-------
 4 files changed, 33 insertions(+), 22 deletions(-)

diff --git a/src/main/java/frc/robot/commands/robotState/ForceBargeCommand.java b/src/main/java/frc/robot/commands/robotState/ForceBargeCommand.java
index ae025a3..878e74d 100644
--- a/src/main/java/frc/robot/commands/robotState/ForceBargeCommand.java
+++ b/src/main/java/frc/robot/commands/robotState/ForceBargeCommand.java
@@ -58,7 +58,7 @@ public boolean isFinished() {
       return robotStateSubsystem.getState() == RobotStates.PROCESSOR_ALGAE
           || (robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE
               && !robotStateSubsystem.getIsAutoPlacing())
-          || !robotStateSubsystem.getIsBargeSafe();
+      /*|| !robotStateSubsystem.getIsBargeSafe() */ ;
     }
   }
 }
diff --git a/src/main/java/frc/robot/constants/BargeAlignConstants.java b/src/main/java/frc/robot/constants/BargeAlignConstants.java
index 6efecf5..52ec593 100644
--- a/src/main/java/frc/robot/constants/BargeAlignConstants.java
+++ b/src/main/java/frc/robot/constants/BargeAlignConstants.java
@@ -4,12 +4,16 @@
 
 public class BargeAlignConstants {
   public static final double kXSpeed = 0.45;
+  public static final double kXRevSpeed = 2;
 
   public static final double kBlueEjectAlgaeX = 7.67; // 7.72
   public static final double kRedEjectAlgaeX = 9.86; // 9.81
   public static final double kBlueRaiseElevatorX = 6.82; // 7.02
   public static final double kRedRaiseElevatorX = 10.71; // 10.51
 
+  public static final double kBlueRevDoneX = kBlueRaiseElevatorX + 0.25;
+  public static final double kRedRevDoneX = kRedRaiseElevatorX - 0.25;
+
   public static final double kBlueUnsafeX = 7.02;
   public static final double kRedUnsafeX = 10.51;
 
diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java
index a0ab829..a2c752d 100644
--- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java
@@ -741,18 +741,18 @@ public void toScoreAlgae() {
         isBargeSafe =
             poseX > RobotStateConstants.kRedBargeSafeX
                 || poseX < RobotStateConstants.kBlueBargeSafeX;
-        if (isBargeSafe) {
-          driveSubsystem.setDriveMultiplier(DriveConstants.kBargeScoreStickMultiplier);
-          if (isAutoPlacing) {
-            setAutoPlacingLed(true);
-            driveSubsystem.setIgnoreSticks(true);
-            bargeAlignSubsystem.startBargeAlign(allianceColor);
-            setState(RobotStates.BARGE_ALIGN);
-          } else {
-            elevatorSubsystem.setPosition(ElevatorConstants.kBargeSetpoint);
-            setState(RobotStates.TO_BARGE_ALGAE);
-          }
+        // if (isBargeSafe) {
+        driveSubsystem.setDriveMultiplier(DriveConstants.kBargeScoreStickMultiplier);
+        if (isAutoPlacing) {
+          setAutoPlacingLed(true);
+          driveSubsystem.setIgnoreSticks(true);
+          bargeAlignSubsystem.startBargeAlign(allianceColor);
+          setState(RobotStates.BARGE_ALIGN);
+        } else {
+          elevatorSubsystem.setPosition(ElevatorConstants.kBargeSetpoint);
+          setState(RobotStates.TO_BARGE_ALGAE);
         }
+        // }
       }
     }
   }
diff --git a/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java
index 6649c50..b69961d 100644
--- a/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java
@@ -39,12 +39,12 @@ public BargeAlignSubsystem(FlyskyJoystick flysky, DriveSubsystem driveSubsystem)
 
   public void startBargeAlign(Alliance alliance) {
     this.alliance = alliance;
-    if (isSafe()) {
-      this.isOnBlueSide = isOnBlueSide();
-      setState(BargeAlignStates.DRIVE);
-      setupBargeAlign();
-      driveOmega.reset();
-    }
+    // if (isSafe()) {
+    this.isOnBlueSide = isOnBlueSide();
+    setState(BargeAlignStates.DRIVE);
+    setupBargeAlign();
+    driveOmega.reset();
+    // }
   }
 
   public void killBargeAlign() {
@@ -96,6 +96,13 @@ private boolean shouldDriveBackwards() {
         : poseX < BargeAlignConstants.kRedUnsafeX;
   }
 
+  private boolean shouldStopDrivingBackwards() {
+    double poseX = driveSubsystem.getPoseMeters().getX();
+    return isOnBlueSide
+        ? poseX < BargeAlignConstants.kBlueRevDoneX
+        : poseX > BargeAlignConstants.kRedRevDoneX;
+  }
+
   private boolean shouldEjectAlgae() {
     double poseX = driveSubsystem.getPoseMeters().getX();
     return isOnBlueSide
@@ -160,14 +167,14 @@ public void periodic() {
         double vOmega =
             driveOmega.calculate(
                 driveSubsystem.getPoseMeters().getRotation().getRadians(), targetYaw.getRadians());
+        double revX = BargeAlignConstants.kXRevSpeed * (isOnBlueSide ? -1 : 1);
+        driveSubsystem.move(revX, getYStickReading(), vOmega, true);
 
-        driveSubsystem.move(-vX, getYStickReading(), vOmega, true);
-
-        Logger.recordOutput("BargeAlign/Vx", vX);
+        Logger.recordOutput("BargeAlign/Vx", revX);
         Logger.recordOutput("BargeAlign/OmegaErr", driveOmega.getError());
         Logger.recordOutput("BargeAlign/Vomega", vOmega);
 
-        if (shouldRaiseElevator()) {
+        if (shouldStopDrivingBackwards()) {
           setState(BargeAlignStates.RAISE_ELEV);
         }
       }