Reverted a push to main

Author: Huck-Richardson

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

@@ -7,10 +7,8 @@ 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.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 double kBlueRaiseElevatorX = 7.02; // 7.22
+  public static final double kRedRaiseElevatorX = 10.51; // 10.31
 
   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,9 +8,8 @@ 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;
 
-  // Use Barge align constants
-  // public static final double kBlueBargeSafeX = 7; // 7.6
-  // public static final double kRedBargeSafeX = DriveConstants.kFieldMaxX - kBlueBargeSafeX;
+  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,7 +9,6 @@
 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;
@@ -673,8 +672,8 @@ public void toScoreAlgae() {
         }
         double poseX = driveSubsystem.getPoseMeters().getX();
         isBargeSafe =
-            poseX > BargeAlignConstants.kRedRaiseElevatorX
-                || poseX < BargeAlignConstants.kBlueRaiseElevatorX;
+            poseX > RobotStateConstants.kRedBargeSafeX
+                || poseX < RobotStateConstants.kBlueBargeSafeX;
         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,24 +76,15 @@ private void setupBargeAlign() {
   private boolean isSafe() {
     double poseX = driveSubsystem.getPoseMeters().getX();
 
-    return poseX > BargeAlignConstants.kRedRaiseElevatorX
-        || poseX < BargeAlignConstants.kBlueRaiseElevatorX;
+    return poseX > RobotStateConstants.kRedBargeSafeX
+        || poseX < RobotStateConstants.kBlueBargeSafeX;
   }
 
   private boolean shouldRaiseElevator() {
     double poseX = driveSubsystem.getPoseMeters().getX();
     return isOnBlueSide
         ? poseX > BargeAlignConstants.kBlueRaiseElevatorX
-            && 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;
+        : poseX < BargeAlignConstants.kRedRaiseElevatorX;
   }
 
   private boolean shouldEjectAlgae() {
@@ -140,8 +131,6 @@ public void periodic() {
         Logger.recordOutput("BargeAlign/Vomega", vOmega);
         if (shouldRaiseElevator()) {
           setState(BargeAlignStates.RAISE_ELEV);
-        } else if (shouldDriveBackwards()) {
-          setState(BargeAlignStates.REVERSE);
         }
       }
       case RAISE_ELEV -> {
@@ -156,15 +145,6 @@ public void periodic() {
           terminate();
         }
       }
-      case REVERSE -> {
-        if (isOnBlueSide) {
-          driveSubsystem.move(
-              -vX, getYStickReading(), BargeAlignConstants.kBlueRaiseElevatorX, true);
-              //TODO add should raise elevator
-        } else {
-          driveSubsystem.move(vX, getYStickReading(), BargeAlignConstants.kRedRaiseElevatorX, true);
-        }
-      }
       case FINISHED -> {}
     }
   }
@@ -178,7 +158,6 @@ public enum BargeAlignStates {
     // INIT,
     DRIVE,
     RAISE_ELEV,
-    REVERSE,
     FINISHED
   }
 }