Superstructures

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

@@ -1,13 +1,18 @@
 package com.team766.framework3;
 
+import com.team766.framework.StackTraceUtils;
 import com.team766.logging.Category;
+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.SubsystemBase;
 import java.util.Objects;
 
 public abstract class Mechanism<R extends Request<?>> extends SubsystemBase implements LoggingBase {
-    private Thread m_runningPeriodic = null;
+    private boolean isRunningPeriodic = false;
+
+    private Superstructure<?> superstructure = null;
 
     private R request = null;
     private boolean isRequestNew = false;
@@ -54,6 +59,29 @@ public Category getLoggerCategory() {
         return Category.MECHANISMS;
     }
 
+    /**
+     * Indicate that this Mechanism is part of a superstructure.
+     *
+     * A Mechanism in a superstructure cannot be reserved individually by Procedures (Procedures
+     * should reserve the entire superstructure) and cannot have an Idle request. Only the
+     * superstructure should set requests on this Mechanism in its {@link #run(R, boolean)} method.
+     *
+     * @param superstructure The superstructure this Mechanism is part of.
+     */
+    /* package */ void setSuperstructure(Superstructure<?> superstructure) {
+        Objects.requireNonNull(superstructure);
+        if (this.superstructure != null) {
+            throw new IllegalStateException("Mechanism is already part of a superstructure");
+        }
+        if (this.getIdleRequest() != null) {
+            throw new UnsupportedOperationException(
+                    "A Mechanism contained in a superstructure cannot define an idle request. "
+                            + "Use the superstructure's idle request to control the idle behavior "
+                            + "of the contained Mechanisms.");
+        }
+        this.superstructure = superstructure;
+    }
+
     public final void setRequest(R request) {
         Objects.requireNonNull(request);
         checkContextReservation();
@@ -88,8 +116,28 @@ protected R getIdleRequest() {
         return null;
     }
 
+    /* package */ boolean isRunningPeriodic() {
+        return isRunningPeriodic;
+    }
+
     protected void checkContextReservation() {
-        if (m_runningPeriodic != null) {
+        if (isRunningPeriodic()) {
+            return;
+        }
+        if (superstructure != null) {
+            if (!superstructure.isRunningPeriodic()) {
+                var exception =
+                        new IllegalStateException(
+                                this.getName()
+                                        + " is part of a superstructure but was used by something outside the superstructure");
+                Logger.get(Category.FRAMEWORK)
+                        .logRaw(
+                                Severity.ERROR,
+                                exception.getMessage()
+                                        + "\n"
+                                        + StackTraceUtils.getStackTrace(exception.getStackTrace()));
+                throw exception;
+            }
             return;
         }
         ReservingCommand.checkCurrentCommandHasReservation(this);
@@ -99,8 +147,17 @@ protected void checkContextReservation() {
     public final void periodic() {
         super.periodic();
 
+        if (superstructure != null) {
+            // This Mechanism's periodic() will be run by its superstructure.
+            return;
+        }
+
+        periodicInternal();
+    }
+
+    /* package */ void periodicInternal() {
         try {
-            m_runningPeriodic = Thread.currentThread();
+            isRunningPeriodic = true;
             if (request == null) {
                 setRequest(getInitialRequest());
             }
@@ -111,7 +168,7 @@ public final void periodic() {
             ex.printStackTrace();
             LoggerExceptionUtils.logException(ex);
         } finally {
-            m_runningPeriodic = null;
+            isRunningPeriodic = false;
         }
     }
 

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

@@ -0,0 +1,24 @@
+package com.team766.framework3;
+
+import java.util.ArrayList;
+import java.util.Objects;
+
+public abstract class Superstructure<R extends Request<?>> extends Mechanism<R> {
+    private ArrayList<Mechanism<?>> submechanisms = new ArrayList<>();
+
+    @Override
+    /* package */ void periodicInternal() {
+        for (var m : submechanisms) {
+            m.periodicInternal();
+        }
+
+        super.periodicInternal();
+    }
+
+    protected <M extends Mechanism<?>> M addMechanism(M submechanism) {
+        Objects.requireNonNull(submechanism);
+        submechanism.setSuperstructure(this);
+        submechanisms.add(submechanism);
+        return submechanism;
+    }
+}

src/test/java/com/team766/framework3/MechanismTest.java

@@ -192,4 +192,71 @@ protected FakeRequest getIdleRequest() {
         assertEquals(new FakeRequest(1), mech.currentRequest);
         assertFalse(mech.wasRequestNew);
     }
+
+    /// Test making a Mechanism part of a superstructure.
+    @Test
+    public void testSuperstructure() {
+        class TestSuperstructure extends Superstructure<FakeRequest> {
+            // NOTE: Real superstructures should have their members be private. This is public
+            // to test handling of bad code patterns, and to allow us to inspect the state of the
+            // inner mechanism for purposes of testing the framework.
+            public final FakeMechanism submechanism;
+
+            public TestSuperstructure() {
+                submechanism = addMechanism(new FakeMechanism());
+            }
+
+            @Override
+            protected FakeRequest getInitialRequest() {
+                return new FakeRequest(0);
+            }
+
+            @Override
+            protected void run(FakeRequest request, boolean isRequestNew) {
+                if (!isRequestNew) return;
+
+                if (request.targetState() == 0) {
+                    submechanism.setRequest(new FakeRequest(2));
+                } else {
+                    submechanism.setRequest(new FakeRequest(4));
+                }
+            }
+        }
+        var superstructure = new TestSuperstructure();
+
+        step();
+        // Sub-mechanisms should run their periodic() method before the superstructure's periodic(),
+        // so we will see the sub-mechanism's initial request after the first step.
+        assertEquals(new FakeRequest(-1), superstructure.submechanism.currentRequest);
+
+        step();
+        // After the second step, the request set by the superstructure on the first step will have
+        // propagated to the sub-mechanism.
+        assertEquals(new FakeRequest(2), superstructure.submechanism.currentRequest);
+
+        // Test error conditions
+
+        assertThrows(
+                IllegalStateException.class,
+                () -> superstructure.submechanism.setRequest(new FakeRequest(0)),
+                "is part of a superstructure");
+
+        assertThrows(NullPointerException.class, () -> superstructure.addMechanism(null));
+
+        assertThrows(
+                IllegalStateException.class,
+                () -> superstructure.addMechanism(superstructure.submechanism),
+                "Mechanism is already part of a superstructure");
+
+        assertThrows(
+                UnsupportedOperationException.class,
+                () ->
+                        superstructure.addMechanism(
+                                new FakeMechanism() {
+                                    protected FakeRequest getIdleRequest() {
+                                        return new FakeRequest(0);
+                                    }
+                                }),
+                "A Mechanism contained in a superstructure cannot define an idle request");
+    }
 }