diff --git a/src/main/java/frc/robot/util/state_machines/StateMachine.java b/src/main/java/frc/robot/util/state_machines/StateMachine.java index 8a72d60..16b8625 100644 --- a/src/main/java/frc/robot/util/state_machines/StateMachine.java +++ b/src/main/java/frc/robot/util/state_machines/StateMachine.java @@ -11,6 +11,7 @@ /** A state machine backed by {@link LifecycleSubsystem}. */ public abstract class StateMachine> extends LifecycleSubsystem { private S state; + private boolean isInitialized = false; /** * Creates a new state machine. @@ -21,13 +22,19 @@ public abstract class StateMachine> extends LifecycleSubsystem protected StateMachine(SubsystemPriority priority, S initialState) { super(priority); state = initialState; - // Log the state once on boot - DogLog.log(subsystemName + "/State", state); } /** Processes collecting inputs, state transitions, and state actions. */ @Override public void robotPeriodic() { + // The first time the robot boots up, we need to set the state from null to the initial state + // This also gives us an opportunity to run the state actions for the initial state + // Think of it as transitioning from the robot being off to initialState + if (!isInitialized) { + doTransition(); + isInitialized = true; + } + collectInputs(); setStateFromRequest(getNextState(state)); @@ -101,9 +108,13 @@ protected void setStateFromRequest(S requestedState) { } state = requestedState; + doTransition(); + } + /** Run side effects that occur when a state transition happens. */ + private void doTransition() { DogLog.log(subsystemName + "/State", state); - afterTransition(requestedState); + afterTransition(state); } }