idk I think I need a mental break

diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
index 7cae9fc5fe5bc907e4d85916b0066ea6c83f623f..ba2325747948b3a8bd3843bd3ed971c77e256d10 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
+++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
@@ -75,7 +75,9 @@ public class AutoShootCommand extends Command {
                                Translation2d.kZero,
                                FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())),
                                8.0); // Random initial goalState to prevent it being null
-        
+        // Jerry Debug
+        System.out.println("The current goal state (including the exit vel):" + goalState);
+
         addRequirements(turret);
     }
 

diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java
index d19db1b99b4162e85d55f64dd039c4e5378ce644..19591391cd0804fb1618d25a1217ac011889cb1c 100644 (file)
--- a/src/main/java/frc/robot/subsystems/hood/Hood.java
+++ b/src/main/java/frc/robot/subsystems/hood/Hood.java
@@ -21,7 +21,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.Constants;
 import frc.robot.constants.IdConstants;
 
-public class Hood extends SubsystemBase implements HoodIO{
+public class Hood extends SubsystemBase implements HoodIO {
     private TalonFX motor = new TalonFX(IdConstants.HOOD_ID, Constants.CANIVORE_SUB);
 
        private final LinearFilter setpointFilter = LinearFilter.singlePoleIIR(0.02, 0.02);

diff --git a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java
index b1079ffce01f6fe1335a329e8fb92324dfc266b3..8d199f01bd437f61ad3ca1b70a1e6efb44cce86f 100644 (file)
--- a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java
+++ b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java
@@ -13,11 +13,11 @@ public class HoodConstants {
     public static final double MAX_ACCELERATION = 160; // rad/s^2
 
     public static final double MAX_ANGLE = 82; // degrees
-    public static final double MIN_ANGLE = 58.5; // degrees 
+    public static final double MIN_ANGLE = 58.5; // degrees
 
     public static final double FEEDFORWARD_KV = 0.12;
 
     public static final double NORMAL_CURRENT_LIMIT = 40.0; // A
-    public static final double CALIBRATING_CURRENT_LIMIT = 10.0; //A
+    public static final double CALIBRATING_CURRENT_LIMIT = 10.0; // A
     public static final double CALIBRATION_CURRENT_THRESHOLD = 9.0; // A
 }

diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
index eba194e8942ae0682f809eb16e3823c975973147..6a8388373d2e366e03c67c29b7d609ee53859c9f 100644 (file)
--- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
@@ -42,6 +42,8 @@ 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) {
@@ -51,7 +53,7 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         } else {
             motor.set(power);
         }
-        
+
         // scale threshold based on power
         double velocityThreshold = SpindexerConstants.spindexerVelocityWithBall * power;
         SmartDashboard.putNumber("Spindexer Ball Count", ballCount);
@@ -63,6 +65,14 @@ 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;
     }

diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java
index 64b098f2f784485ff2c0cb448bf709ca57b0ee52..540ba7f3ac08d8f00abf146ed2a42fe9e67680b4 100644 (file)
--- a/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java
@@ -2,10 +2,10 @@ package frc.robot.subsystems.spindexer;
 
 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 currentLimit = 40; // A
     public static final double spindexerMaxPower = 0.75; 
     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
-    
+    public static final double JAM_CURRENT_THRESHOLD = 9.0; // A
 }