From d31423427c5b0cad60038223a5e5d51091152451 Mon Sep 17 00:00:00 2001 From: Ryan Cahoon Date: Fri, 13 Sep 2024 04:49:11 -0700 Subject: [PATCH] Pre-initialize autonomous Procedure --- .../AutonomousModeStateMachine.java | 92 ++++++ .../com/team766/hal/GenericRobotMain3.java | 55 +--- src/test/java/com/team766/TestCase3.java | 2 +- .../AutonomousModeStateMachineTest.java | 286 ++++++++++++++++++ 4 files changed, 391 insertions(+), 44 deletions(-) create mode 100644 src/main/java/com/team766/framework3/AutonomousModeStateMachine.java create mode 100644 src/test/java/com/team766/framework3/AutonomousModeStateMachineTest.java diff --git a/src/main/java/com/team766/framework3/AutonomousModeStateMachine.java b/src/main/java/com/team766/framework3/AutonomousModeStateMachine.java new file mode 100644 index 00000000..f73f5447 --- /dev/null +++ b/src/main/java/com/team766/framework3/AutonomousModeStateMachine.java @@ -0,0 +1,92 @@ +package com.team766.framework3; + +import com.team766.logging.Category; +import com.team766.logging.Logger; +import com.team766.logging.Severity; +import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.Supplier; + +public class AutonomousModeStateMachine { + private enum AutonomousState { + /** + * m_autonomous has not been started yet. + * It can be scheduled the next time autonomousInit is called. + */ + NEW, + /** + * m_autonomous is currently running. + */ + SCHEDULED, + /** + * m_autonomous has finished, has been canceled, or is null. + * A new instance of the autonomous Command needs to be created before + * autonomous mode can be enabled. + */ + INVALID, + } + + private final Supplier selector; + private AutonomousMode m_autonMode = null; + private Command m_autonomous = null; + private AutonomousState m_autonState = AutonomousState.INVALID; + + public AutonomousModeStateMachine(Supplier selector) { + this.selector = selector; + } + + public void stopAutonomousMode(final String reason) { + if (m_autonState == AutonomousState.SCHEDULED) { + m_autonomous.cancel(); + m_autonState = AutonomousState.INVALID; + Logger.get(Category.AUTONOMOUS) + .logRaw(Severity.INFO, "Resetting autonomus procedure from " + reason); + } + } + + private void refreshAutonomousMode() { + final AutonomousMode autonomousMode = selector.get(); + if (m_autonMode != autonomousMode) { + stopAutonomousMode("selection of new autonomous mode " + autonomousMode); + m_autonState = AutonomousState.INVALID; + } + if (m_autonState == AutonomousState.INVALID && autonomousMode != null) { + m_autonomous = autonomousMode.instantiate(); + m_autonMode = autonomousMode; + m_autonState = AutonomousState.NEW; + Logger.get(Category.AUTONOMOUS) + .logRaw( + Severity.INFO, + "Initialized new autonomus procedure " + m_autonomous.getName()); + } + } + + public void startAutonomousMode() { + refreshAutonomousMode(); + switch (m_autonState) { + case INVALID -> { + Logger.get(Category.AUTONOMOUS) + .logRaw(Severity.WARNING, "No autonomous mode selected"); + } + case SCHEDULED -> { + Logger.get(Category.AUTONOMOUS) + .logRaw( + Severity.INFO, + "Continuing previous autonomus procedure " + + m_autonomous.getName()); + } + case NEW -> { + m_autonomous.schedule(); + m_autonState = AutonomousState.SCHEDULED; + Logger.get(Category.AUTONOMOUS) + .logRaw( + Severity.INFO, + "Starting new autonomus procedure " + m_autonomous.getName()); + } + } + } + + public void reinitializeAutonomousMode(final String reason) { + stopAutonomousMode(reason); + refreshAutonomousMode(); + } +} diff --git a/src/main/java/com/team766/hal/GenericRobotMain3.java b/src/main/java/com/team766/hal/GenericRobotMain3.java index 391e69a1..136aaac5 100755 --- a/src/main/java/com/team766/hal/GenericRobotMain3.java +++ b/src/main/java/com/team766/hal/GenericRobotMain3.java @@ -1,13 +1,10 @@ package com.team766.hal; -import com.team766.framework3.AutonomousMode; +import com.team766.framework3.AutonomousModeStateMachine; import com.team766.framework3.RuleEngine; import com.team766.framework3.SchedulerMonitor; import com.team766.framework3.SchedulerUtils; import com.team766.library.RateLimiter; -import com.team766.logging.Category; -import com.team766.logging.Logger; -import com.team766.logging.Severity; import com.team766.web.AutonomousSelector; import com.team766.web.ConfigUI; import com.team766.web.Dashboard; @@ -15,7 +12,6 @@ import com.team766.web.LogViewer; import com.team766.web.ReadLogs; import com.team766.web.WebServer; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; // Team 766 - Robot Interface Base class @@ -26,9 +22,7 @@ public final class GenericRobotMain3 implements GenericRobotMainBase { private RuleEngine m_lights; private WebServer m_webServer; - private AutonomousSelector m_autonSelector; - private AutonomousMode m_autonMode = null; - private Command m_autonomous = null; + private AutonomousModeStateMachine autonomous; // Reset the autonomous routine if the robot is disabled for more than this // number of seconds. @@ -46,14 +40,15 @@ public GenericRobotMain3(RobotConfigurator3 configurator) { SchedulerMonitor.start(); this.configurator = configurator; - m_autonSelector = new AutonomousSelector<>(configurator.getAutonomousModes()); + var autonSelector = new AutonomousSelector<>(configurator.getAutonomousModes()); + autonomous = new AutonomousModeStateMachine(autonSelector::getSelectedAutonMode); m_webServer = new WebServer(); m_webServer.addHandler(new Dashboard()); - m_webServer.addHandler(new DriverInterface(m_autonSelector)); + m_webServer.addHandler(new DriverInterface(autonSelector)); m_webServer.addHandler(new ConfigUI()); m_webServer.addHandler(new LogViewer()); m_webServer.addHandler(new ReadLogs()); - m_webServer.addHandler(m_autonSelector); + m_webServer.addHandler(autonSelector); m_webServer.start(); } @@ -89,14 +84,14 @@ public void disabledPeriodic() { // when the communications are restored, we want to continue executing // the routine that was interrupted, since it has knowledge of where the // robot is on the field, the state of the robot's mechanisms, etc. - // Thus, we set a threshold on the amount of time spent in autonomous of + // Thus, we set a threshold on the amount of time spent in disabled of // 10 seconds. It is almost certain that it will take longer than 10 // seconds to reset the field if a match is to be replayed, but it is // also almost certain that a communication drop will be much shorter // than 10 seconds. double timeInState = RobotProvider.instance.getClock().getTime() - m_disabledModeStartTime; if (timeInState > RESET_IN_DISABLED_PERIOD) { - resetAutonomousMode("time in disabled mode"); + autonomous.reinitializeAutonomousMode("time in disabled mode"); } CommandScheduler.getInstance().run(); if (m_lights != null && m_lightUpdateLimiter.next()) { @@ -105,42 +100,20 @@ public void disabledPeriodic() { } public void resetAutonomousMode(final String reason) { - if (m_autonomous != null) { - m_autonomous.cancel(); - m_autonomous = null; - m_autonMode = null; - Logger.get(Category.AUTONOMOUS) - .logRaw(Severity.INFO, "Resetting autonomus procedure from " + reason); - } + autonomous.reinitializeAutonomousMode(reason); } public void autonomousInit() { faultInAutoInit = true; - if (m_autonomous != null) { - Logger.get(Category.AUTONOMOUS) - .logRaw( - Severity.INFO, - "Continuing previous autonomus procedure " + m_autonomous.getName()); - } else if (m_autonSelector.getSelectedAutonMode() == null) { - Logger.get(Category.AUTONOMOUS).logRaw(Severity.WARNING, "No autonomous mode selected"); - } + autonomous.startAutonomousMode(); + faultInAutoInit = false; } public void autonomousPeriodic() { if (faultInRobotInit || faultInAutoInit) return; - final AutonomousMode autonomousMode = m_autonSelector.getSelectedAutonMode(); - if (autonomousMode != null && m_autonMode != autonomousMode) { - m_autonomous = autonomousMode.instantiate(); - m_autonomous.schedule(); - m_autonMode = autonomousMode; - Logger.get(Category.AUTONOMOUS) - .logRaw( - Severity.INFO, - "Starting new autonomus procedure " + m_autonomous.getName()); - } CommandScheduler.getInstance().run(); if (m_lights != null && m_lightUpdateLimiter.next()) { m_lights.run(); @@ -150,11 +123,7 @@ public void autonomousPeriodic() { public void teleopInit() { faultInTeleopInit = true; - if (m_autonomous != null) { - m_autonomous.cancel(); - m_autonomous = null; - m_autonMode = null; - } + autonomous.stopAutonomousMode("entering teleop"); faultInTeleopInit = false; } diff --git a/src/test/java/com/team766/TestCase3.java b/src/test/java/com/team766/TestCase3.java index 11777c9d..5bc24e2c 100644 --- a/src/test/java/com/team766/TestCase3.java +++ b/src/test/java/com/team766/TestCase3.java @@ -14,7 +14,7 @@ public abstract class TestCase3 { @BeforeEach - public void setUp() { + public final void setUpFramework() { assert HAL.initialize(500, 0); ConfigFileTestUtils.resetStatics(); diff --git a/src/test/java/com/team766/framework3/AutonomousModeStateMachineTest.java b/src/test/java/com/team766/framework3/AutonomousModeStateMachineTest.java new file mode 100644 index 00000000..8e9e14e4 --- /dev/null +++ b/src/test/java/com/team766/framework3/AutonomousModeStateMachineTest.java @@ -0,0 +1,286 @@ +package com.team766.framework3; + +import static org.junit.jupiter.api.Assertions.*; + +import com.team766.TestCase3; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import java.util.ArrayList; +import java.util.function.Supplier; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +public class AutonomousModeStateMachineTest extends TestCase3 { + private int autonomousModeInstanceCounter; + private int procedureAge; + private AutonomousMode selectedAutonomousMode; + private ArrayList commands; + private AutonomousModeStateMachine sm; + + @BeforeEach + public void setUp() { + autonomousModeInstanceCounter = 0; + commands = new ArrayList<>(); + Supplier supplier = () -> selectedAutonomousMode; + sm = new AutonomousModeStateMachine(supplier); + } + + private void selectNewAutonomousMode() { + final int thisInstanceCount = ++autonomousModeInstanceCounter; + selectedAutonomousMode = + new AutonomousMode( + "Auton" + thisInstanceCount, + () -> { + assertEquals(thisInstanceCount, autonomousModeInstanceCounter); + procedureAge = 0; + var procedure = + new RunCommand(() -> ++procedureAge).ignoringDisable(true); + commands.add(procedure); + return procedure; + }); + } + + /// Represents the code running when the user hasn't selected an autonomous mode via the web UI. + @Test + public void testOperationsWithNullAutonomousMode() { + selectedAutonomousMode = null; + + sm.stopAutonomousMode("stopping"); + sm.reinitializeAutonomousMode("reinitializing"); + sm.startAutonomousMode(); + sm.stopAutonomousMode("stopping"); + } + + /// This represents the case where the robot code enters teleop without first running + /// autonomous. + /// Doesn't result in any action being taken, but we want to make sure it doesn't trigger a + /// NullPointerException or any other bad side effect. + @Test + public void testStopWithoutStart() { + selectNewAutonomousMode(); + + sm.stopAutonomousMode("stopping"); + + assertEquals(0, commands.size()); + } + + /// This represents the case where the RoboRIO reboots during autonomous mode, so the + /// robot code directly enters autonomous without being disabled first. + /// The robot should initialize and run an instance of the autonomous Command. + @Test + public void testStartWithoutInitialize() { + selectNewAutonomousMode(); + + sm.startAutonomousMode(); + + assertEquals(1, commands.size()); + assertTrue(commands.get(0).isScheduled()); + } + + /// This represents the usual case where a robot sits in disabled mode for a while + /// before autonomous is enabled. + /// The robot should initialize the autonomous Command while in disabled, and then use + /// that Command when autonomous starts. + @Test + public void testStartAfterInitialize() { + selectNewAutonomousMode(); + + sm.reinitializeAutonomousMode("initializing"); + sm.reinitializeAutonomousMode("initializing again"); + + assertEquals(1, commands.size()); + assertFalse(commands.get(0).isScheduled()); + + sm.startAutonomousMode(); + + assertEquals(1, commands.size()); + assertTrue(commands.get(0).isScheduled()); + } + + /// Calling startAutonomousMode twice is representative of what would happen if the robot + /// becomes momentarily disabled during autonomous, for example from a transient wifi issue. + /// No new Command should be created in the second call - the one created during the first + /// call to startAutonomousMode should be continued. + @Test + public void testResume() { + selectNewAutonomousMode(); + + sm.startAutonomousMode(); + + assertEquals(1, commands.size()); + assertTrue(commands.get(0).isScheduled()); + + sm.startAutonomousMode(); + + assertEquals(1, commands.size()); + assertTrue(commands.get(0).isScheduled()); + } + + /// Test the usual case where the robot runs autonomous and then enters teleop. + @Test + public void testStop() { + selectNewAutonomousMode(); + + sm.startAutonomousMode(); + + assertEquals(1, commands.size()); + assertTrue(commands.get(0).isScheduled()); + + sm.stopAutonomousMode("stopping"); + + assertEquals(1, commands.size()); + assertFalse(commands.get(0).isScheduled()); + + sm.stopAutonomousMode("stopping again"); + + assertEquals(1, commands.size()); + assertFalse(commands.get(0).isScheduled()); + } + + /// Autonomous -> Teleop -> Autonomous. This could happen frequently during testing + /// in the shop, or after a field fault in competition. Should create a second instance + /// of the autononomus Command for the second run of autonomous. + @Test + public void testRunTwice() { + selectNewAutonomousMode(); + + sm.startAutonomousMode(); + + assertEquals(1, commands.size()); + assertTrue(commands.get(0).isScheduled()); + + sm.stopAutonomousMode("stopping"); + + assertEquals(1, commands.size()); + assertFalse(commands.get(0).isScheduled()); + + sm.startAutonomousMode(); + + assertEquals(2, commands.size()); + assertFalse(commands.get(0).isScheduled()); + assertTrue(commands.get(1).isScheduled()); + } + + /// Autonomous -> Teleop -> Disabled -> Autonomous + @Test + public void testRunTwiceWithReinitialize() { + selectNewAutonomousMode(); + + sm.startAutonomousMode(); + + assertEquals(1, commands.size()); + assertTrue(commands.get(0).isScheduled()); + + sm.stopAutonomousMode("stopping"); + + assertEquals(1, commands.size()); + assertFalse(commands.get(0).isScheduled()); + + sm.reinitializeAutonomousMode("reinitializing"); + + assertEquals(2, commands.size()); + assertFalse(commands.get(0).isScheduled()); + assertFalse(commands.get(1).isScheduled()); + + sm.startAutonomousMode(); + + assertEquals(2, commands.size()); + assertFalse(commands.get(0).isScheduled()); + assertTrue(commands.get(1).isScheduled()); + } + + /// If the user selects a different autonomous mode while the robot is in disabled mode, + /// an instance of the new mode's Command should be instantiated and then used when the robot + /// enters autnonomous. + @Test + public void testNewSelectionWhenDisabled() { + selectNewAutonomousMode(); + + sm.reinitializeAutonomousMode("initializing"); + + assertEquals(1, commands.size()); + assertFalse(commands.get(0).isScheduled()); + + selectNewAutonomousMode(); + + sm.reinitializeAutonomousMode("reinitializing"); + + assertEquals(2, commands.size()); + assertFalse(commands.get(0).isScheduled()); + assertFalse(commands.get(1).isScheduled()); + + sm.startAutonomousMode(); + + assertEquals(2, commands.size()); + assertFalse(commands.get(0).isScheduled()); + assertTrue(commands.get(1).isScheduled()); + } + + /// If the user selects a different autonomous mode between two autonomous runs, + /// an instance of the new mode's Command should be instantiated and then used when the robot + /// enters autnonomous the second time. + @Test + public void testRunTwiceWithNewSelection() { + selectNewAutonomousMode(); + + sm.startAutonomousMode(); + + assertEquals(1, commands.size()); + assertTrue(commands.get(0).isScheduled()); + + sm.stopAutonomousMode("stopping"); + + selectNewAutonomousMode(); + + sm.startAutonomousMode(); + + assertEquals(2, commands.size()); + assertFalse(commands.get(0).isScheduled()); + assertTrue(commands.get(1).isScheduled()); + } + + /// If the user removes their selection of an autonomous mode after it has been initialized, + /// the robot should not run any Command when the robot enters autonomous. + @Test + public void testDeselectionWhenDisabled() { + selectNewAutonomousMode(); + + sm.reinitializeAutonomousMode("initializing"); + + assertEquals(1, commands.size()); + assertFalse(commands.get(0).isScheduled()); + + selectedAutonomousMode = null; + + sm.reinitializeAutonomousMode("reinitializing"); + + assertEquals(1, commands.size()); + assertFalse(commands.get(0).isScheduled()); + + sm.startAutonomousMode(); + + assertEquals(1, commands.size()); + assertFalse(commands.get(0).isScheduled()); + } + + /// If the user removes their selection of an autonomous mode between two autonomous runs, + /// the robot should not run any Command when the robot enters autonomous the second time. + @Test + public void testRunTwiceWithDeselection() { + selectNewAutonomousMode(); + + sm.startAutonomousMode(); + + assertEquals(1, commands.size()); + assertTrue(commands.get(0).isScheduled()); + + sm.stopAutonomousMode("stopping"); + + selectedAutonomousMode = null; + + sm.startAutonomousMode(); + + assertEquals(1, commands.size()); + assertFalse(commands.get(0).isScheduled()); + } +}