Merge remote-tracking branch 'origin' into shooter-calcs

diff --cc src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index cd4910529dbacde9b49fff3cea62053673312dcb,b646ac80509844b569765bf82fd6c08cc3e90207..419f0f9aac03d52680178e72015bb61e1e2babf9
--- 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
@@@ -22,7 -20,10 +22,8 @@@ import frc.robot.subsystems.hood.Hood
  import frc.robot.subsystems.shooter.Shooter;
  import frc.robot.subsystems.spindexer.Spindexer;
  import frc.robot.subsystems.turret.Turret;
 -import frc.robot.subsystems.hood.Hood;
 -import frc.robot.subsystems.Intake.Intake;
  import lib.controllers.PS5Controller;
+ import lib.controllers.PS5Controller.DPad;
  import lib.controllers.PS5Controller.PS5Axis;
  import lib.controllers.PS5Controller.PS5Button;
  
@@@ -79,14 -79,23 +79,23 @@@ public class PS5ControllerDriverConfig 
                  interrupted -> getDrivetrain().setStateDeadband(true),
                  () -> false, getDrivetrain()).withTimeout(2));
  
+         // Trench align
+         controller.get(DPad.LEFT).onTrue(new InstantCommand(() -> {
+             getDrivetrain().setTrenchAssist(true);
+             getDrivetrain().setTrenchAlign(true);
+         }))
+                 .onFalse(new InstantCommand(() -> {
+                     getDrivetrain().setTrenchAssist(false);
+                     getDrivetrain().setTrenchAlign(false);
+                 }));
+ 
          // Reverse motors
-         if (intake != null && spindexer != null && shooter != null){
-             driver.get(PS5Button.CIRCLE).onTrue(new InstantCommand(()->{
+         if (intake != null && spindexer != null && shooter != null) {
+             controller.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> {
                  reverseMotors = new ReverseMotors(intake, spindexer);
 -                reverseMotors.schedule();
 +                              CommandScheduler.getInstance().schedule(reverseMotors);
-             })
-             ).onFalse(new InstantCommand(()->{
-                 if(reverseMotors != null){
+             })).onFalse(new InstantCommand(() -> {
+                 if (reverseMotors != null) {
                      reverseMotors.cancel();
                  }
              }));