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 295ee91b..83d58c80 100644 --- a/src/main/java/frc/robot/CoordinationLayer.java +++ b/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); 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..40b5f45c --- /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("StopRollers") { + @Override + public void periodic(StateMachine stateMachine, IntakeSubsystem world) { + world.rollersLeadMotorIO.controlNeutral(); + } + }; + + public static State runRollersState = + new State("RunRollers") { + @Override + public void periodic(StateMachine stateMachine, IntakeSubsystem world) { + world.rollersLeadMotorIO.controlToVelocityUnprofiled(world.requestedRollerSpeed); + } + }; + + public static State dejamState = + new State("Dejam") { + @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 72dcc478..7fcb7e29 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 edu.wpi.first.wpilibj.RobotController; @@ -30,8 +36,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; @@ -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(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) { 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"]; }