(#151) Add intake roller state machine with dejam mode

src/main/deploy/constants/comp/IntakeConstants.json

@@ -87,6 +87,26 @@
     "kV": 0.0,
     "kA": 0.0
   },
+  "rollerMovementThreshold": {
+    "value": 200.0,
+    "unit": "RPM"
+  },
+  "rollerJamCurrentThreshold": {
+    "value": 35.0,
+    "unit": "Amp"
+  },
+  "rollerJamDetectionTime": {
+    "value": 0.5,
+    "unit": "Second"
+  },
+  "rollerDejamTime": {
+    "value": 0.5,
+    "unit": "Second"
+  },
+  "rollerDejamVoltage": {
+    "value": -6.0,
+    "unit": "Volt"
+  },
   "pivotSupplyCurrentLimit": {
     "value": 40.0,
     "unit": "Amp"

src/main/java/frc/robot/CoordinationLayer.java

@@ -740,15 +740,6 @@ public void coordinateRobotActions() {
       intake.ifPresent(IntakeSubsystem::stopRollers);
     }
 
-    // Holds the intake down with a set voltage when intaking
-    if (runningIntakeRollers && deployIntake) {
-      intake.ifPresent(
-          intake -> {
-            intake.controlPivotMotorIOWithVoltage(
-                JsonConstants.intakeConstants.pivotVoltageWhenIntaking);
-          });
-    }
-
     // Handle intake deploy/retract
     if (deployIntake) {
       intake.ifPresent(IntakeSubsystem::deploy);

src/main/java/frc/robot/constants/IntakeConstants.java

@@ -80,6 +80,19 @@ public class IntakeConstants {
   public PIDGains rollersPIDGains =
       PIDGains.kPID(20.0, 10.0, 10.0); // These values are placeholders
 
+  // Dejam parameters
+  /**
+   * The threshold for movement of the rollers. This is used for deciding when to command the
+   * rollers to control a velocity vs. coast and how to determine if the intake rollers are "moving"
+   * for dejam detection.
+   */
+  public final AngularVelocity rollerMovementThreshold = RPM.of(200);
+
+  public final Current rollerJamCurrentThreshold = Amps.of(35);
+  public final Time rollerJamDetectionTime = Seconds.of(0.5);
+  public final Time rollerDejamTime = Seconds.of(0.5);
+  public final Voltage rollerDejamVoltage = Volts.of(-6.0);
+
   // Current Limits (These current limits are placeholders and were picked randomly)
   public final Current pivotSupplyCurrentLimit = Amps.of(40.0);
   public final Current pivotStatorCurrentLimit = Amps.of(40.0);

src/main/java/frc/robot/subsystems/intake/IntakeRollerState.java

@@ -0,0 +1,35 @@
+package frc.robot.subsystems.intake;
+
+import coppercore.controls.state_machine.State;
+import coppercore.controls.state_machine.StateMachine;
+import frc.robot.constants.JsonConstants;
+
+/** Contains states for the intake rollers. */
+public class IntakeRollerState {
+  private IntakeRollerState() {}
+
+  public static State<IntakeSubsystem> stopRollersState =
+      new State<IntakeSubsystem>("StopRollers") {
+        @Override
+        public void periodic(StateMachine<IntakeSubsystem> stateMachine, IntakeSubsystem world) {
+          world.rollersLeadMotorIO.controlNeutral();
+        }
+      };
+
+  public static State<IntakeSubsystem> runRollersState =
+      new State<IntakeSubsystem>("RunRollers") {
+        @Override
+        public void periodic(StateMachine<IntakeSubsystem> stateMachine, IntakeSubsystem world) {
+          world.rollersLeadMotorIO.controlToVelocityUnprofiled(world.requestedRollerSpeed);
+        }
+      };
+
+  public static State<IntakeSubsystem> dejamState =
+      new State<IntakeSubsystem>("Dejam") {
+        @Override
+        public void periodic(StateMachine<IntakeSubsystem> stateMachine, IntakeSubsystem world) {
+          world.rollersLeadMotorIO.controlOpenLoopVoltage(
+              JsonConstants.intakeConstants.rollerDejamVoltage);
+        }
+      };
+}

src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java

@@ -1,17 +1,23 @@
 package frc.robot.subsystems.intake;
 
+import static edu.wpi.first.units.Units.Amps;
 import static edu.wpi.first.units.Units.Degrees;
 import static edu.wpi.first.units.Units.Hertz;
 import static edu.wpi.first.units.Units.RPM;
 import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.Seconds;
 
 import coppercore.controls.state_machine.StateMachine;
 import coppercore.parameter_tools.LoggedTunableNumber;
 import coppercore.wpilib_interface.MonitoredSubsystem;
 import coppercore.wpilib_interface.subsystems.motors.MotorIO;
 import coppercore.wpilib_interface.subsystems.motors.MotorInputsAutoLogged;
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.MutAngularVelocity;
 import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotController;
@@ -30,8 +36,15 @@ public class IntakeSubsystem extends MonitoredSubsystem {
   TestModeManager<RollerTestMode> rollerTestModeManager =
       new TestModeManager<RollerTestMode>("IntakeRollers", RollerTestMode.class);
 
+  /** Controls the pivot, test modes, homing, and general intake operation */
   StateMachine<IntakeSubsystem> intakeStateMachine;
 
+  /**
+   * Controls the rollers and whether or not they need to be dejamming. This state machine will only
+   * run when not in test mode state.
+   */
+  StateMachine<IntakeSubsystem> rollerStateMachine;
+
   protected MotorIO pivotMotorIO;
   protected MotorIO rollersLeadMotorIO;
   protected MotorIO rollersFollowerMotorIO;
@@ -48,6 +61,10 @@ public class IntakeSubsystem extends MonitoredSubsystem {
   private IntakeDependencies dependencies = new IntakeDependencies();
 
   private boolean needsReHome = false;
+  protected final MutAngularVelocity requestedRollerSpeed = RPM.mutable(0.0);
+  protected final Debouncer dejamNeededDebouncer =
+      new Debouncer(
+          JsonConstants.intakeConstants.rollerJamDetectionTime.in(Seconds), DebounceType.kRising);
 
   // Dependencies (these are what we would have fetched using extensive supplier networks in 2025
   // and before)
@@ -98,6 +115,13 @@ public IntakeSubsystem(
             IntakeState.homingDoneState)
         .forEach(this.intakeStateMachine::registerState);
 
+    this.rollerStateMachine = new StateMachine<IntakeSubsystem>(this);
+    List.of(
+            IntakeRollerState.stopRollersState,
+            IntakeRollerState.runRollersState,
+            IntakeRollerState.dejamState)
+        .forEach(rollerStateMachine::registerState);
+
     pivotMotorIO.setRequestUpdateFrequency(Hertz.of(1000));
 
     // ### Test Mode Transitions
@@ -168,18 +192,49 @@ public IntakeSubsystem(
     // to position state
     IntakeState.homingDoneState.whenFinished().transitionTo(IntakeState.controlToPositionState);
 
+    // ### Roller state machine transitions
+    IntakeRollerState.stopRollersState
+        .when(
+            intake ->
+                intake.requestedRollerSpeed.gt(
+                    JsonConstants.intakeConstants.rollerMovementThreshold),
+            "Target roller speed > movement threshold")
+        .transitionTo(IntakeRollerState.runRollersState);
+    IntakeRollerState.runRollersState
+        .when(
+            intake ->
+                intake.requestedRollerSpeed.lt(
+                    JsonConstants.intakeConstants.rollerMovementThreshold),
+            "Target roller speed < movement threshold")
+        .transitionTo(IntakeRollerState.stopRollersState);
+    IntakeRollerState.runRollersState
+        .when(
+            intake ->
+                intake.dejamNeededDebouncer.calculate(
+                    intake.rollerLeadMotorInputs.supplyCurrentAmps
+                            > JsonConstants.intakeConstants.rollerJamCurrentThreshold.in(Amps)
+                        && intake.rollerLeadMotorInputs.velocityRadiansPerSecond
+                            < JsonConstants.intakeConstants.rollerMovementThreshold.in(
+                                RadiansPerSecond)),
+            "Velocity < threshold and current > threshold for at least dejam detection time")
+        .transitionTo(IntakeRollerState.dejamState);
+    IntakeRollerState.dejamState
+        .whenTimeout(JsonConstants.intakeConstants.rollerDejamTime)
+        .transitionTo(IntakeRollerState.runRollersState);
+
     this.intakeStateMachine.setState(IntakeState.waitForButtonState);
+    this.rollerStateMachine.setState(IntakeRollerState.stopRollersState);
     StateMachineDump.write("intake", this.intakeStateMachine);
+    StateMachineDump.write("intake_rollers", this.rollerStateMachine);
   }
 
   public void runRollers(AngularVelocity rollerSpeed) {
+    this.requestedRollerSpeed.mut_replace(rollerSpeed);
     rollersLeadMotorIO.controlToVelocityUnprofiled(rollerSpeed);
   }
 
   public void stopRollers() {
-    rollersLeadMotorIO.controlNeutral();
-    // We don't need to command the follower motor to stop because
-    // it is always following the lead motor
+    requestedRollerSpeed.mut_replace(RPM.zero());
   }
 
   public void setTargetPivotAngle(Angle angle) {
@@ -247,6 +302,13 @@ public void monitoredPeriodic() {
 
     intakeStateMachine.periodic();
 
+    if (!rollerTestModeManager.isInTestMode()) {
+      Logger.recordOutput("Intake/RollerState", rollerStateMachine.getCurrentState().getName());
+      rollerStateMachine.periodic();
+    } else {
+      Logger.recordOutput("Intake/RollerState", "TestMode");
+    }
+
     // Ensure that even if we accidentally command the follower motor to do something
     // it won't cause any issues because we always command it to follow the lead motor
     // at the end of the periodic
@@ -262,10 +324,6 @@ protected void controlToTargetPivotAngle() {
     pivotMotorIO.controlToPositionUnprofiled(this.targetPivotAngle);
   }
 
-  public void controlPivotMotorIOWithVoltage(Voltage v) {
-    pivotMotorIO.controlOpenLoopVoltage(v);
-  }
-
   protected void zeroPositionIfBelowZero() {
     // Commented out because the intake can actually go down to ~-8 degrees now.
     // if (pivotInputs.positionRadians < 0) {

state_machine_graphs/intake_rollers.dot

@@ -0,0 +1,30 @@
+digraph {
+
+  // Graphviz Format settings
+
+  node [
+    shape=box,
+    style=rounded
+  ];
+
+  edge [
+    fontsize=10
+  ];
+
+
+  // States
+
+  RunRollers ;
+  Dejam ;
+  StopRollers [shape=doublecircle];
+
+  // Transitions
+
+  RunRollers -> StopRollers [label="Target roller speed < movement threshold"];
+  RunRollers -> Dejam [label="Velocity < threshold and current > threshold for at least dejam detection time"];
+
+  Dejam -> RunRollers [label="timeout"];
+
+  StopRollers -> RunRollers [label="Target roller speed > movement threshold"];
+
+}

state_machine_graphs/turret.dot

@@ -18,6 +18,7 @@ digraph {
   HomingWaitForStoppingState ;
   IdleState [shape=doublecircle];
   WearInState ;
+  HomingWaitForButtonChirpState ;
   TrackHeadingState ;
   TestModeState ;
   HomingWaitForButtonState ;
@@ -34,11 +35,16 @@ digraph {
 
   WearInState -> HomingWaitForButtonState [label="When WearInState finished"];
 
+  HomingWaitForButtonChirpState -> IdleState [label="When HomingWaitForButtonChirpState finished"];
+  HomingWaitForButtonChirpState -> HomingWaitForButtonState [label="timeout"];
+  HomingWaitForButtonChirpState -> HomingWaitForMovementState [label="Robot is enabled"];
+
   TrackHeadingState -> IdleState [label="Action != TrackHeading"];
 
   TestModeState -> IdleState [label="Not in turret test mode"];
 
   HomingWaitForButtonState -> IdleState [label="When HomingWaitForButtonState finished"];
+  HomingWaitForButtonState -> HomingWaitForButtonChirpState [label="timeout"];
   HomingWaitForButtonState -> HomingWaitForMovementState [label="Robot is enabled"];
 
 }