From f99c280a984c0f1bb73537a393a1dbe348f9a8aa Mon Sep 17 00:00:00 2001
From: KaraParikh1 <karaaparikh@gmail.com>
Date: Sat, 27 Sep 2025 21:58:49 -0400
Subject: [PATCH] Final changes at CRI (height tuning, auto, vision, algae
 clear)

---
 src/main/java/frc/robot/RobotContainer.java        |  4 ++--
 src/main/java/frc/robot/config/CoralConfig.java    |  4 ++--
 src/main/java/frc/robot/subsystems/Coral.java      | 12 ++++++++----
 src/main/java/frc/robot/subsystems/Drivetrain.java |  3 ++-
 4 files changed, 14 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 7a54c1e..2b7045a 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -232,7 +232,7 @@ private void configureBindings() {
                     applyDeadband(-controller.getLeftY() * speedMultiplier),
                     applyDeadband(-controller.getLeftX() * speedMultiplier),
                     applyDeadband(-controller.getRightX() * speedMultiplier),
-                    fieldRelative,
+                    true,
                     true),
             drivetrain));
 
@@ -265,6 +265,6 @@ public void periodic() {
   // autonomous command
   public Command getAutonomousCommand() {
     return Commands.sequence(
-        Commands.waitSeconds(2), coral.goToReef(drivetrain, () -> reefSection, () -> 3));
+        Commands.waitSeconds(1), coral.goToReef(drivetrain, () -> reefSection, () -> 3));
   }
 }
diff --git a/src/main/java/frc/robot/config/CoralConfig.java b/src/main/java/frc/robot/config/CoralConfig.java
index b2aba7a..90ee4f1 100644
--- a/src/main/java/frc/robot/config/CoralConfig.java
+++ b/src/main/java/frc/robot/config/CoralConfig.java
@@ -8,8 +8,8 @@ public class CoralConfig {
   public static final double MOVEMENT_I = 0.0;
   public static final double MOVEMENT_D = 0.0;
 
-  public static final double POSITION_TOLERANCE = 0.5;
-  public static final double VELOCITY_TOLERANCE = 1.0;
+  public static final double POSITION_TOLERANCE = 0.15;
+  public static final double VELOCITY_TOLERANCE = 0.15;
 
   public static final double INTAKE_HEIGHT = 0.065;
   public static final Rotation2d INTAKE_ANGLE_UPPER = Rotation2d.fromRadians(1.65);
diff --git a/src/main/java/frc/robot/subsystems/Coral.java b/src/main/java/frc/robot/subsystems/Coral.java
index 510cf93..ede0b90 100644
--- a/src/main/java/frc/robot/subsystems/Coral.java
+++ b/src/main/java/frc/robot/subsystems/Coral.java
@@ -24,7 +24,7 @@ public class Coral extends SubsystemBase {
   private CoralPivot coralPivot = new CoralPivot();
 
   // heights for levels
-  private double[] elevatorScoringHeights = {0.0, 0.900, 1.29, 1.360};
+  private double[] elevatorScoringHeights = {0.0, 0.910, 1.31, 1.360};
 
   // rotations for levels
   private Rotation2d[] coralScoringRotations = {
@@ -34,10 +34,10 @@ public class Coral extends SubsystemBase {
     Rotation2d.fromRadians(0.3)
   };
   private Rotation2d[] reefClearRotationsPrimary = {
-    Rotation2d.fromRadians(1.5), Rotation2d.fromRadians(1.5)
+    Rotation2d.fromRadians(1.9), Rotation2d.fromRadians(1.9)
   };
   private Rotation2d[] reefClearRotationsSecondary = {
-    Rotation2d.fromRadians(0.95), Rotation2d.fromRadians(0.95)
+    Rotation2d.fromRadians(1.5), Rotation2d.fromRadians(1.5)
   };
   private double[] reefClearHeights = {0.65, 1.0564};
 
@@ -202,7 +202,11 @@ public Command goToReef(Drivetrain drivetrain, IntSupplier idx, IntSupplier heig
                       .getRadians());
             },
             () -> CoralConfig.MOVEMENT_DISTANCE)
-        .until(() -> movementController.atSetpoint())
+        .until(
+            () ->
+                movementController.atSetpoint()
+                    && drivetrain.rotController.atSetpoint()
+                    && drivetrain.xController.atSetpoint())
         .andThen(
             Commands.waitSeconds(1.0)
                 .andThen(
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index e26ecfe..02c1090 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -210,7 +210,8 @@ public void periodic() {
     // Puts the pose data from one camera into a list
     List<PhotonPipelineResult> results = Vision.camera.getAllUnreadResults();
 
-    // If there is pose data from the cameras, get the latest estimated pose and update the 'vision'
+    // If there is pose data from the cameras, get the latest estimated pose and update the
+    // 'vision'
     // photon pose estimator
     // If there is no multi tag result and the distance from the camera to the target is greater
     // than