Mechanisms return Status from run()

Team766 · Dec 13, 2024 · d50e529 · d50e529
1 parent 3abc687
commit d50e529
Show file tree

Hide file tree

Showing 12 changed files with 85 additions and 48 deletions.
diff --git a/src/main/java/com/team766/framework3/FunctionalInstantProcedure.java b/src/main/java/com/team766/framework3/FunctionalInstantProcedure.java
@@ -5,12 +5,12 @@
 public final class FunctionalInstantProcedure extends InstantProcedure {
     private final Runnable runnable;
 
-    public FunctionalInstantProcedure(Set<Mechanism<?>> reservations, Runnable runnable) {
+    public FunctionalInstantProcedure(Set<Mechanism<?, ?>> reservations, Runnable runnable) {
         this(runnable.toString(), reservations, runnable);
     }
 
     public FunctionalInstantProcedure(
-            String name, Set<Mechanism<?>> reservations, Runnable runnable) {
+            String name, Set<Mechanism<?, ?>> reservations, Runnable runnable) {
         super(name, reservations);
         this.runnable = runnable;
     }

diff --git a/src/main/java/com/team766/framework3/FunctionalProcedure.java b/src/main/java/com/team766/framework3/FunctionalProcedure.java
@@ -6,12 +6,12 @@
 public final class FunctionalProcedure extends Procedure {
     private final Consumer<Context> runnable;
 
-    public FunctionalProcedure(Set<Mechanism<?>> reservations, Consumer<Context> runnable) {
+    public FunctionalProcedure(Set<Mechanism<?, ?>> reservations, Consumer<Context> runnable) {
         this(runnable.toString(), reservations, runnable);
     }
 
     public FunctionalProcedure(
-            String name, Set<Mechanism<?>> reservations, Consumer<Context> runnable) {
+            String name, Set<Mechanism<?, ?>> reservations, Consumer<Context> runnable) {
         super(name, reservations);
         this.runnable = runnable;
     }

diff --git a/src/main/java/com/team766/framework3/InstantProcedure.java b/src/main/java/com/team766/framework3/InstantProcedure.java
@@ -8,7 +8,7 @@ protected InstantProcedure() {
         super();
     }
 
-    protected InstantProcedure(String name, Set<Mechanism<?>> reservations) {
+    protected InstantProcedure(String name, Set<Mechanism<?, ?>> reservations) {
         super(name, reservations);
     }
 

diff --git a/src/main/java/com/team766/framework3/Mechanism.java b/src/main/java/com/team766/framework3/Mechanism.java
@@ -8,18 +8,21 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import java.util.ArrayList;
+import java.util.NoSuchElementException;
 import java.util.Objects;
 
-public abstract class Mechanism<R extends Request> extends SubsystemBase implements LoggingBase {
+public abstract class Mechanism<R extends Request, S extends Record & Status> extends SubsystemBase
+        implements LoggingBase {
     private Thread m_runningPeriodic = null;
 
-    private Mechanism<?> superstructure = 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 ArrayList<Mechanism<?, ?>> submechanisms = new ArrayList<>();
 
     private R request = null;
     private boolean isRequestNew = false;
+    private S status = null;
 
     /**
      * This Command runs when no other Command (i.e. Procedure) is reserving this Mechanism.
@@ -72,7 +75,7 @@ public Category getLoggerCategory() {
      *
      * @param superstructure The superstructure this Mechanism is part of.
      */
-    public void setSuperstructure(Mechanism<?> superstructure) {
+    public void setSuperstructure(Mechanism<?, ?> superstructure) {
         Objects.requireNonNull(superstructure);
         if (this.superstructure != null) {
             throw new IllegalStateException("Mechanism is already part of a superstructure");
@@ -143,6 +146,13 @@ protected void checkContextReservation() {
         ReservingCommand.checkCurrentCommandHasReservation(this);
     }
 
+    public S getMechanismStatus() {
+        if (status == null) {
+            throw new NoSuchElementException(getName() + " has not published a status yet");
+        }
+        return status;
+    }
+
     @Override
     public final void periodic() {
         super.periodic();
@@ -166,7 +176,8 @@ private void periodicInternal() {
             }
             boolean wasRequestNew = isRequestNew;
             isRequestNew = false;
-            run(request, wasRequestNew);
+            status = run(request, wasRequestNew);
+            StatusBus.publishStatus(status);
         } catch (Exception ex) {
             ex.printStackTrace();
             LoggerExceptionUtils.logException(ex);
@@ -175,5 +186,5 @@ private void periodicInternal() {
         }
     }
 
-    protected abstract void run(R request, boolean isRequestNew);
+    protected abstract S run(R request, boolean isRequestNew);
 }
diff --git a/src/main/java/com/team766/framework3/Procedure.java b/src/main/java/com/team766/framework3/Procedure.java
@@ -22,19 +22,19 @@ private static synchronized int createNewId() {
     }
 
     private final String name;
-    private final Set<Mechanism<?>> reservations;
+    private final Set<Mechanism<?, ?>> reservations;
 
     protected Procedure() {
         this.name = createName();
         this.reservations = Sets.newHashSet();
     }
 
-    protected Procedure(Set<Mechanism<?>> reservations) {
+    protected Procedure(Set<Mechanism<?, ?>> reservations) {
         this.name = createName();
         this.reservations = reservations;
     }
 
-    protected Procedure(String name, Set<Mechanism<?>> reservations) {
+    protected Procedure(String name, Set<Mechanism<?, ?>> reservations) {
         this.name = name;
         this.reservations = reservations;
     }
@@ -59,22 +59,22 @@ public Category getLoggerCategory() {
         return Category.PROCEDURES;
     }
 
-    protected final <M extends Mechanism<?>> M reserve(M m) {
+    protected final <M extends Mechanism<?, ?>> M reserve(M m) {
         reservations.add(m);
         return m;
     }
 
-    protected final void reserve(Mechanism<?>... ms) {
+    protected final void reserve(Mechanism<?, ?>... ms) {
         for (var m : ms) {
             reservations.add(m);
         }
     }
 
-    protected final void reserve(Collection<? extends Mechanism<?>> ms) {
+    protected final void reserve(Collection<? extends Mechanism<?, ?>> ms) {
         reservations.addAll(ms);
     }
 
-    public final Set<Mechanism<?>> reservations() {
+    public final Set<Mechanism<?, ?>> reservations() {
         return reservations;
     }
 

diff --git a/src/main/java/com/team766/framework3/Rule.java b/src/main/java/com/team766/framework3/Rule.java
@@ -132,7 +132,9 @@ public Builder withOnTriggeringProcedure(
 
         /** Specify a creator for the Procedure that should be run when this rule starts triggering. */
         public Builder withOnTriggeringProcedure(
-                RulePersistence rulePersistence, Set<Mechanism<?>> reservations, Runnable action) {
+                RulePersistence rulePersistence,
+                Set<Mechanism<?, ?>> reservations,
+                Runnable action) {
             applyRulePersistence(
                     rulePersistence, () -> new FunctionalInstantProcedure(reservations, action));
             return this;
@@ -141,7 +143,7 @@ public Builder withOnTriggeringProcedure(
         /** Specify a creator for the Procedure that should be run when this rule starts triggering. */
         public Builder withOnTriggeringProcedure(
                 RulePersistence rulePersistence,
-                Set<Mechanism<?>> reservations,
+                Set<Mechanism<?, ?>> reservations,
                 Consumer<Context> action) {
             applyRulePersistence(
                     rulePersistence, () -> new FunctionalProcedure(reservations, action));
@@ -156,7 +158,7 @@ public Builder withFinishedTriggeringProcedure(Supplier<Procedure> action) {
 
         /** Specify a creator for the Procedure that should be run when this rule was triggering before and is no longer triggering. */
         public Builder withFinishedTriggeringProcedure(
-                Set<Mechanism<?>> reservations, Runnable action) {
+                Set<Mechanism<?, ?>> reservations, Runnable action) {
             this.finishedTriggeringProcedure =
                     () -> new FunctionalInstantProcedure(reservations, action);
             return this;
@@ -221,7 +223,7 @@ private List<Rule> build(BooleanSupplier parentPredicate) {
     private final BooleanSupplier predicate;
     private final Map<TriggerType, Supplier<Procedure>> triggerProcedures =
             Maps.newEnumMap(TriggerType.class);
-    private final Map<TriggerType, Set<Mechanism<?>>> triggerReservations =
+    private final Map<TriggerType, Set<Mechanism<?, ?>>> triggerReservations =
             Maps.newEnumMap(TriggerType.class);
     private final Map<TriggerType, Cancellation> triggerCancellation =
             Maps.newEnumMap(TriggerType.class);
@@ -263,7 +265,7 @@ private Rule(
         }
     }
 
-    private static Set<Mechanism<?>> getReservationsForProcedure(Supplier<Procedure> supplier) {
+    private static Set<Mechanism<?, ?>> getReservationsForProcedure(Supplier<Procedure> supplier) {
         if (supplier != null) {
             Procedure procedure = supplier.get();
             if (procedure != null) {
@@ -314,7 +316,7 @@ public String getName() {
         }
     }
 
-    /* package */ Set<Mechanism<?>> getMechanismsToReserve() {
+    /* package */ Set<Mechanism<?, ?>> getMechanismsToReserve() {
         return triggerReservations.getOrDefault(currentTriggerType, Collections.emptySet());
     }
 

diff --git a/src/main/java/com/team766/framework3/RuleEngine.java b/src/main/java/com/team766/framework3/RuleEngine.java
@@ -75,7 +75,7 @@ protected Rule getRuleForTriggeredProcedure(Command command) {
     }
 
     public final void run() {
-        Set<Mechanism<?>> mechanismsToUse = new HashSet<>();
+        Set<Mechanism<?, ?>> mechanismsToUse = new HashSet<>();
 
         // TODO(MF3): when creating a Procedure, check that the reservations are the same as
         // what the Rule pre-computed.
@@ -94,9 +94,9 @@ public final void run() {
                     int priority = getPriorityForRule(rule);
 
                     // see if there are mechanisms a potential procedure would want to reserve
-                    Set<Mechanism<?>> reservations = rule.getMechanismsToReserve();
+                    Set<Mechanism<?, ?>> reservations = rule.getMechanismsToReserve();
                     log(Severity.INFO, "Rule " + rule.getName() + " would reserve " + reservations);
-                    for (Mechanism<?> mechanism : reservations) {
+                    for (Mechanism<?, ?> mechanism : reservations) {
                         // see if any of the mechanisms higher priority rules will use would also be
                         // used by this lower priority rule's procedure.
                         if (mechanismsToUse.contains(mechanism)) {

diff --git a/src/main/java/com/team766/framework3/WPILibCommandProcedure.java b/src/main/java/com/team766/framework3/WPILibCommandProcedure.java
@@ -21,14 +21,14 @@ public WPILibCommandProcedure(final Command command) {
     }
 
     @SuppressWarnings("unchecked")
-    private static Set<Mechanism<?>> checkSubsystemsAreMechanisms(Set<Subsystem> requirements) {
+    private static Set<Mechanism<?, ?>> checkSubsystemsAreMechanisms(Set<Subsystem> requirements) {
         for (var s : requirements) {
-            if (!(s instanceof Mechanism<?>)) {
+            if (!(s instanceof Mechanism<?, ?>)) {
                 throw new IllegalArgumentException(
                         "Maroon Framework requires the use of Mechanism instead of Subsystem");
             }
         }
-        return (Set<Mechanism<?>>) (Set<?>) requirements;
+        return (Set<Mechanism<?, ?>>) (Set<?>) requirements;
     }
 
     @Override

diff --git a/src/test/java/com/team766/framework3/FakeMechanism.java b/src/test/java/com/team766/framework3/FakeMechanism.java
@@ -2,8 +2,8 @@
 
 import static com.team766.framework3.Conditions.checkForStatusWith;
 
-class FakeMechanism extends Mechanism<FakeMechanism.FakeRequest> {
-    record FakeStatus(int currentState) implements Status {}
+class FakeMechanism extends Mechanism<FakeMechanism.FakeRequest, FakeMechanism.FakeStatus> {
+    public record FakeStatus(int currentState) implements Status {}
 
     public record FakeRequest(int targetState) implements Request {
         @Override
@@ -21,9 +21,10 @@ protected FakeRequest getInitialRequest() {
     }
 
     @Override
-    protected void run(FakeRequest request, boolean isRequestNew) {
+    protected FakeStatus run(FakeRequest request, boolean isRequestNew) {
         currentRequest = request;
         wasRequestNew = isRequestNew;
+        return new FakeStatus(request.targetState());
     }
 }
 

diff --git a/src/test/java/com/team766/framework3/FakeProcedure.java b/src/test/java/com/team766/framework3/FakeProcedure.java
@@ -7,12 +7,12 @@ class FakeProcedure extends Procedure {
     private int age = 0;
     private boolean ended = false;
 
-    public FakeProcedure(int lifetime, Set<Mechanism<?>> reservations) {
+    public FakeProcedure(int lifetime, Set<Mechanism<?, ?>> reservations) {
         super(reservations);
         this.lifetime = lifetime;
     }
 
-    public FakeProcedure(String name, int lifetime, Set<Mechanism<?>> reservations) {
+    public FakeProcedure(String name, int lifetime, Set<Mechanism<?, ?>> reservations) {
         super(name, reservations);
         this.lifetime = lifetime;
     }

diff --git a/src/test/java/com/team766/framework3/MechanismTest.java b/src/test/java/com/team766/framework3/MechanismTest.java
@@ -5,6 +5,7 @@
 
 import com.team766.TestCase3;
 import com.team766.framework3.FakeMechanism.FakeRequest;
+import com.team766.framework3.FakeMechanism.FakeStatus;
 import java.util.Set;
 import java.util.concurrent.atomic.AtomicReference;
 import org.junit.jupiter.api.Test;
@@ -67,18 +68,38 @@ public void testRequests() {
         assertTrue(cmd.isFinished());
     }
 
+    /// Test a Mechanism publishing a Status via its run() method return value.
+    @Test
+    public void testStatuses() {
+        // FakeMechanism publishes a FakeStatus with the state value which was most recently set in
+        // its Request.
+        var mech =
+                new FakeMechanism() {
+                    @Override
+                    protected FakeRequest getIdleRequest() {
+                        return new FakeRequest(10);
+                    }
+                };
+        step();
+        step();
+        assertEquals(new FakeStatus(10), StatusBus.getStatusOrThrow(FakeStatus.class));
+        assertEquals(new FakeStatus(10), mech.getMechanismStatus());
+    }
+
     /// Test that checkContextReservation throws an exception when called from a Procedure which has
     /// not reserved the Mechanism.
     @Test
     public void testFailedCheckContextReservationInProcedure() {
-        class DummyMechanism extends Mechanism<FakeRequest> {
+        class DummyMechanism extends Mechanism<FakeRequest, FakeStatus> {
             @Override
             protected FakeRequest getInitialRequest() {
                 return new FakeRequest(-1);
             }
 
             @Override
-            protected void run(FakeRequest request, boolean isRequestNew) {}
+            protected FakeStatus run(FakeRequest request, boolean isRequestNew) {
+                return new FakeStatus(request.targetState());
+            }
         }
         var mech = new DummyMechanism();
 
@@ -116,12 +137,13 @@ public void testCheckContextReservationInRun() {
         var mech =
                 new FakeMechanism() {
                     @Override
-                    protected void run(FakeRequest request, boolean isRequestNew) {
+                    protected FakeStatus run(FakeRequest request, boolean isRequestNew) {
                         try {
                             checkContextReservation();
                         } catch (Throwable ex) {
                             thrownException.set(ex);
                         }
+                        return new FakeStatus(request.targetState());
                     }
                 };
         step();
@@ -196,7 +218,7 @@ protected FakeRequest getIdleRequest() {
     /// Test making a Mechanism part of a superstructure.
     @Test
     public void testSuperstructure() {
-        class Superstructure extends Mechanism<FakeRequest> {
+        class Superstructure extends Mechanism<FakeRequest, FakeStatus> {
             // 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.
@@ -213,14 +235,15 @@ protected FakeRequest getInitialRequest() {
             }
 
             @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));
+            protected FakeStatus run(FakeRequest request, boolean isRequestNew) {
+                if (isRequestNew) {
+                    if (request.targetState() == 0) {
+                        submechanism.setRequest(new FakeRequest(2));
+                    } else {
+                        submechanism.setRequest(new FakeRequest(4));
+                    }
                 }
+                return new FakeStatus(request.targetState());
             }
         }
         var superstructure = new Superstructure();

diff --git a/src/test/java/com/team766/framework3/RuleTest.java b/src/test/java/com/team766/framework3/RuleTest.java
@@ -139,9 +139,9 @@ public void testGetCancellation() {
 
     @Test
     public void testGetMechanismsToReserve() {
-        final Set<Mechanism<?>> newlyMechanisms =
+        final Set<Mechanism<?, ?>> newlyMechanisms =
                 Set.of(new FakeMechanism1(), new FakeMechanism2());
-        final Set<Mechanism<?>> finishedMechanisms = Set.of(new FakeMechanism());
+        final Set<Mechanism<?, ?>> finishedMechanisms = Set.of(new FakeMechanism());
 
         Rule duckDuckGooseGoose =
                 getSingleElement(