Final changes at CRI (height tuning, auto, vision, algae clear)

Author: KaraParikh1

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));
   }
 }

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);

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(

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