From 938576cba0f2a1180b36ba2053dd9ea3a293c8df Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Tue, 24 Mar 2026 12:23:11 -0400 Subject: [PATCH 1/2] (#151) Add intake roller state machine with dejam --- .../constants/comp/IntakeConstants.json | 20 +++++ .../java/frc/robot/CoordinationLayer.java | 9 --- .../frc/robot/constants/IntakeConstants.java | 13 +++ .../subsystems/intake/IntakeRollerState.java | 35 ++++++++ .../subsystems/intake/IntakeSubsystem.java | 79 +++++++++++++++++-- 5 files changed, 140 insertions(+), 16 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeRollerState.java diff --git a/src/main/deploy/constants/comp/IntakeConstants.json b/src/main/deploy/constants/comp/IntakeConstants.json index 284d2435..cdcda702 100644 --- a/src/main/deploy/constants/comp/IntakeConstants.json +++ b/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" diff --git a/src/main/java/frc/robot/CoordinationLayer.java b/src/main/java/frc/robot/CoordinationLayer.java index 12e48145..368274b4 100644 --- a/src/main/java/frc/robot/CoordinationLayer.java +++ b/src/main/java/frc/robot/CoordinationLayer.java @@ -737,15 +737,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); diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java index 5d970939..7d1a7c0a 100644 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ b/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); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollerState.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollerState.java new file mode 100644 index 00000000..d59beb8b --- /dev/null +++ b/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 stopRollersState = + new State() { + @Override + public void periodic(StateMachine stateMachine, IntakeSubsystem world) { + world.rollersLeadMotorIO.controlNeutral(); + } + }; + + public static State runRollersState = + new State() { + @Override + public void periodic(StateMachine stateMachine, IntakeSubsystem world) { + world.rollersLeadMotorIO.controlToVelocityUnprofiled(world.requestedRollerSpeed); + } + }; + + public static State dejamState = + new State() { + @Override + public void periodic(StateMachine stateMachine, IntakeSubsystem world) { + world.rollersLeadMotorIO.controlOpenLoopVoltage( + JsonConstants.intakeConstants.rollerDejamVoltage); + } + }; +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 4b6fd25f..a8e895a7 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/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 frc.robot.constants.JsonConstants; @@ -29,8 +35,15 @@ public class IntakeSubsystem extends MonitoredSubsystem { TestModeManager rollerTestModeManager = new TestModeManager("IntakeRollers", RollerTestMode.class); + /** Controls the pivot, test modes, homing, and general intake operation */ StateMachine 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 rollerStateMachine; + protected MotorIO pivotMotorIO; protected MotorIO rollersLeadMotorIO; protected MotorIO rollersFollowerMotorIO; @@ -47,6 +60,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) @@ -97,6 +114,13 @@ public IntakeSubsystem( IntakeState.homingDoneState) .forEach(this.intakeStateMachine::registerState); + this.rollerStateMachine = new StateMachine(this); + List.of( + IntakeRollerState.stopRollersState, + IntakeRollerState.runRollersState, + IntakeRollerState.dejamState) + .forEach(rollerStateMachine::registerState); + pivotMotorIO.setRequestUpdateFrequency(Hertz.of(1000)); // ### Test Mode Transitions @@ -167,18 +191,48 @@ 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.runRollersState); + 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); StateMachineDump.write("intake", this.intakeStateMachine); + StateMachineDump.write("intakeRollers", 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) { @@ -244,18 +298,29 @@ 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 rollersFollowerMotorIO.follow(JsonConstants.canBusAssignment.intakeRollersLeadMotorId, false); } - protected void controlToTargetPivotAngle() { - pivotMotorIO.controlToPositionUnprofiled(this.targetPivotAngle); + /** + * This method invokes the rollerStateMachine. It must be called whenever the intake rollers are + * allowed to run by the state machine (that is, all times that aren't test mode) + */ + protected void allowRollersToRun() { + rollerStateMachine.periodic(); } - public void controlPivotMotorIOWithVoltage(Voltage v) { - pivotMotorIO.controlOpenLoopVoltage(v); + protected void controlToTargetPivotAngle() { + pivotMotorIO.controlToPositionUnprofiled(this.targetPivotAngle); } protected void zeroPositionIfBelowZero() { From f32a0b31d9c671bf409d43953831a922a83a0229 Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Tue, 24 Mar 2026 13:19:06 -0400 Subject: [PATCH 2/2] (#151) Remove unused method, fix error in state machine logic, add state machine dumps --- .../subsystems/intake/IntakeRollerState.java | 6 ++-- .../subsystems/intake/IntakeSubsystem.java | 13 ++------ state_machine_graphs/intake_rollers.dot | 30 +++++++++++++++++++ state_machine_graphs/turret.dot | 6 ++++ 4 files changed, 42 insertions(+), 13 deletions(-) create mode 100644 state_machine_graphs/intake_rollers.dot diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollerState.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollerState.java index d59beb8b..40b5f45c 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollerState.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeRollerState.java @@ -9,7 +9,7 @@ public class IntakeRollerState { private IntakeRollerState() {} public static State stopRollersState = - new State() { + new State("StopRollers") { @Override public void periodic(StateMachine stateMachine, IntakeSubsystem world) { world.rollersLeadMotorIO.controlNeutral(); @@ -17,7 +17,7 @@ public void periodic(StateMachine stateMachine, IntakeSubsystem }; public static State runRollersState = - new State() { + new State("RunRollers") { @Override public void periodic(StateMachine stateMachine, IntakeSubsystem world) { world.rollersLeadMotorIO.controlToVelocityUnprofiled(world.requestedRollerSpeed); @@ -25,7 +25,7 @@ public void periodic(StateMachine stateMachine, IntakeSubsystem }; public static State dejamState = - new State() { + new State("Dejam") { @Override public void periodic(StateMachine stateMachine, IntakeSubsystem world) { world.rollersLeadMotorIO.controlOpenLoopVoltage( diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index a8e895a7..c1dc59e3 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -205,7 +205,7 @@ public IntakeSubsystem( intake.requestedRollerSpeed.lt( JsonConstants.intakeConstants.rollerMovementThreshold), "Target roller speed < movement threshold") - .transitionTo(IntakeRollerState.runRollersState); + .transitionTo(IntakeRollerState.stopRollersState); IntakeRollerState.runRollersState .when( intake -> @@ -222,8 +222,9 @@ public IntakeSubsystem( .transitionTo(IntakeRollerState.runRollersState); this.intakeStateMachine.setState(IntakeState.waitForButtonState); + this.rollerStateMachine.setState(IntakeRollerState.stopRollersState); StateMachineDump.write("intake", this.intakeStateMachine); - StateMachineDump.write("intakeRollers", this.rollerStateMachine); + StateMachineDump.write("intake_rollers", this.rollerStateMachine); } public void runRollers(AngularVelocity rollerSpeed) { @@ -311,14 +312,6 @@ public void monitoredPeriodic() { rollersFollowerMotorIO.follow(JsonConstants.canBusAssignment.intakeRollersLeadMotorId, false); } - /** - * This method invokes the rollerStateMachine. It must be called whenever the intake rollers are - * allowed to run by the state machine (that is, all times that aren't test mode) - */ - protected void allowRollersToRun() { - rollerStateMachine.periodic(); - } - protected void controlToTargetPivotAngle() { pivotMotorIO.controlToPositionUnprofiled(this.targetPivotAngle); } diff --git a/state_machine_graphs/intake_rollers.dot b/state_machine_graphs/intake_rollers.dot new file mode 100644 index 00000000..191d3df4 --- /dev/null +++ b/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"]; + +} diff --git a/state_machine_graphs/turret.dot b/state_machine_graphs/turret.dot index 082447bf..4528671a 100644 --- a/state_machine_graphs/turret.dot +++ b/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"]; }