Mechanisms return Status from run()

Team766 · Dec 14, 2024 · d4029e5 · d4029e5
1 parent 2069112
commit d4029e5
Show file tree

Hide file tree

Showing 14 changed files with 119 additions and 50 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
@@ -7,15 +7,18 @@
 import com.team766.logging.Severity;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+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 boolean isRunningPeriodic = false;
 
-    private Superstructure<?> superstructure = null;
+    private Superstructure<?, ?> superstructure = null;
 
     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.
@@ -68,7 +71,7 @@ public Category getLoggerCategory() {
      *
      * @param superstructure The superstructure this Mechanism is part of.
      */
-    /* package */ void setSuperstructure(Superstructure<?> superstructure) {
+    /* package */ void setSuperstructure(Superstructure<?, ?> 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();
@@ -163,7 +173,8 @@ public final void periodic() {
             }
             boolean wasRequestNew = isRequestNew;
             isRequestNew = false;
-            run(request, wasRequestNew);
+            status = run(request, wasRequestNew);
+            StatusBus.publishStatus(status);
         } catch (Exception ex) {
             ex.printStackTrace();
             LoggerExceptionUtils.logException(ex);
@@ -172,5 +183,5 @@ public final void periodic() {
         }
     }
 
-    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 Cancellation cancellationOnFinish;
 
@@ -262,7 +264,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) {
@@ -313,7 +315,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/SensorMechanism.java b/src/main/java/com/team766/framework3/SensorMechanism.java
@@ -0,0 +1,27 @@
+package com.team766.framework3;
+
+/**
+ * A SensorMechanism is a Mechanism which only produces data; it can't be commanded by a Procedure
+ * or RuleEngine. This is useful for writing code for cameras and other sensors.
+ */
+public abstract class SensorMechanism<S extends Record & Status>
+        extends Mechanism<SensorMechanism.EmptyRequest, S> {
+    protected record EmptyRequest() implements Request {
+        @Override
+        public boolean isDone() {
+            return true;
+        }
+    }
+
+    @Override
+    protected final EmptyRequest getInitialRequest() {
+        return new EmptyRequest();
+    }
+
+    @Override
+    protected final S run(EmptyRequest request, boolean isRequestNew) {
+        return run();
+    }
+
+    protected abstract S run();
+}
diff --git a/src/main/java/com/team766/framework3/Superstructure.java b/src/main/java/com/team766/framework3/Superstructure.java
@@ -3,8 +3,9 @@
 import java.util.ArrayList;
 import java.util.Objects;
 
-public abstract class Superstructure<R extends Request> extends Mechanism<R> {
-    private ArrayList<Mechanism<?>> submechanisms = new ArrayList<>();
+public abstract class Superstructure<R extends Request, S extends Record & Status>
+        extends Mechanism<R, S> {
+    private ArrayList<Mechanism<?, ?>> submechanisms = new ArrayList<>();
 
     @Override
     /* package */ void periodicInternal() {
@@ -15,7 +16,7 @@ public abstract class Superstructure<R extends Request> extends Mechanism<R> {
         super.periodicInternal();
     }
 
-    protected <M extends Mechanism<?>> M addMechanism(M submechanism) {
+    protected <M extends Mechanism<?, ?>> M addMechanism(M submechanism) {
         Objects.requireNonNull(submechanism);
         submechanism.setSuperstructure(this);
         submechanisms.add(submechanism);

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