diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index dbd33aa..50292bb 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,7 +9,7 @@ // Systems import frc.robot.systems.FSMSystem; import frc.robot.systems.AutoHandlerSystem; -import frc.robot.systems.AutoHandlerSystem.AutoFSMState; +import frc.robot.systems.AutoHandlerSystem.AutoPathIndex; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -18,16 +18,6 @@ public class Robot extends TimedRobot { private TeleopInput input; - //Predefined auto paths - private static final AutoFSMState[] PATH1 = new AutoFSMState[]{ - AutoFSMState.STATE1, AutoFSMState.STATE2, AutoFSMState.STATE3}; - - private static final AutoFSMState[] PATH2 = new AutoFSMState[]{ - AutoFSMState.STATE3, AutoFSMState.STATE2, AutoFSMState.STATE1}; - - private static final AutoFSMState[] PATH3 = new AutoFSMState[]{ - AutoFSMState.STATE1, AutoFSMState.STATE3, AutoFSMState.STATE2}; - // Systems private FSMSystem fsmSystem; private AutoHandlerSystem autoHandler; @@ -49,7 +39,7 @@ public void robotInit() { @Override public void autonomousInit() { System.out.println("-------- Autonomous Init --------"); - autoHandler.reset(PATH1); + autoHandler.reset(AutoPathIndex.PATH1); } @Override diff --git a/src/main/java/frc/robot/systems/AutoHandlerSystem.java b/src/main/java/frc/robot/systems/AutoHandlerSystem.java index 361c53b..a1a801d 100644 --- a/src/main/java/frc/robot/systems/AutoHandlerSystem.java +++ b/src/main/java/frc/robot/systems/AutoHandlerSystem.java @@ -8,6 +8,11 @@ public enum AutoFSMState { STATE2, STATE3 } + public enum AutoPathIndex { + PATH1, + PATH2, + PATH3 + } /* ======================== Private variables ======================== */ //Contains the sequential list of states in the current auto path that must be executed @@ -22,6 +27,15 @@ public enum AutoFSMState { //FSM Systems that the autoHandlerFSM uses private FSMSystem subsystem1; + //Predefined auto paths + private static final AutoFSMState[] PATH1 = new AutoFSMState[]{ + AutoFSMState.STATE1, AutoFSMState.STATE2, AutoFSMState.STATE3}; + + private static final AutoFSMState[] PATH2 = new AutoFSMState[]{ + AutoFSMState.STATE3, AutoFSMState.STATE2, AutoFSMState.STATE1}; + + private static final AutoFSMState[] PATH3 = new AutoFSMState[]{ + AutoFSMState.STATE1, AutoFSMState.STATE3, AutoFSMState.STATE2}; /* ======================== Constructor ======================== */ /** * Create FSMSystem and initialize to starting state. @@ -50,10 +64,16 @@ public AutoFSMState getCurrentState() { * Ex. if the robot is enabled, disabled, then reenabled. * @param path the auto path to be executed */ - public void reset(AutoFSMState[] path) { + public void reset(AutoPathIndex path) { currentStateIndex = 0; isCurrentStateFinished = false; - currentStateList = path; + if (path == AutoPathIndex.PATH1) { + currentStateList = PATH1; + } else if (path == AutoPathIndex.PATH2) { + currentStateList = PATH2; + } else if (path == AutoPathIndex.PATH3) { + currentStateList = PATH3; + } } /**