diff --git a/.vscode/settings.json b/.vscode/settings.json index e35b592..93573a6 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -26,5 +26,6 @@ } }, ], - "java.test.defaultConfig": "WPIlibUnitTests" + "java.test.defaultConfig": "WPIlibUnitTests", + "java.checkstyle.configuration": "${workspaceFolder}\\checks.xml" } diff --git a/build.gradle b/build.gradle index d19c7a6..d2fd751 100644 --- a/build.gradle +++ b/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 diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f5ac61d..cc8443d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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 diff --git a/src/main/java/frc/robot/systems/AutoHandlerSystem.java b/src/main/java/frc/robot/systems/AutoHandlerSystem.java new file mode 100644 index 0000000..3f132f6 --- /dev/null +++ b/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++; + } + } +} diff --git a/src/main/java/frc/robot/systems/FSMSystem.java b/src/main/java/frc/robot/systems/FSMSystem.java index 012cfd7..2d98986 100644 --- a/src/main/java/frc/robot/systems/FSMSystem.java +++ b/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; + } }