From 105caca3bccbde3d19f29535b4f7c26f08fee19a Mon Sep 17 00:00:00 2001
From: Jonah Snider <jonah@jonahsnider.com>
Date: Sun, 6 Oct 2024 17:08:13 -0700
Subject: [PATCH] Update StateMachine to run state actions once on boot for
 initial state

---
 .../robot/util/state_machines/StateMachine.java | 17 ++++++++++++++---
 1 file changed, 14 insertions(+), 3 deletions(-)

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<S extends Enum<S>> extends LifecycleSubsystem {
   private S state;
+  private boolean isInitialized = false;
 
   /**
    * Creates a new state machine.
@@ -21,13 +22,19 @@ public abstract class StateMachine<S extends Enum<S>> 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);
   }
 }