diff --git a/src/main/java/com/team766/framework3/Mechanism.java b/src/main/java/com/team766/framework3/Mechanism.java index b4679096..826e2e22 100644 --- a/src/main/java/com/team766/framework3/Mechanism.java +++ b/src/main/java/com/team766/framework3/Mechanism.java @@ -4,8 +4,10 @@ import com.team766.logging.Logger; import com.team766.logging.LoggerExceptionUtils; import com.team766.logging.Severity; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.Objects; public abstract class Mechanism> extends SubsystemBase implements LoggingBase { private Thread m_runningPeriodic = null; @@ -13,18 +15,80 @@ public abstract class Mechanism> extends SubsystemBase impl private R request = null; private boolean isRequestNew = false; + /** + * This Command runs when no other Command (i.e. Procedure) is reserving this Mechanism. + * If this Mechanism defines getIdleRequest, this Command serves to apply that request. + */ + private final class IdleCommand extends Command { + public IdleCommand() { + addRequirements(Mechanism.this); + } + + @Override + public void initialize() { + try { + final var r = getIdleRequest(); + if (r != null) { + SchedulerMonitor.currentCommand = this; + setRequest(r); + } + } catch (Exception ex) { + ex.printStackTrace(); + LoggerExceptionUtils.logException(ex); + } finally { + SchedulerMonitor.currentCommand = null; + } + } + + @Override + public boolean isFinished() { + return false; + } + } + + public Mechanism() { + setDefaultCommand(new IdleCommand()); + } + @Override public Category getLoggerCategory() { return Category.MECHANISMS; } public final void setRequest(R request) { + Objects.requireNonNull(request); checkContextReservation(); this.request = request; isRequestNew = true; log(this.getClass().getName() + " processing request: " + request); } + /** + * The request returned by this method will be set as the request for this Mechanism when the + * Mechanism is first created. + * + * If this Mechanism defines getIdleRequest(), then this Initial request will only be passed to + * the first call to run() (after that, the Idle request may take over). Otherwise, this Initial + * request will be passed to run() until something calls setRequest() on this Mechanism. + * + * This method will only be called once, immediately before the first call to run(). Because it + * is called before run(), it cannot call getMechanismStatus() or otherwise depend on the Status + * published by this Mechanism. + */ + protected abstract R getInitialRequest(); + + /** + * The request returned by this method will be set as the request for this Mechanism when no + * Procedures are reserving this Mechanism. This happens when a Procedure which reserved this + * Mechanism completes. It can also happen when a Procedure that reserves this Mechanism is + * preempted by another Procedure, but the new Procedure does not reserve this Mechanism. + * getIdleRequest is especially in the latter case, because it can help to "clean up" after the + * cancelled Procedure, returning this Mechanism back to some safe state. + */ + protected R getIdleRequest() { + return null; + } + protected void checkContextReservation() { var owningCommand = CommandScheduler.getInstance().requiring(this); if ((owningCommand == null || SchedulerMonitor.currentCommand != owningCommand) @@ -50,6 +114,9 @@ public final void periodic() { try { m_runningPeriodic = Thread.currentThread(); + if (request == null) { + setRequest(getInitialRequest()); + } boolean wasRequestNew = isRequestNew; isRequestNew = false; run(request, wasRequestNew); diff --git a/src/main/java/com/team766/hal/GenericRobotMain3.java b/src/main/java/com/team766/hal/GenericRobotMain3.java index 391e69a1..5824ce11 100755 --- a/src/main/java/com/team766/hal/GenericRobotMain3.java +++ b/src/main/java/com/team766/hal/GenericRobotMain3.java @@ -162,11 +162,11 @@ public void teleopInit() { public void teleopPeriodic() { if (faultInRobotInit || faultInTeleopInit) return; + CommandScheduler.getInstance().run(); if (m_oi != null && RobotProvider.instance.hasNewDriverStationData()) { RobotProvider.instance.refreshDriverStationData(); m_oi.run(); } - CommandScheduler.getInstance().run(); if (m_lights != null && m_lightUpdateLimiter.next()) { m_lights.run(); } diff --git a/src/test/java/com/team766/TestCase3.java b/src/test/java/com/team766/TestCase3.java index 526544a9..c80178dd 100644 --- a/src/test/java/com/team766/TestCase3.java +++ b/src/test/java/com/team766/TestCase3.java @@ -7,11 +7,15 @@ import com.team766.hal.RobotProvider; import com.team766.hal.mock.TestRobotProvider; import edu.wpi.first.hal.HAL; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; import edu.wpi.first.wpilibj2.command.CommandScheduler; import java.io.IOException; import java.nio.file.Files; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.extension.ExtendWith; +import org.littletonrobotics.conduit.ConduitApi; +import org.littletonrobotics.junction.inputs.LoggedDriverStation; +import org.littletonrobotics.junction.inputs.LoggedSystemStats; @ExtendWith(TestLoggerExtension.class) public abstract class TestCase3 { @@ -20,12 +24,36 @@ public abstract class TestCase3 { public void setUp() { assert HAL.initialize(500, 0); + setRobotEnabled(true); + ConfigFileTestUtils.resetStatics(); SchedulerUtils.reset(); RobotProvider.instance = new TestRobotProvider(); } + protected void setRobotEnabled(boolean enabled) { + DriverStationSim.setDsAttached(true); + DriverStationSim.setEnabled(enabled); + updateDriverStationData(); + } + + protected void setRobotAutonomous(boolean autonomous) { + DriverStationSim.setAutonomous(autonomous); + updateDriverStationData(); + } + + protected void updateDriverStationData() { + // Flush data that was set using DriverStationSim + DriverStationSim.notifyNewData(); + // AdvantageKit inserts itself into DriverStation, so we also need to flush AdvantageKit + // data in order for DriverStation to get the updated data. + // https://github.com/Mechanical-Advantage/AdvantageKit/blob/e236e4bf57188addc3befd2d827660b470d9af89/docs/COMMON-ISSUES.md#unit-testing + ConduitApi.getInstance().captureData(); + LoggedDriverStation.periodic(); + LoggedSystemStats.periodic(); + } + protected void loadConfig(String configJson) throws IOException { var configFilePath = Files.createTempFile("testConfig", ".txt"); Files.writeString(configFilePath, configJson); @@ -33,6 +61,7 @@ protected void loadConfig(String configJson) throws IOException { } protected void step() { + updateDriverStationData(); CommandScheduler.getInstance().run(); } } diff --git a/src/test/java/com/team766/framework3/FakeMechanism.java b/src/test/java/com/team766/framework3/FakeMechanism.java index b69694a4..f53d0607 100644 --- a/src/test/java/com/team766/framework3/FakeMechanism.java +++ b/src/test/java/com/team766/framework3/FakeMechanism.java @@ -10,15 +10,18 @@ public boolean isDone(FakeStatus status) { } } - private FakeRequest currentRequest; + FakeRequest currentRequest; + Boolean wasRequestNew = null; - public FakeRequest currentRequest() { - return currentRequest; + @Override + protected FakeRequest getInitialRequest() { + return new FakeRequest(-1); } @Override protected void run(FakeRequest request, boolean isRequestNew) { - this.currentRequest = request; + currentRequest = request; + wasRequestNew = isRequestNew; } } diff --git a/src/test/java/com/team766/framework3/MechanismTest.java b/src/test/java/com/team766/framework3/MechanismTest.java index ca06b007..2c05bfce 100644 --- a/src/test/java/com/team766/framework3/MechanismTest.java +++ b/src/test/java/com/team766/framework3/MechanismTest.java @@ -6,7 +6,6 @@ import com.team766.TestCase3; import com.team766.framework3.FakeMechanism.FakeRequest; import java.util.Set; -import java.util.concurrent.atomic.AtomicBoolean; import java.util.concurrent.atomic.AtomicReference; import org.junit.jupiter.api.Test; @@ -15,16 +14,7 @@ public class MechanismTest extends TestCase3 { /// called from a Procedure which reserves the Mechanism. @Test public void testRequests() { - var currentRequest = new AtomicReference(); - var wasRequestNew = new AtomicBoolean(); - var mech = - new Mechanism() { - @Override - protected void run(FakeRequest request, boolean isRequestNew) { - currentRequest.set(request); - wasRequestNew.set(isRequestNew); - } - }; + var mech = new FakeMechanism(); var cmd = new ContextImpl( @@ -54,23 +44,23 @@ protected void run(FakeRequest request, boolean isRequestNew) { // Step 1. Test running the Mechanism in its uninitialized state. step(); - assertEquals(null, currentRequest.get()); - assertFalse(wasRequestNew.get()); + assertEquals(mech.getInitialRequest(), mech.currentRequest); + assertFalse(mech.wasRequestNew); // Step 2. The Mechanism receives the first request. step(); - assertEquals(new FakeRequest(0), currentRequest.get()); - assertTrue(wasRequestNew.get()); + assertEquals(new FakeRequest(0), mech.currentRequest); + assertTrue(mech.wasRequestNew); // Step 3. The Mechanism continues with its first request. step(); - assertEquals(new FakeRequest(0), currentRequest.get()); - assertFalse(wasRequestNew.get()); + assertEquals(new FakeRequest(0), mech.currentRequest); + assertFalse(mech.wasRequestNew); // Step 4. The Mechanism receives the second request. step(); - assertEquals(new FakeRequest(1), currentRequest.get()); - assertTrue(wasRequestNew.get()); + assertEquals(new FakeRequest(1), mech.currentRequest); + assertTrue(mech.wasRequestNew); // Poke the Procedure to ensure it has finished. step(); @@ -82,6 +72,12 @@ protected void run(FakeRequest request, boolean isRequestNew) { @Test public void testFailedCheckContextReservationInProcedure() { class DummyMechanism extends Mechanism { + @Override + protected FakeRequest getInitialRequest() { + return new FakeRequest(-1); + } + + @Override protected void run(FakeRequest request, boolean isRequestNew) {} } var mech = new DummyMechanism(); @@ -120,7 +116,7 @@ public void testCheckContextReservationInRun() { var thrownException = new AtomicReference(); @SuppressWarnings("unused") var mech = - new Mechanism() { + new FakeMechanism() { @Override protected void run(FakeRequest request, boolean isRequestNew) { try { @@ -133,4 +129,69 @@ protected void run(FakeRequest request, boolean isRequestNew) { step(); assertNull(thrownException.get()); } + + /// Test that the Initial request runs after Mechanism creation. + @Test + public void testInitialRequest() { + var mech = new FakeMechanism(); + step(); + assertEquals(new FakeRequest(-1), mech.currentRequest); + assertTrue(mech.wasRequestNew); + // Subsequent steps should continue to pass the Initial request to run(), + // but it should not be indicated as a new request. + mech.currentRequest = null; + step(); + assertEquals(new FakeRequest(-1), mech.currentRequest); + assertFalse(mech.wasRequestNew); + } + + /// Test that the Idle request runs if no other Command reserves this Mechanism. + @Test + public void testIdleRequest() { + var mech = + new FakeMechanism() { + @Override + protected FakeRequest getIdleRequest() { + return new FakeRequest(0); + } + }; + + // The first step should run the Initial request. + step(); + assertEquals(new FakeRequest(-1), mech.currentRequest); + assertTrue(mech.wasRequestNew); + // On subsequent steps, the Idle request should take over (as long as a Command hasn't + // reserved this Mechanism). + step(); + assertEquals(new FakeRequest(0), mech.currentRequest); + assertTrue(mech.wasRequestNew); + // Subsequent steps should continue to pass the Idle request to run(), + // but it should not be indicated as a new request. + mech.currentRequest = null; + step(); + assertEquals(new FakeRequest(0), mech.currentRequest); + assertFalse(mech.wasRequestNew); + + // When a Command is scheduled which reserves this Procedure, it should preempt + // the Idle request. + new FunctionalProcedure( + Set.of(mech), + context -> { + mech.setRequest(new FakeRequest(1)); + context.waitFor(() -> false); + }) + .createCommandToRunProcedure() + .schedule(); + step(); + step(); // NOTE: Second step() is needed because Scheduler runs Procedures after Subsystems + assertEquals(new FakeRequest(1), mech.currentRequest); + assertTrue(mech.wasRequestNew); + // Subsequent steps should allow the scheduled Command to continue. It should not be + // interrupted by the Idle request, even if the scheduled Command does not set a + // new request. + mech.currentRequest = null; + step(); + assertEquals(new FakeRequest(1), mech.currentRequest); + assertFalse(mech.wasRequestNew); + } }