Merge branch 'main' into trench-assist-mega

diff --cc src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index d38caa59ac8423b51f64c776749e3f020c144748,fd5e459b08d7b19bc223c9e16b421e195edfab39..ee9ffd0e203d32cfa33e5e489f9fed95cb07bff6
--- 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
@@@ -74,17 -79,23 +79,30 @@@ public class PS5ControllerDriverConfig 
                  interrupted -> getDrivetrain().setStateDeadband(true),
                  () -> false, getDrivetrain()).withTimeout(2));
  
 -        if (intake != null && spindexer != null && shooter != null){
 -            driver.get(PS5Button.CIRCLE).onTrue(new InstantCommand(()->{
 +        // Trench align
 +        driver.get(DPad.LEFT).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAlign(true)))
 +                .onFalse(new InstantCommand(() -> getDrivetrain().setTrenchAlign(false)));
 +
 +        // Trench assist
 +        driver.get(DPad.RIGHT).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAssist(true)))
 +                .onFalse(new InstantCommand(() -> getDrivetrain().setTrenchAssist(false)));
 +
+         // Reverse motors
 -            })
 -            ).onFalse(new InstantCommand(()->{
 -                if(reverseMotors != null){
++        if (intake != null && spindexer != null && shooter != null) {
++            driver.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> {
+                 reverseMotors = new ReverseMotors(intake, spindexer);
+                 reverseMotors.schedule();
++            })).onFalse(new InstantCommand(() -> {
++                if (reverseMotors != null) {
+                     reverseMotors.cancel();
+                 }
+             }));
+         }
+ 
          // Intake
          if (intake != null) {
-             driver.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> {
+             // Toggle intake
+             driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> {
                  if (intakeBoolean) {
                      intake.extend();
                      intake.spinStart();
@@@ -97,24 -108,24 +115,24 @@@
              }));
  
              // Retract if hold for 3 seconds
-             driver.get(PS5Button.CROSS).debounce(3.0).onTrue(new InstantCommand(() -> {
 -            driver.get(PS5Button.RIGHT_TRIGGER).debounce(3.0).onTrue(new InstantCommand(()->{
++            driver.get(PS5Button.RIGHT_TRIGGER).debounce(3.0).onTrue(new InstantCommand(() -> {
                  intake.retract();
                  intakeBoolean = true;
              }));
  
              // Calibration
-             driver.get(PS5Button.OPTIONS).onTrue(new InstantCommand(() -> {
 -            driver.get(PS5Button.PS).onTrue(new InstantCommand(()->{
++            driver.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
                  intake.calibrate();
 -            })).onFalse(new InstantCommand(()->{
 +            })).onFalse(new InstantCommand(() -> {
                  intake.stopCalibrating();
              }));
          }
  
          // Spindexer
 -        if (spindexer != null){
 +        if (spindexer != null) {
              // Will only run if we are not calling default shoot command
-             driver.get(PS5Button.LB).onTrue(new InstantCommand(() -> spindexer.maxSpindexer()))
 -            driver.get(PS5Button.LEFT_TRIGGER).onTrue(new InstantCommand(()-> spindexer.maxSpindexer()))
 -            .onFalse(new InstantCommand(()-> spindexer.stopSpindexer()));
++            driver.get(PS5Button.LEFT_TRIGGER).onTrue(new InstantCommand(() -> spindexer.maxSpindexer()))
 +                    .onFalse(new InstantCommand(() -> spindexer.stopSpindexer()));
          }
  
          // Auto shoot
@@@ -136,6 -149,30 +156,28 @@@
              })).onFalse(new InstantCommand(() -> {
                  climb.stopCalibrating();
              }));
 -            driver.get(PS5Button.CROSS).onTrue(new InstantCommand(()-> {
+ 
+             // Climb retract
 -                new ClimbDriveCommand(climb, getDrivetrain()),
 -                new InstantCommand(()-> this.startRumble()),
 -                new WaitCommand(1),
 -                new InstantCommand(()-> this.endRumble())
 -            )
 -            );
++            driver.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> {
+                 climb.retract();
+             }));
+ 
+             // Drive to climb position and rumble
+             driver.get(PS5Button.TRIANGLE).onTrue(new SequentialCommandGroup(
 -            driver.get(PS5Button.PS).onTrue(new InstantCommand(()->{
++                    new ClimbDriveCommand(climb, getDrivetrain()),
++                    new InstantCommand(() -> this.startRumble()),
++                    new WaitCommand(1),
++                    new InstantCommand(() -> this.endRumble())));
+         }
+ 
+         // Hood
+         if (hood != null) {
+             // Calibration
 -            })).onFalse(new InstantCommand(()->{
++            driver.get(PS5Button.PS).onTrue(new InstantCommand(() -> {
+                 hood.calibrate();
++            })).onFalse(new InstantCommand(() -> {
+                 hood.stopCalibrating();
+             }));
          }
      }