stuffing, reverses intake when unjamming

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index db87084b75436c3957a50944e37c27adc796cc12..5dcbaee848d854cf5e5171656f7bbe05feba1250 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -246,8 +246,8 @@ public class RobotContainer {
       }));
     }
 
-    if (turret != null && drive != null && hood != null && shooter != null && spindexer != null) {
-      Command runSpindexer = new RunSpindexer(spindexer, turret, hood);
+    if (turret != null && drive != null && hood != null && shooter != null && spindexer != null && intake != null) {
+      Command runSpindexer = new RunSpindexer(spindexer, turret, hood, intake);
       NamedCommands.registerCommand("Auto shoot", new AutoShootCommand(turret, drive, hood, shooter, spindexer));
       NamedCommands.registerCommand("Start Spindexer",
           new InstantCommand(() -> CommandScheduler.getInstance().schedule(runSpindexer)));

diff --git a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java
index 05ae1b66121ce9688dc3c7f207c98b80208c5fee..2ed506dd809a1fa6ed8c844c22b80267de541326 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java
+++ b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java
@@ -2,16 +2,13 @@ package frc.robot.commands.gpm;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Intake.Intake;
-import frc.robot.subsystems.spindexer.Spindexer;
 
 public class ReverseMotors extends Command {
     private Intake intake;
-    private Spindexer spindexer;
 
 
-    public ReverseMotors(Intake intake, Spindexer spindexer){
+    public ReverseMotors(Intake intake){
         this.intake = intake;
-        this.spindexer = spindexer;
 
         addRequirements(intake);
     }

diff --git a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java
index 4a6ac7c6d1216137bfea72b1ddd70fe865068d3a..adc423f9f4edcfd7b75ce5034e382a2c065d3487 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java
+++ b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java
@@ -5,7 +5,10 @@ import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ScheduleCommand;
 import frc.robot.constants.Constants;
+import frc.robot.constants.IntakeConstants;
+import frc.robot.subsystems.Intake.Intake;
 import frc.robot.subsystems.hood.Hood;
 import frc.robot.subsystems.spindexer.Spindexer;
 import frc.robot.subsystems.spindexer.SpindexerConstants;
@@ -15,6 +18,7 @@ public class RunSpindexer extends Command {
     private Spindexer spindexer;
     private Turret turret;
     private Hood hood;
+    private Intake intake;
 
     private Debouncer jam_debouncer = new Debouncer(SpindexerConstants.JAM_DEBOUNCE_TIME, DebounceType.kRising); // if there is jam I would think this is 0 -> 1
 
@@ -22,11 +26,14 @@ public class RunSpindexer extends Command {
     private boolean wasHoodForcedDown = false;
 
     private Timer reverseTimer = new Timer();
+
+    private double storedIntakeSpeed = 0.0;
     
-    public RunSpindexer(Spindexer spindexer, Turret turret, Hood hood) {
+    public RunSpindexer(Spindexer spindexer, Turret turret, Hood hood, Intake intake) {
         this.spindexer = spindexer;
         this.turret = turret;
         this.hood = hood;
+        this.intake = intake;
 
         addRequirements(spindexer);
     }
@@ -60,13 +67,22 @@ public class RunSpindexer extends Command {
             reversing = true;
             reverseTimer.reset();
             reverseTimer.start();
+            storedIntakeSpeed = intake.getSpeed();
         }
         if (!reversing) {
             spindexer.maxSpindexer();
         } else {
             spindexer.reverseSpindexer();
+
+            if (intake.getPosition() > IntakeConstants.INTERMEDIATE_EXTENSION + 1.0) {
+                intake.spinReverse();
+            } else {
+                intake.extend();
+            }
+
             if (reverseTimer.hasElapsed(SpindexerConstants.REVERSE_DEBOUNCE_TIME)) {
                 reversing = false;
+                intake.spin(storedIntakeSpeed);
             }
         }
         if (!Constants.DISABLE_SMART_DASHBOARD) {

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 07fba40fc67928e24783063bc7b063220a3531bb..ec1916f32a6d182ee0de92a894e98765e1990d8f 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -124,11 +124,11 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
         }
 
         // Spindexer
-        if (spindexer != null && turret != null && hood != null) {
+        if (spindexer != null && turret != null && hood != null && intake != null) {
 
             // Toggle spindexer
             controller.get(PS5Button.LEFT_TRIGGER).toggleOnTrue(
-                new RunSpindexer(spindexer, turret, hood)
+                new RunSpindexer(spindexer, turret, hood, intake)
             );
         }
 

diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java
index bfbd44de738a5d826b1ffee6e77794c77ba1bdc6..b0eea6670cce58b34b3e789e3b6d3485a76b23ac 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java
@@ -302,6 +302,10 @@ public class Intake extends SubsystemBase implements IntakeIO{
         rollerMotor.set(speed);
     }
 
+    public double getSpeed() {
+        return rollerMotor.get();
+    }
+
     /**
      * Start the intake roller spinning.
      */

diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
index 6b49f3bcd81ea7fc22172f2c7aa30199b81201b7..4cba8edf1c58b69a2284da2aa40a02241ac0ee1b 100644 (file)
--- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
@@ -94,7 +94,7 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         wasSpindexerSlow = isSpindexerSlow;
 
         if (!Constants.DISABLE_SMART_DASHBOARD) {
-            SmartDashboard.putBoolean("Spindexer Jamming", state == SpindexerState.REVERSE);
+            SmartDashboard.putBoolean("Spindexer Reversing", state == SpindexerState.REVERSE);
         }
     }