From fe75b88904108785fc8ae7ad877b2aaeb90396a9 Mon Sep 17 00:00:00 2001
From: eileha <eileenhan369@gmail.com>
Date: Sat, 21 Feb 2026 11:16:14 -0800
Subject: [PATCH] reverse motors command

---
 .../frc/robot/commands/gpm/ReverseMotors.java | 52 +++++++++++++++++++
 .../controls/PS5ControllerDriverConfig.java   |  9 ++++
 .../robot/subsystems/spindexer/Spindexer.java |  4 ++
 .../spindexer/SpindexerConstants.java         |  2 +
 4 files changed, 67 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/gpm/ReverseMotors.java

diff --git a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java
new file mode 100644
index 0000000..b8e8703
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java
@@ -0,0 +1,52 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.Intake.Intake;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.ShooterConstants;
+import frc.robot.subsystems.spindexer.Spindexer;
+import lib.controllers.PS5Controller.PS5Button;
+
+public class ReverseMotors extends Command {
+    // Reverse motors (intake, spindexer, shooter: shoot out constant speed)
+
+        // if (intake != null && spindexer != null && shooter != null){
+        //     driver.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() ->{
+        //         intake.spinReverse();
+        //         // reverse spindexer
+        //         shooter.
+        //     }))
+        // }
+    private Intake intake;
+    private Spindexer spindexer;
+    private Shooter shooter;
+
+
+    public ReverseMotors(Intake intake, Spindexer spindexer, Shooter shooter){
+        this.intake = intake;
+        this.spindexer = spindexer;
+
+        addRequirements(intake, spindexer, shooter);
+    }
+
+    @Override
+    public void initialize(){
+
+    }
+
+    @Override
+    public void execute(){
+        intake.spinReverse();
+        spindexer.reverseSpindexer();
+        shooter.setShooter(ShooterConstants.SHOOTER_VELOCITY);
+    }
+
+    @Override
+    public void end(boolean interrupted){
+        intake.spinStop();
+        spindexer.stopSpindexer();
+        shooter.setShooter(0);
+    }
+
+}
diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index f4d0bcc..21a7a35 100644
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -8,8 +8,10 @@ import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import frc.robot.Robot;
 import frc.robot.commands.gpm.AutoShootCommand;
+import frc.robot.commands.gpm.ReverseMotors;
 import frc.robot.constants.Constants;
 import frc.robot.subsystems.Climb.LinearClimb;
 import frc.robot.subsystems.drivetrain.Drivetrain;
@@ -102,6 +104,13 @@ 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
diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
index 05bf846..d4992d3 100644
--- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
@@ -59,6 +59,10 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         power = SpindexerConstants.spindexerMaxPower;
     }
 
+    public void reverseSpindexer(){
+        power = SpindexerConstants.spindexerReversePower;
+    }
+
     public void stopSpindexer() {
         power = 0.0;
     }
diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java
index 42c4def..cdd4a69 100644
--- a/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java
@@ -4,6 +4,8 @@ public class SpindexerConstants {
     public static final double spindexerVelocityWithBall = 6.0; // rps (for counting balls)
     public static final double currentLimit = 40; //A
     public static final double spindexerMaxPower = 0.5; 
+    public static final double spindexerReversePower = -0.2;
     public static final double CURRENT_SPIKE_LIMIT = 80;
     public static final double CURRENT_TIME_LIMIT = 1.0; //s
+    
 }
-- 
2.39.5