Mechanism initial and idle requests

src/main/java/com/team766/framework3/Mechanism.java

@@ -4,6 +4,7 @@
 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;
 
@@ -13,6 +14,41 @@ public abstract class Mechanism<R extends Request<?>> 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;
@@ -25,6 +61,32 @@ public final void setRequest(R request) {
         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 +112,9 @@ public final void periodic() {
 
         try {
             m_runningPeriodic = Thread.currentThread();
+            if (request == null) {
+                setRequest(getInitialRequest());
+            }
             boolean wasRequestNew = isRequestNew;
             isRequestNew = false;
             run(request, wasRequestNew);

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();
         }

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,19 +24,44 @@ 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);
         ConfigFileReader.instance = new ConfigFileReader(configFilePath.toString());
     }
 
     protected void step() {
+        updateDriverStationData();
         CommandScheduler.getInstance().run();
     }
 }

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;
     }
 }
 

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<FakeRequest>();
-        var wasRequestNew = new AtomicBoolean();
-        var mech =
-                new Mechanism<FakeRequest>() {
-                    @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<FakeRequest> {
+            @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<Throwable>();
         @SuppressWarnings("unused")
         var mech =
-                new Mechanism<FakeRequest>() {
+                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);
+    }
 }