reverse motors command

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 (file)
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 f4d0bcc8d5c40313b535e97fc401cbb1c46aa3ca..21a7a35eeee79b0276993139a94366c7d979bc31 100644 (file)
--- 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 05bf846bd1713cf34d50183ecbea9235ac6c0b8d..d4992d30f0eb9e87e6a83d9506dff4260a937e67 100644 (file)
--- 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 42c4defbe0d999ceeee7833ea0e92f2272cf9513..cdd4a6980735e82b043e79fd9483c7edc5e34877 100644 (file)
--- 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
+    
 }