diff --git a/Lessons (Software)/StateMachine.md b/Lessons (Software)/StateMachine.md index e3be911c..f6f40aab 100644 --- a/Lessons (Software)/StateMachine.md +++ b/Lessons (Software)/StateMachine.md @@ -134,3 +134,301 @@ This diagram shows a real-world application used by Team 3255. 7. **Assign Buttons:** Assign buttons to trigger state transitions. 8. **Identify Conditions:** Identify transition conditions, including inputs. 9. **Create Diagram:** Create a diagram of the states and transitions to show how the completion of one command leads to the next. + +--- + +## State Transitions + +State transitions are the core mechanism that moves the robot from one state to another. Understanding how to properly implement transitions is critical to a functioning state machine. + +### How to Implement State Transitions + +State transitions typically happen in one of two ways: + +1. **Button/Input-Triggered Transitions:** The operator or driver presses a button to request a state change +2. **Condition-Based Transitions:** The robot automatically transitions when certain conditions are met (sensors, timers, command completion) + +### Transition Logic Flow + +```java +// Example: Setting a target state via button press +if (operatorController.getAButtonPressed()) { + currentTargetState = States.INTAKE; +} + +// Check if we can transition to the target state +if (currentTargetState != currentState) { + if (canTransitionTo(currentTargetState)) { + currentState = currentTargetState; + // Execute the command for the new state + } +} +``` + +### Best Practices for Transitions + +* **Validate Transitions:** Always check that preconditions are met before transitioning +* **Avoid Direct Jumps:** Some states may need intermediate states (e.g., can't go from "Intake" to "Shoot" without "Store") +* **Use Target States:** Allow operators to request a state, but only transition when safe/ready +* **Sensor Feedback:** Use sensor data to confirm when a state is complete before transitioning + +--- + +## Commands in State Machines + +Commands are the building blocks that execute actions for each state. Every state typically has an associated command that controls what the robot does while in that state. + +### Command Structure + +WPILib commands have four key methods that you need to understand: + +#### `initialize()` + +The `initialize()` method runs **once** when the command starts. + +**What to put in `initialize()`:** + +* **Start motors** - Set motor speeds or positions for this state +* Reset/zero encoders or timers if needed +* Record starting sensor values +* Log that the command has started +* Set any flags or state variables + +**Example:** + +```java +@Override +public void initialize() { + // Reset the intake motor position + subIntake.resetEncoder(); + + // Log the command start + System.out.println("Intake command initialized"); + + // Set initial state + hasDetectedGamePiece = false; + + // Start the motors for this state + subIntake.setSpeed(constIntake.INTAKE_SPEED); +} +``` + +!!! important "State Machine Pattern" + In a state machine, `initialize()` should set up motors for the new state. The previous state's `end()` does NOT stop motors - control transfers directly to the new state's `initialize()`. + +#### `execute()` + +The `execute()` method runs **continuously** (every 20ms) while the command is active. + +**What to put in `execute()` for state machines:** + +* Monitor sensor values +* Update control loops based on feedback +* Adjust motor speeds if needed (fine-tuning) +* Perform continuous calculations + +!!! note + In many state machine commands, `execute()` can be empty or minimal since motors are set up in `initialize()` and run until the state changes. + +**Example:** + +```java +@Override +public void execute() { + // Monitor for game piece detection + if (subIntake.hasGamePiece()) { + hasDetectedGamePiece = true; + } + + // Motors are already running from initialize() + // Only adjust if needed based on feedback +} +``` + +#### `end(boolean interrupted)` + +The `end()` method runs **once** when the command finishes (either normally or when interrupted). + +!!! warning "State Machine Exception" + In a state machine, **DO NOT** use the `end()` method to stop motors. The next state's command will take control of the motors. Stopping motors in `end()` can cause unwanted behavior during state transitions. + +**What to put in `end()` for state machines:** + +* Log completion status (optional) +* Clean up non-motor resources if needed +* Reset temporary flags (if necessary) + +**What NOT to put in `end()` for state machines:** + +* ❌ Motor stop commands +* ❌ Setting motor speeds to zero +* ❌ Changing motor states + +**Example:** + +```java +@Override +public void end(boolean interrupted) { + // Log completion (optional) + if (interrupted) { + System.out.println("Intake command was interrupted"); + } else { + System.out.println("Intake command completed successfully"); + } + + // DO NOT stop motors here in a state machine + // The next state's initialize() will take control +} +``` + +!!! note + For non-state machine commands (like standalone commands), you should stop motors in `end()`. But in a state machine, motor control is handled by state transitions. + +#### `isFinished()` + +The `isFinished()` method runs **continuously** and determines when the command should end. + +**When to return `true`:** + +* When using **sensor-based completion**: Check if sensor conditions are met +* When using **time-based completion**: Check if enough time has elapsed +* When using **position-based completion**: Check if the mechanism reached the target position + +**When to return `false`:** + +* When the command should run indefinitely (until interrupted) +* When the completion condition hasn't been met yet + +**Example 1 - Sensor Check (Preferred for most cases):** + +```java +@Override +public boolean isFinished() { + // Finish when we detect a game piece + return subIntake.hasGamePiece(); +} +``` + +**Example 2 - Time-Based:** + +```java +private Timer timer = new Timer(); + +@Override +public void initialize() { + timer.restart(); +} + +@Override +public boolean isFinished() { + // Finish after 2 seconds + return timer.hasElapsed(2.0); +} +``` + +**Example 3 - Position-Based:** + +```java +@Override +public boolean isFinished() { + // Finish when elevator reaches target position + return Math.abs(subElevator.getPosition() - targetPosition) < constElevator.POSITION_TOLERANCE; +} +``` + +**Example 4 - Never Finish (Manual Control):** + +```java +@Override +public boolean isFinished() { + // This command runs until interrupted by another command + return false; +} +``` + +### Sensor Checks vs. Returning True + +!!! tip "When to use sensor checks vs. always returning true" + + **Use sensor checks (`isFinished()` returns sensor value):** + + * For autonomous actions that need confirmation (intaking, scoring, etc.) + * When you want the robot to automatically proceed to the next state + * When safety requires verification before continuing + + **Always return true:** + + * For instant state changes (prep commands that just set positions) + * When the command completes immediately in `initialize()` + + **Always return false:** + + * For manual control commands (teleoperated driving) + * For commands that should run until interrupted by the operator + +### Command Lifecycle Example + +Here's a complete example showing all four methods working together in a state machine: + +```java +public class IntakeCommand extends Command { + private final Intake subIntake; + private final Timer timeout = new Timer(); + + public IntakeCommand(Intake intake) { + this.subIntake = intake; + addRequirements(intake); + } + + @Override + public void initialize() { + // Start fresh - set up for this state + subIntake.resetSensors(); + timeout.restart(); + System.out.println("Starting intake..."); + + // Start the motors for this state + subIntake.setSpeed(constIntake.INTAKE_SPEED); + } + + @Override + public void execute() { + // Continuously monitor and maintain state + // Motor speed is already set in initialize() + + // Could adjust based on feedback if needed + // subIntake.adjustSpeed(...); + } + + @Override + public void end(boolean interrupted) { + // Log completion (optional) + if (interrupted) { + System.out.println("Intake interrupted"); + } else { + System.out.println("Game piece acquired!"); + } + + // DO NOT stop motors here + // The next state will take control + } + + @Override + public boolean isFinished() { + // Finish when sensor detects game piece OR timeout + return subIntake.hasGamePiece() || timeout.hasElapsed(3.0); + } +} +``` + +--- + +## Putting It All Together + +When building a state machine with commands: + +1. **Define your states** based on what the robot needs to accomplish +2. **Create commands** for each state with proper `initialize()`, `execute()`, `end()`, and `isFinished()` methods +3. **Implement transitions** between states based on button inputs and sensor feedback +4. **Use `isFinished()`** to determine when commands complete and trigger automatic transitions +5. **Test thoroughly** to ensure smooth transitions and proper command behavior