debouncer for anti jamming of spindexer

diff --git a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java
index 17a615fea8bd2c00860e9d68746b8d578451a2dd..6e9e2ee4ccf2dcdc83d919e5bac196b59d14c613 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java
+++ b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java
@@ -1,12 +1,17 @@
 package frc.robot.commands.gpm;
 
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.spindexer.Spindexer;
+import frc.robot.subsystems.spindexer.SpindexerConstants;
 import frc.robot.subsystems.turret.Turret;
 
 public class RunSpindexer extends Command {
     private Spindexer spindexer;
     private Turret turret;
+    private Debouncer jam_debouncer = new Debouncer(0.3, DebounceType.kFalling);
+    private boolean reversing = false;
     public RunSpindexer(Spindexer spindexer, Turret turret){
         this.spindexer = spindexer;
         this.turret = turret;
@@ -15,16 +20,31 @@ public class RunSpindexer extends Command {
 
     @Override
     public void execute() {
-        if (turret.atSetpoint()) {
-            spindexer.maxSpindexer();
-        } else {
+        if (!turret.atSetpoint()) {
             spindexer.stopSpindexer();
+            reversing = false;
+            return; // this is so the balls don't fly out when unaligned
+        }
+        boolean jammed = spindexer.getStatorCurrent() > SpindexerConstants.JAM_CURRENT_THRESHOLD;
+        if (jammed) {
+            reversing = true;
+        }
+
+        if (reversing) {
+            spindexer.reverseSpindexer();
+            
+            if (jam_debouncer.calculate(!jammed)) {
+                reversing = false;
+            }
+        } else {
+            spindexer.maxSpindexer();
         }
     }
 
     @Override
     public void end(boolean interrupted) {
         spindexer.stopSpindexer();
+        reversing = false;
     }
 
     @Override

diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
index 6a8388373d2e366e03c67c29b7d609ee53859c9f..53ed3e9cd251a00bf96e22c292f7c76b5abb1037 100644 (file)
--- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
@@ -42,8 +42,6 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         updateInputs();
         Logger.processInputs("Spindexer", inputs);
 
-        // reverse on jam
-        checkJam();
         if (state == SpindexerState.MAX) {
             motor.set(SpindexerConstants.spindexerMaxPower);
         } else if (state == SpindexerState.REVERSE) {
@@ -65,14 +63,6 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         wasSpindexerSlow = isSpindexerSlow;
     }
 
-    public void checkJam() {
-        // if a current spike then reverse
-        if (inputs.spindexerCurrent >= SpindexerConstants.JAM_CURRENT_THRESHOLD) {
-            state = SpindexerState.REVERSE;
-            System.out.println("Jammed: Reversing");
-        }
-    }
-
     public void maxSpindexer() {
         state = SpindexerState.MAX;
     }
@@ -90,6 +80,10 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         state = SpindexerState.CUSTOM;
     }
 
+    public double getStatorCurrent() {
+        return inputs.spindexerCurrent;
+    }
+
     @Override
     public void updateInputs() {
         inputs.spindexerVelocity = motor.getVelocity().getValueAsDouble(); //SpindexerConstants.gearRatio;