Auto handler

.vscode/settings.json

@@ -26,5 +26,6 @@
       }
     },
   ],
-  "java.test.defaultConfig": "WPIlibUnitTests"
+  "java.test.defaultConfig": "WPIlibUnitTests",
+  "java.checkstyle.configuration": "${workspaceFolder}\\checks.xml"
 }

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1"
+    id "edu.wpi.first.GradleRIO" version "2023.4.3"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

src/main/java/frc/robot/Robot.java

@@ -8,6 +8,8 @@
 
 // Systems
 import frc.robot.systems.FSMSystem;
+import frc.robot.systems.AutoHandlerSystem;
+import frc.robot.systems.AutoHandlerSystem.AutoPath;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -17,7 +19,11 @@ public class Robot extends TimedRobot {
 	private TeleopInput input;
 
 	// Systems
-	private FSMSystem fsmSystem;
+	private FSMSystem subSystem1;
+	private FSMSystem subSystem2;
+	private FSMSystem subSystem3;
+
+	private AutoHandlerSystem autoHandler;
 
 	/**
 	 * This function is run when the robot is first started up and should be used for any
@@ -29,29 +35,36 @@ public void robotInit() {
 		input = new TeleopInput();
 
 		// Instantiate all systems here
-		fsmSystem = new FSMSystem();
+		subSystem1 = new FSMSystem();
+		subSystem2 = new FSMSystem();
+		subSystem3 = new FSMSystem();
+		autoHandler = new AutoHandlerSystem(subSystem1, subSystem2, subSystem3);
 	}
 
 	@Override
 	public void autonomousInit() {
 		System.out.println("-------- Autonomous Init --------");
-		fsmSystem.reset();
+		autoHandler.reset(AutoPath.PATH1);
 	}
 
 	@Override
 	public void autonomousPeriodic() {
-		fsmSystem.update(null);
+		autoHandler.update();
 	}
 
 	@Override
 	public void teleopInit() {
 		System.out.println("-------- Teleop Init --------");
-		fsmSystem.reset();
+		subSystem1.reset();
+		subSystem2.reset();
+		subSystem3.reset();
 	}
 
 	@Override
 	public void teleopPeriodic() {
-		fsmSystem.update(input);
+		subSystem1.update(input);
+		subSystem2.update(input);
+		subSystem3.update(input);
 	}
 
 	@Override

src/main/java/frc/robot/systems/AutoHandlerSystem.java

@@ -0,0 +1,118 @@
+package frc.robot.systems;
+
+public class AutoHandlerSystem {
+	/* ======================== Constants ======================== */
+	// Auto FSM state definitions
+	public enum AutoFSMState {
+		STATE1,
+		STATE2,
+		STATE3
+	}
+	public enum AutoPath {
+		PATH1,
+		PATH2,
+		PATH3
+	}
+
+	/* ======================== Private variables ======================== */
+	//Contains the sequential list of states in the current auto path that must be executed
+	private AutoFSMState[] currentStateList;
+
+	//The index in the currentStateList where the currentState is at
+	private int currentStateIndex;
+
+	//FSM Systems that the autoHandlerFSM uses
+	private FSMSystem subsystem1;
+	private FSMSystem subsystem2;
+	private FSMSystem subsystem3;
+
+	//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.
+	 * Initializes any subsystems such as driveFSM, armFSM, ect.
+	 * @param fsm1 the first subsystem that the auto handler will call functions on
+	 * @param fsm2 the second subsystem that the auto handler will call functions on
+	 * @param fsm3 the third subsystem that the auto handler will call functions on
+	 */
+	public AutoHandlerSystem(FSMSystem fsm1, FSMSystem fsm2, FSMSystem fsm3) {
+		subsystem1 = fsm1;
+		subsystem2 = fsm2;
+		subsystem3 = fsm3;
+	}
+
+	/* ======================== Public methods ======================== */
+	/**
+	 * Return current FSM state.
+	 * @return Current FSM state
+	 */
+	public AutoFSMState getCurrentState() {
+		return currentStateList[currentStateIndex];
+	}
+
+	/**
+	 * Reset this system to its start state. This may be called from mode init
+	 * when the robot is enabled.
+	 *
+	 * Note this is distinct from the one-time initialization in the constructor
+	 * as it may be called multiple times in a boot cycle,
+	 * Ex. if the robot is enabled, disabled, then reenabled.
+	 * @param path the auto path to be executed
+	 */
+	public void reset(AutoPath path) {
+		subsystem1.reset();
+		subsystem2.reset();
+		subsystem3.reset();
+
+		currentStateIndex = 0;
+		if (path == AutoPath.PATH1) {
+			currentStateList = PATH1;
+		} else if (path == AutoPath.PATH2) {
+			currentStateList = PATH2;
+		} else if (path == AutoPath.PATH3) {
+			currentStateList = PATH3;
+		}
+	}
+
+	/**
+	 * This function runs the auto's current state.
+	 */
+	public void update() {
+		if (currentStateIndex >= currentStateList.length) {
+			return;
+		}
+
+		boolean isCurrentStateFinished;
+		System.out.println("In State: " + getCurrentState());
+		switch (getCurrentState()) {
+			case STATE1:
+				isCurrentStateFinished = subsystem1.updateAutonomous(AutoFSMState.STATE1)
+					&& subsystem2.updateAutonomous(AutoFSMState.STATE1)
+					&& subsystem3.updateAutonomous(AutoFSMState.STATE1);
+				break;
+			case STATE2:
+				isCurrentStateFinished = subsystem1.updateAutonomous(AutoFSMState.STATE2)
+					&& subsystem2.updateAutonomous(AutoFSMState.STATE2)
+					&& subsystem3.updateAutonomous(AutoFSMState.STATE2);
+				break;
+			case STATE3:
+				isCurrentStateFinished = subsystem1.updateAutonomous(AutoFSMState.STATE3)
+					&& subsystem2.updateAutonomous(AutoFSMState.STATE3)
+					&& subsystem3.updateAutonomous(AutoFSMState.STATE3);
+				break;
+			default:
+				throw new IllegalStateException("Invalid state: " + getCurrentState().toString());
+		}
+		if (isCurrentStateFinished) {
+			currentStateIndex++;
+		}
+	}
+}

src/main/java/frc/robot/systems/FSMSystem.java

@@ -8,6 +8,7 @@
 // Robot Imports
 import frc.robot.TeleopInput;
 import frc.robot.HardwareMap;
+import frc.robot.systems.AutoHandlerSystem.AutoFSMState;
 
 public class FSMSystem {
 	/* ======================== Constants ======================== */
@@ -63,6 +64,7 @@ public void reset() {
 		// Call one tick of update to ensure outputs reflect start state
 		update(null);
 	}
+
 	/**
 	 * Update FSM based on new inputs. This function only calls the FSM state
 	 * specific handlers.
@@ -85,6 +87,24 @@ public void update(TeleopInput input) {
 		currentState = nextState(input);
 	}
 
+	/**
+	 * Performs specific action based on the autoState passed in.
+	 * @param autoState autoState that the subsystem executes.
+	 * @return if the action carried out in this state has finished executing
+	 */
+	public boolean updateAutonomous(AutoFSMState autoState) {
+		switch (autoState) {
+			case STATE1:
+				return handleAutoState1();
+			case STATE2:
+				return handleAutoState2();
+			case STATE3:
+				return handleAutoState3();
+			default:
+				return true;
+		}
+	}
+
 	/* ======================== Private methods ======================== */
 	/**
 	 * Decide the next state to transition to. This is a function of the inputs
@@ -129,4 +149,28 @@ private void handleStartState(TeleopInput input) {
 	private void handleOtherState(TeleopInput input) {
 		exampleMotor.set(MOTOR_RUN_POWER);
 	}
+
+	/**
+	 * Performs action for auto STATE1.
+	 * @return if the action carried out has finished executing
+	 */
+	private boolean handleAutoState1() {
+		return true;
+	}
+
+	/**
+	 * Performs action for auto STATE2.
+	 * @return if the action carried out has finished executing
+	 */
+	private boolean handleAutoState2() {
+		return true;
+	}
+
+	/**
+	 * Performs action for auto STATE3.
+	 * @return if the action carried out has finished executing
+	 */
+	private boolean handleAutoState3() {
+		return true;
+	}
 }