Merge branch 'main' into trench-assist-mega

diff --cc src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 0e035cc3b8e222bd06dbffed6c99d11914bbb571,00278b12f918992ac42da02ca0afc8d0518efe70..d943631a231643c9a5bcb48fd9e89a7a9722d6ac
--- 1/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
--- 2/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@@ -57,70 -74,61 +74,69 @@@ public class PS5ControllerDriverConfig 
                  interrupted -> getDrivetrain().setStateDeadband(true),
                  () -> false, getDrivetrain()).withTimeout(2));
  
-         controller.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAlign(true)))
++        // TrenchAlign - on TRIANGLE button
++        driver.get(PS5Button.TRIANGLE).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAlign(true)))
 +            .onFalse(new InstantCommand(() -> getDrivetrain().setTrenchAlign(false)));
 +
-         controller.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAssist(true)))
++        // TrenchAssist - on RB button
++        driver.get(PS5Button.RB).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAssist(true)))
 +            .onFalse(new InstantCommand(() -> getDrivetrain().setTrenchAssist(false)));
 +
- 
- 
- 
-         // // Intake
-         // if (intake != null) {
-         //     driver.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> {
-         //         if (intakeBoolean) {
-         //             intake.extend();
-         //             intake.spinStart();
-         //             intakeBoolean = false;
-         //         } else {
-         //             intake.intermediateExtend();
-         //             intake.spinStop();
-         //             intakeBoolean = true;
-         //         }
-         //     }));
- 
-         //     // Retract if hold for 3 seconds
-         //     driver.get(PS5Button.CROSS).debounce(3.0).onTrue(new InstantCommand(()->{
-         //         intake.retract();
-         //         intakeBoolean = true;
-         //     }));
- 
-         //     // Calibration
-         //     driver.get(PS5Button.OPTIONS).onTrue(new InstantCommand(()->{
-         //         intake.calibrate();
-         //     })).onFalse(new InstantCommand(()->{
-         //         intake.stopCalibrating();
-         //     }));
-         // }
- 
-         // // Spindexer
-         // if (spindexer != null){
-         //     // Will only run if we are not calling default shoot command
-         //     driver.get(PS5Button.LB).onTrue(new InstantCommand(()-> spindexer.maxSpindexer()))
-         //     .onFalse(new InstantCommand(()-> spindexer.stopSpindexer()));
-         // }
- 
-         // // Auto shoot
-         // if (turret != null) {
-         //     driver.get(PS5Button.SQUARE).onTrue(
-         //             new InstantCommand(() -> {
-         //                 if (autoShoot != null && autoShoot.isScheduled()) {
-         //                     autoShoot.cancel();
-         //                 } else {
-         //                     autoShoot = new AutoShootCommand(turret, getDrivetrain(), hood, shooter, spindexer);
-         //                     CommandScheduler.getInstance().schedule(autoShoot);
-         //                 }
-         //             }));
-         // }
- 
-         // if (climb != null) {
-         //     driver.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> {
-         //         climb.hardstopCalibration();
-         //     })).onFalse(new InstantCommand(() -> {
-         //         climb.stopCalibrating();
-         //     }));
-         // }
+         // Intake
+         if (intake != null) {
+             driver.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> {
+                 if (intakeBoolean) {
+                     intake.extend();
+                     intake.spinStart();
+                     intakeBoolean = false;
+                 } else {
+                     intake.intermediateExtend();
+                     intake.spinStop();
+                     intakeBoolean = true;
+                 }
+             }));
+ 
+             // Retract if hold for 3 seconds
+             driver.get(PS5Button.CROSS).debounce(3.0).onTrue(new InstantCommand(()->{
+                 intake.retract();
+                 intakeBoolean = true;
+             }));
+ 
+             // Calibration
+             driver.get(PS5Button.OPTIONS).onTrue(new InstantCommand(()->{
+                 intake.calibrate();
+             })).onFalse(new InstantCommand(()->{
+                 intake.stopCalibrating();
+             }));
+         }
+ 
+         // Spindexer
+         if (spindexer != null){
+             // Will only run if we are not calling default shoot command
+             driver.get(PS5Button.LB).onTrue(new InstantCommand(()-> spindexer.maxSpindexer()))
+             .onFalse(new InstantCommand(()-> spindexer.stopSpindexer()));
+         }
+ 
+         // Auto shoot
+         if (turret != null) {
+             driver.get(PS5Button.SQUARE).onTrue(
+                     new InstantCommand(() -> {
+                         if (autoShoot != null && autoShoot.isScheduled()) {
+                             autoShoot.cancel();
+                         } else {
+                             autoShoot = new AutoShootCommand(turret, getDrivetrain(), hood, shooter, spindexer);
+                             CommandScheduler.getInstance().schedule(autoShoot);
+                         }
+                     }));
+         }
+ 
+         if (climb != null) {
+             driver.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> {
+                 climb.hardstopCalibration();
+             })).onFalse(new InstantCommand(() -> {
+                 climb.stopCalibrating();
+             }));
+         }
      }
  
      @Override

diff --cc src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
Simple merge