fix

diff --git a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java
index b8e870347656a7851896a44e70bc7eb7f97a7031..4cea990d9ca6d3bb548acc53a3705a4fbca27eba 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java
+++ b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java
@@ -44,9 +44,8 @@ public class ReverseMotors extends Command {
 
     @Override
     public void end(boolean interrupted){
-        intake.spinStop();
-        spindexer.stopSpindexer();
-        shooter.setShooter(0);
+        intake.spinStart();
+        spindexer.maxSpindexer();
     }
 
 }

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 17e7a9decec29e89c1a6d8ea0284978080dac4fd..bc25a2d16636f65539bfc2ebb169cba3fb39bf8b 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -32,6 +32,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
     private final BooleanSupplier slowModeSupplier = () -> false;
     private boolean intakeBoolean = true;
     private Command autoShoot = null;
+    private Command reverseMotors = null;
     private Shooter shooter;
     private Turret turret;
     private Hood hood;
@@ -63,7 +64,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
                 new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
 
         // Cancel commands
-        driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> {
+        driver.get(PS5Button.RB).onTrue(new InstantCommand(() -> {
             getDrivetrain().setIsAlign(false);
             getDrivetrain().setDesiredPose(() -> null);
             CommandScheduler.getInstance().cancelAll();
@@ -76,9 +77,20 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
                 interrupted -> getDrivetrain().setStateDeadband(true),
                 () -> false, getDrivetrain()).withTimeout(2));
 
+        // Reverse motors
+        if (intake != null && spindexer != null && shooter != null){
+            driver.get(PS5Button.CIRCLE).onTrue(new InstantCommand(()->{
+                reverseMotors = new ReverseMotors(intake, spindexer, shooter);
+                reverseMotors.schedule();
+            })
+            ).onFalse(new InstantCommand(()->{
+                reverseMotors.cancel();
+            }));
+        }
+
         // Intake
         if (intake != null) {
-            driver.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> {
+            driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> {
                 if (intakeBoolean) {
                     intake.extend();
                     intake.spinStart();
@@ -91,7 +103,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
             }));
 
             // 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(()->{
                 intake.retract();
                 intakeBoolean = true;
             }));
@@ -104,17 +116,10 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
             }));
         }
 
-        // Reverse motors
-        if (intake != null && spindexer != null && shooter != null){
-            driver.get(PS5Button.CIRCLE).onTrue(
-                new ReverseMotors(intake, spindexer, shooter)
-            );
-        }
-
         // Spindexer
         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()));
         }
 
@@ -141,7 +146,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
             }));
 
             // Climb climb
-            driver.get(PS5Button.LEFT_TRIGGER).onTrue(new InstantCommand(()-> {
+            driver.get(PS5Button.CROSS).onTrue(new InstantCommand(()-> {
                 climb.goDown();
             }));
         }