Superstructures

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

@@ -1,13 +1,23 @@
 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.ArrayList;
+import java.util.Objects;
 
 public abstract class Mechanism<R extends Request<?>> extends SubsystemBase implements LoggingBase {
     private Thread m_runningPeriodic = null;
 
+    private Mechanism<?> superstructure = null;
+
+    // If this Mechanism is a superstructure, this is a list of its constituent Mechanisms.
+    private ArrayList<Mechanism<?>> submechanisms = new ArrayList<>();
+
     private R request = null;
     private boolean isRequestNew = false;
 
@@ -53,6 +63,30 @@ 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.
+     */
+    public void setSuperstructure(Mechanism<?> superstructure) {
+        Objects.requireNonNull(superstructure);
+        if (this.superstructure != null) {
+            throw new IllegalStateException("Mechanism is already part of a superstructure");
+        }
+        if (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;
+        superstructure.submechanisms.add(this);
+    }
+
     public final void setRequest(R request) {
         checkContextReservation();
         this.request = request;
@@ -90,14 +124,42 @@ protected void checkContextReservation() {
         if (m_runningPeriodic != null) {
             return;
         }
+        if (superstructure != null) {
+            if (superstructure.m_runningPeriodic == null) {
+                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);
     }
 
     @Override
     public final void periodic() {
         super.periodic();
 
+        if (superstructure != null) {
+            // This Mechanism's periodic() will be run by its superstructure.
+            return;
+        }
+
+        periodicInternal();
+    }
+
+    private void periodicInternal() {
         try {
+            for (var m : submechanisms) {
+                m.periodicInternal();
+            }
             m_runningPeriodic = Thread.currentThread();
             if (request == null) {
                 setRequest(getInitialRequest());

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

@@ -192,4 +192,73 @@ 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 Superstructure extends Mechanism<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 Superstructure() {
+                submechanism = new FakeMechanism();
+                submechanism.setSuperstructure(this);
+            }
+
+            @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 Superstructure();
+
+        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.submechanism.setSuperstructure(null));
+
+        assertThrows(
+                IllegalStateException.class,
+                () -> superstructure.submechanism.setSuperstructure(superstructure),
+                "Mechanism is already part of a superstructure");
+
+        assertThrows(
+                UnsupportedOperationException.class,
+                () ->
+                        new FakeMechanism() {
+                            protected FakeRequest getIdleRequest() {
+                                return new FakeRequest(0);
+                            }
+                        }.setSuperstructure(superstructure),
+                "A Mechanism contained in a superstructure cannot define an idle request");
+    }
 }