Updates StateSubsystemBase to match testing 12/10/24

Team3128 · Dec 11, 2024 · ddb31f4 · ddb31f4
1 parent bcde06e
commit ddb31f4
Showing 1 changed file with 32 additions and 42 deletions.
diff --git a/src/main/java/common/core/subsystems/StateSubsystemBase.java b/src/main/java/common/core/subsystems/StateSubsystemBase.java
@@ -16,19 +16,18 @@
 
 public abstract class StateSubsystemBase<S extends Enum<S>> extends SubsystemBase{
 
-    private S state;
-    private final Class<S> enumType;
-    private final List<StandardizedSubsystem> subsystems;
-    private TransitionManager<S> transitionManager;
-    private Transition<S> requTransition = null;
-    private Transition<S> currTransition = null;
+    protected S state;
+    protected final Class<S> enumType;
+    protected final List<StandardizedSubsystem> subsystems;
+    protected TransitionManager<S> transitionManager;
+    private Transition<S> lastScheduledTransition = null;
 
 
-    public StateSubsystemBase(S initialState, Class<S> enumType) {
+    public StateSubsystemBase(S initialState, Class<S> enumType, TransitionManager<S> transitionManager) {
         state = initialState;
         this.enumType = enumType;
         this.subsystems = new LinkedList<>();
-        transitionManager = new TransitionManager<>(enumType);
+        this.transitionManager = transitionManager;
         initStateTracker();
         registerTransitions();
     }
@@ -48,26 +47,32 @@ public void initStateTracker() {
     public abstract void registerTransitions();
 
     public boolean setState(S nextState) {
-        Transition<S> transition = transitionManager.getTransition(getState(), nextState);
-
-        Log.info("State", "Check1");
+        Log.info("COMMANDED", state.name() + " -> " + nextState.name());
         // if not the same state
-        if(!stateEquals(nextState)) requTransition = transition;
-        else return false;
+        if(stateEquals(nextState)) {
+            Log.info("COMMANDED", "State already set to " + nextState.name());
+            return false;
+        }
 
+        Transition<S> transition = transitionManager.getTransition(getState(), nextState);
         // if invalid trnasition
-        if(transition == null) return false;
-
-        // if not transitioning
-        Log.info("State", "Check2");
-        if(!isTransitioning()) {
-            Log.info("State", "Transitioning...");
-            currTransition = transition;
-            currTransition.execute();
-            return true;
+        if(transition == null) {
+            Log.info("TRANSITION", "State transition null");
+            return false;
         }
+        Log.info("TRANSITION", transition.toString());
 
-        return false;
+        if(isTransitioning()) {
+            Log.info("TRANSITION", "Already transitioning, procceeding to override");
+            lastScheduledTransition.cancel();
+        }
+
+        state = nextState;
+        lastScheduledTransition = transition;
+        lastScheduledTransition.execute();
+
+        if(isTransitioning()) Log.info("State", "Transitioning...");
+        return true;
     }
 
     public Command setStateAsCommand(S nextState) {
@@ -83,20 +88,12 @@ public boolean stateEquals(S other) {
         return state.name().equals(other.name());
     }
 
-    public Transition<S> getRequTransition() {
-        return requTransition;
-    }
-
-    public Transition<S> getCurrTransition() {
-        return currTransition;
-    }
-
     public boolean isTransitioning() {
-        return currTransition != null;
+        return lastScheduledTransition != null && lastScheduledTransition.isRunning();
     }
 
     public Command stop() {
-        currTransition.cancel();
+        lastScheduledTransition.cancel();
         Command stop = new InstantCommand();
         for(StandardizedSubsystem subsystem : subsystems) {
             stop.andThen(subsystem.stop());
@@ -105,7 +102,7 @@ public Command stop() {
     }
 
     public Command reset() {
-        currTransition.cancel();
+        lastScheduledTransition.cancel();
         Command reset = new InstantCommand();
         for(StandardizedSubsystem subsystem : subsystems) {
             if(subsystem instanceof PositionSubsystemBase) {
@@ -119,16 +116,9 @@ public TransitionManager<S> getTransitionManager() {
         return transitionManager;
     }
 
-    @Override
-    public void periodic() {
-        if (isTransitioning() && currTransition.isFinished()) {
-            currTransition = null;
-        }
-    }
-
     @Override
     public void initSendable(SendableBuilder builder) {
-        builder.setSmartDashboardType("PIDController");
+        builder.setSmartDashboardType(getName());
         for(S state : enumType.getEnumConstants()) {
             builder.addBooleanProperty(state.name(), ()-> stateEquals(state), (boolean set)->{
                 if(set) setState(state);