generated from Tino-FRC-2473/FSMBotTemplate
-
Notifications
You must be signed in to change notification settings - Fork 2
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* Added code for nextState from FSM * adjustments to shooter mech * Fix Checkstyles Fixed All checkstyle issues with the code. Please make sure you have the plugin enabled when writing the code and please follow the checkstyles * fixed checkstyles again * Fixed Shooter Code for Testing * finalized shooter code * addressing svr sw mech pr adjustments * chain mech complete * restoring chain mech files * added auto functions * fixed merge conflicts * fixed checkstyle * resolving pr comments --------- Co-authored-by: Araav <nayakaraav@gmail.com> Co-authored-by: Aaron <143990364+Adbritto@users.noreply.github.com> Co-authored-by: Daiji <duchino292@student.fuhsd.org> Co-authored-by: Akshay Anand <95551900+AknA13@users.noreply.github.com>
- Loading branch information
5 people
authored
Jan 24, 2024
1 parent
f10d043
commit 25387d1
Showing
7 changed files
with
440 additions
and
48 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
173 changes: 173 additions & 0 deletions
173
src/main/java/frc/robot/systems/ClimberMechFSMLeft.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,173 @@ | ||
| package frc.robot.systems; | ||
|
|
||
| // WPILib Imports | ||
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | ||
|
|
||
| // Third party Hardware Imports | ||
| import com.revrobotics.CANSparkMax; | ||
| import com.revrobotics.SparkLimitSwitch; | ||
|
|
||
| // Robot Imports | ||
| import frc.robot.TeleopInput; | ||
| import frc.robot.HardwareMap; | ||
| import frc.robot.systems.AutoHandlerSystem.AutoFSMState; | ||
|
|
||
| public class ClimberMechFSMLeft { | ||
| /* ======================== Constants ======================== */ | ||
| // FSM state definitions | ||
| public enum ClimberMechFSMState { | ||
| IDLE_STOP, | ||
| RETRACTING | ||
| } | ||
|
|
||
| private static final float MOTOR_RUN_POWER = -0.1f; | ||
| private boolean limitPressed = false; | ||
|
|
||
|
|
||
| /* ======================== Private variables ======================== */ | ||
| private ClimberMechFSMState currentState; | ||
|
|
||
| // Hardware devices should be owned by one and only one system. They must | ||
| // be private to their owner system and may not be used elsewhere. | ||
| private CANSparkMax motor; | ||
| private SparkLimitSwitch peakLimitSwitch; | ||
|
|
||
| /* ======================== Constructor ======================== */ | ||
| /** | ||
| * Create FSMSystem and initialize to starting state. Also perform any | ||
| * one-time initialization or configuration of hardware required. Note | ||
| * the constructor is called only once when the robot boots. | ||
| */ | ||
| public ClimberMechFSMLeft() { | ||
| // Perform hardware init | ||
| motor = new CANSparkMax(HardwareMap.CAN_ID_SPARK_LEFT_CLIMBER_MOTOR, | ||
| CANSparkMax.MotorType.kBrushless); | ||
| motor.setIdleMode(CANSparkMax.IdleMode.kBrake); | ||
|
|
||
| peakLimitSwitch = motor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyClosed); | ||
| peakLimitSwitch.enableLimitSwitch(false); | ||
|
|
||
| // Reset state machine | ||
| reset(); | ||
| } | ||
|
|
||
| /* ======================== Public methods ======================== */ | ||
| /** | ||
| * Return current FSM state. | ||
| * @return Current FSM state | ||
| */ | ||
| public ClimberMechFSMState getCurrentState() { | ||
| return currentState; | ||
| } | ||
| /** | ||
| * Reset this system to its start state. This may be called from mode init | ||
| * when the robot is enabled. | ||
| * | ||
| * Note this is distinct from the one-time initialization in the constructor | ||
| * as it may be called multiple times in a boot cycle, | ||
| * Ex. if the robot is enabled, disabled, then reenabled. | ||
| */ | ||
| public void reset() { | ||
| currentState = ClimberMechFSMState.IDLE_STOP; | ||
|
|
||
| // Call one tick of update to ensure outputs reflect start state | ||
| update(null); | ||
| } | ||
|
|
||
| /** | ||
| * Update FSM based on new inputs. This function only calls the FSM state | ||
| * specific handlers. | ||
| * @param input Global TeleopInput if robot in teleop mode or null if | ||
| * the robot is in autonomous mode. | ||
| */ | ||
| public void update(TeleopInput input) { | ||
| if (peakLimitSwitch.isPressed()) { | ||
| limitPressed = true; | ||
| } | ||
|
|
||
| if (input == null) { | ||
| return; | ||
| } | ||
|
|
||
| switch (currentState) { | ||
| case IDLE_STOP: | ||
| handleIdleState(input); | ||
| break; | ||
| case RETRACTING: | ||
| handleRetractingState(input); | ||
| break; | ||
| default: | ||
| throw new IllegalStateException("Invalid state: " + currentState.toString()); | ||
| } | ||
| SmartDashboard.putString("Current State", currentState.toString()); | ||
| SmartDashboard.putBoolean("Bottom Limit Switch Pressed", peakLimitSwitchHit()); | ||
| SmartDashboard.putBoolean("Retract Button Pressed", input.isRetractClimberButtonPressed()); | ||
| currentState = nextState(input); | ||
| } | ||
|
|
||
| /** | ||
| * Performs specific action based on the autoState passed in. | ||
| * @param autoState autoState that the subsystem executes. | ||
| * @return if the action carried out in this state has finished executing | ||
| */ | ||
| public boolean updateAutonomous(AutoFSMState autoState) { | ||
| switch (autoState) { | ||
| default: | ||
| return true; | ||
| } | ||
| } | ||
|
|
||
| /* ======================== Private methods ======================== */ | ||
| /** | ||
| * Decide the next state to transition to. This is a function of the inputs | ||
| * and the current state of this FSM. This method should not have any side | ||
| * effects on outputs. In other words, this method should only read or get | ||
| * values to decide what state to go to. | ||
| * @param input Global TeleopInput if robot in teleop mode or null if | ||
| * the robot is in autonomous mode. | ||
| * @return FSM state for the next iteration | ||
| */ | ||
| private ClimberMechFSMState nextState(TeleopInput input) { | ||
| switch (currentState) { | ||
| case IDLE_STOP: | ||
| if (input.isRetractClimberButtonPressed() && !peakLimitSwitchHit()) { | ||
| return ClimberMechFSMState.RETRACTING; | ||
| } else { | ||
| return ClimberMechFSMState.IDLE_STOP; | ||
| } | ||
| case RETRACTING: | ||
| if (input.isRetractClimberButtonPressed() && !peakLimitSwitchHit()) { | ||
| return ClimberMechFSMState.RETRACTING; | ||
| } else { | ||
| return ClimberMechFSMState.IDLE_STOP; | ||
| } | ||
| default: | ||
| throw new IllegalStateException("Invalid state: " + currentState.toString()); | ||
| } | ||
| } | ||
|
|
||
| /* ------------------------ FSM state handlers ------------------------ */ | ||
| /** | ||
| * Handle behavior in START_STATE. | ||
| * @param input Global TeleopInput if robot in teleop mode or null if | ||
| * the robot is in autonomous mode. | ||
| */ | ||
| private void handleIdleState(TeleopInput input) { | ||
| motor.set(0); | ||
| } | ||
| /** | ||
| * Handle behavior in RETRACTING state. | ||
| * @param input Global TeleopInput if robot in teleop mode or null if | ||
| * the robot is in autonomous mode. | ||
| */ | ||
| private void handleRetractingState(TeleopInput input) { | ||
| motor.set(-MOTOR_RUN_POWER); | ||
| } | ||
|
|
||
| private boolean peakLimitSwitchHit() { | ||
| return limitPressed; | ||
| } | ||
|
|
||
|
|
||
|
|
||
| } |
Oops, something went wrong.