From 1f2bc173002727bf00bd465aeb37f950cb1cb385 Mon Sep 17 00:00:00 2001
From: Pix_XL <82770059+PIXLwastaken@users.noreply.github.com>
Date: Wed, 2 Apr 2025 18:39:39 -0400
Subject: [PATCH] Update AutoAlign.java

---
 .../frc/robot/vision/commands/AutoAlign.java  | 127 ++++++++++--------
 1 file changed, 68 insertions(+), 59 deletions(-)

diff --git a/src/main/java/frc/robot/vision/commands/AutoAlign.java b/src/main/java/frc/robot/vision/commands/AutoAlign.java
index 03784f2..462a62b 100644
--- a/src/main/java/frc/robot/vision/commands/AutoAlign.java
+++ b/src/main/java/frc/robot/vision/commands/AutoAlign.java
@@ -63,6 +63,8 @@ public class AutoAlign extends Command {
   double xSpeed;
   double ySpeed;
   double omegaSpeed;
+  Supplier<ReefFace> nearestReefFace;
+  boolean isLeftAlign;
 
   private final static CommandSwerveDrivetrain drivetrainSubsystem = CommandSwerveDrivetrain.getInstance();
 //   private final FieldCentric fieldCentricSwerveRequest = new FieldCentric()
@@ -84,69 +86,16 @@ public class AutoAlign extends Command {
   public AutoAlign(Supplier<Pose2d> targetPose, Supplier<ReefFace> nearestReefFace, boolean isLeftAlign) {
     this(drivetrainSubsystem, poseProvider);
     this.goalPose2d = targetPose.get();
+    this.nearestReefFace = nearestReefFace;
+    this.isLeftAlign = isLeftAlign;
+    
     //we are getting ATag Pose
     //firstly if elevator is L4, update the AprilTag X, Y
       //need to do - transform for left or right wrist
     //else
       //need to do - transform for left or right wrist
-    boolean isRightWrist = (RushinatorWrist.kLastState == RushinatorWrist.State.kTravelRight) ||
-                (RushinatorWrist.kLastState == RushinatorWrist.State.kTravelL4Right) ||
-                (RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL4RightWrist) || 
-                (RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL3RightWrist) || 
-                (RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL2RightWrist) || 
-                (RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL1Mid) ||
-                (RushinatorWrist.kLastState == RushinatorWrist.State.kGroundMid) ||
-                (RushinatorWrist.kLastState == RushinatorWrist.State.kHPMid);
-    boolean isElevatorL4 = (ElevatorSubsystem.kLastState == ElevatorSubsystem.State.kCoralL4) || 
-                          (ElevatorSubsystem.kLastState == ElevatorSubsystem.State.kCoralL4AutonScore) || 
-                          (ElevatorSubsystem.kLastState == ElevatorSubsystem.State.kCoralScoreL4);
-    
-    SmartDashboard.putBoolean("isRightWrist - AutoAlign", isRightWrist);
-    SmartDashboard.putBoolean("isElevatorL4 - AutoAlign", isElevatorL4);
-    SmartDashboard.putString("elevator kLastState - AutoAlign", ElevatorSubsystem.kLastState.name());
-    SmartDashboard.putString("alliance - AutoAlign", DriverStation.getAlliance().toString());
-    SmartDashboard.putBoolean("requesting lineup left branch - AutoAlign", isLeftAlign);
-    SmartDashboard.putString("nearest ReefFace accessed - AutoAlign", nearestReefFace.get().name());
-    if(isElevatorL4 == true) {
-      System.out.println("REACHED ELEVATOR L4 IF STATEMENT IN AUTOALIGN");
-      ReefFace newReefFace = updateReefFace(nearestReefFace.get());
-      SmartDashboard.putString("updated ReefFace - AutoAlign", newReefFace.name());
-      goalPose2d = new Pose2d(newReefFace.aprilTagX, newReefFace.aprilTagY, Rotation2d.fromDegrees(newReefFace.aprilTagTheta));
-      if(isRightWrist) {
-        if(isLeftAlign) {
-          goalPose2d = goalPose2d.transformBy(leftBranchTransformRightWrist);
-        }
-        else {
-          goalPose2d = goalPose2d.transformBy(rightBranchTransformRightWrist);
-        }
-      }
-      else {
-        if(isLeftAlign) {
-          goalPose2d = goalPose2d.transformBy(leftBranchTransformLeftWrist);
-        }
-        else {
-          goalPose2d = goalPose2d.transformBy(rightBranchTransformLeftWrist);
-        }
-      }
-    }
-    else {
-      if(isRightWrist) {
-        if(isLeftAlign) {
-          goalPose2d = goalPose2d.transformBy(leftBranchTransformRightWrist);
-        }
-        else {
-          goalPose2d = goalPose2d.transformBy(rightBranchTransformRightWrist);
-        }
-      }
-      else {
-        if(isLeftAlign) {
-          goalPose2d = goalPose2d.transformBy(leftBranchTransformLeftWrist);
-        }
-        else {
-          goalPose2d = goalPose2d.transformBy(rightBranchTransformLeftWrist);
-        }
-      }
-    }
+
+
 
     goalPose2d = Conversions.rotatePose(goalPose2d.transformBy(robotOffset), Rotation2d.kZero);
     
@@ -264,6 +213,66 @@ public void initialize() {
   public void execute() {
     var robotPose = poseProvider.get();
 
+    boolean isRightWrist = (RushinatorWrist.kLastState == RushinatorWrist.State.kTravelRight) ||
+    (RushinatorWrist.kLastState == RushinatorWrist.State.kTravelL4Right) ||
+    (RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL4RightWrist) || 
+    (RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL3RightWrist) || 
+    (RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL2RightWrist) || 
+    (RushinatorWrist.kLastState == RushinatorWrist.State.kScoreL1Mid) ||
+    (RushinatorWrist.kLastState == RushinatorWrist.State.kGroundMid) ||
+    (RushinatorWrist.kLastState == RushinatorWrist.State.kHPMid);
+boolean isElevatorL4 = (ElevatorSubsystem.kLastState == ElevatorSubsystem.State.kCoralL4) || 
+              (ElevatorSubsystem.kLastState == ElevatorSubsystem.State.kCoralL4AutonScore) || 
+              (ElevatorSubsystem.kLastState == ElevatorSubsystem.State.kCoralScoreL4);
+
+SmartDashboard.putBoolean("isRightWrist - AutoAlign", isRightWrist);
+SmartDashboard.putBoolean("isElevatorL4 - AutoAlign", isElevatorL4);
+SmartDashboard.putString("elevator kLastState - AutoAlign", ElevatorSubsystem.kLastState.name());
+SmartDashboard.putString("alliance - AutoAlign", DriverStation.getAlliance().toString());
+SmartDashboard.putBoolean("requesting lineup left branch - AutoAlign", isLeftAlign);
+SmartDashboard.putString("nearest ReefFace accessed - AutoAlign", nearestReefFace.get().name());
+
+    if(isElevatorL4 == true) {
+      System.out.println("REACHED ELEVATOR L4 IF STATEMENT IN AUTOALIGN");
+      ReefFace newReefFace = updateReefFace(nearestReefFace.get());
+      SmartDashboard.putString("updated ReefFace - AutoAlign", newReefFace.name());
+      goalPose2d = new Pose2d(newReefFace.aprilTagX, newReefFace.aprilTagY, Rotation2d.fromDegrees(newReefFace.aprilTagTheta));
+      if(isRightWrist) {
+        if(isLeftAlign) {
+          goalPose2d = goalPose2d.transformBy(leftBranchTransformRightWrist);
+        }
+        else {
+          goalPose2d = goalPose2d.transformBy(rightBranchTransformRightWrist);
+        }
+      }
+      else {
+        if(isLeftAlign) {
+          goalPose2d = goalPose2d.transformBy(leftBranchTransformLeftWrist);
+        }
+        else {
+          goalPose2d = goalPose2d.transformBy(rightBranchTransformLeftWrist);
+        }
+      }
+    }
+    else {
+      if(isRightWrist) {
+        if(isLeftAlign) {
+          goalPose2d = goalPose2d.transformBy(leftBranchTransformRightWrist);
+        }
+        else {
+          goalPose2d = goalPose2d.transformBy(rightBranchTransformRightWrist);
+        }
+      }
+      else {
+        if(isLeftAlign) {
+          goalPose2d = goalPose2d.transformBy(leftBranchTransformLeftWrist);
+        }
+        else {
+          goalPose2d = goalPose2d.transformBy(rightBranchTransformLeftWrist);
+        }
+      }
+    }
+
     xSpeed = xController.calculate(robotPose.getX(), this.goalPose2d.getX());
     if (xController.atSetpoint()) {
       xSpeed = 0;
@@ -279,7 +288,7 @@ public void execute() {
       omegaSpeed = 0;
     }
 
-    ChassisSpeeds speeds = applyLimits(new ChassisSpeeds(-xSpeed, -ySpeed, omegaSpeed));
+    ChassisSpeeds speeds = applyLimits(new ChassisSpeeds(xSpeed, ySpeed, omegaSpeed));
 
     // CommandSwerveDrivetrain.getInstance().applyRequest( () -> 
     //     RobotContainer.drive.withVelocityX(speeds.vxMetersPerSecond)