diff --git a/src/main/java/com/team766/framework3/FunctionalInstantProcedure.java b/src/main/java/com/team766/framework3/FunctionalInstantProcedure.java index 131369f2..d9eebf7a 100644 --- 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> reservations, Runnable runnable) { + public FunctionalInstantProcedure(Set> reservations, Runnable runnable) { this(runnable.toString(), reservations, runnable); } public FunctionalInstantProcedure( - String name, Set> reservations, Runnable runnable) { + String name, Set> 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 index 141c416c..2c0c9a11 100644 --- 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 runnable; - public FunctionalProcedure(Set> reservations, Consumer runnable) { + public FunctionalProcedure(Set> reservations, Consumer runnable) { this(runnable.toString(), reservations, runnable); } public FunctionalProcedure( - String name, Set> reservations, Consumer runnable) { + String name, Set> reservations, Consumer 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 index bb3be2bf..b1406c56 100644 --- 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> reservations) { + protected InstantProcedure(String name, Set> reservations) { super(name, reservations); } diff --git a/src/main/java/com/team766/framework3/Mechanism.java b/src/main/java/com/team766/framework3/Mechanism.java index 380e87ac..aa055d8e 100644 --- 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 extends SubsystemBase implements LoggingBase { +public abstract class Mechanism 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 index 92fdc12f..db8dcd6f 100644 --- 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> reservations; + private final Set> reservations; protected Procedure() { this.name = createName(); this.reservations = Sets.newHashSet(); } - protected Procedure(Set> reservations) { + protected Procedure(Set> reservations) { this.name = createName(); this.reservations = reservations; } - protected Procedure(String name, Set> reservations) { + protected Procedure(String name, Set> reservations) { this.name = name; this.reservations = reservations; } @@ -59,22 +59,22 @@ public Category getLoggerCategory() { return Category.PROCEDURES; } - protected final > M reserve(M m) { + protected final > 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> ms) { + protected final void reserve(Collection> ms) { reservations.addAll(ms); } - public final Set> reservations() { + public final Set> reservations() { return reservations; } diff --git a/src/main/java/com/team766/framework3/Rule.java b/src/main/java/com/team766/framework3/Rule.java index 9510f5c9..d811b668 100644 --- 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> reservations, Runnable action) { + RulePersistence rulePersistence, + Set> 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> reservations, + Set> reservations, Consumer action) { applyRulePersistence( rulePersistence, () -> new FunctionalProcedure(reservations, action)); @@ -156,7 +158,7 @@ public Builder withFinishedTriggeringProcedure(Supplier 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> reservations, Runnable action) { + Set> reservations, Runnable action) { this.finishedTriggeringProcedure = () -> new FunctionalInstantProcedure(reservations, action); return this; @@ -221,7 +223,7 @@ private List build(BooleanSupplier parentPredicate) { private final BooleanSupplier predicate; private final Map> triggerProcedures = Maps.newEnumMap(TriggerType.class); - private final Map>> triggerReservations = + private final Map>> triggerReservations = Maps.newEnumMap(TriggerType.class); private final Cancellation cancellationOnFinish; @@ -262,7 +264,7 @@ private Rule( } } - private static Set> getReservationsForProcedure(Supplier supplier) { + private static Set> getReservationsForProcedure(Supplier supplier) { if (supplier != null) { Procedure procedure = supplier.get(); if (procedure != null) { @@ -313,7 +315,7 @@ public String getName() { } } - /* package */ Set> getMechanismsToReserve() { + /* package */ Set> 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 index 56067cfe..dad1c82a 100644 --- 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> mechanismsToUse = new HashSet<>(); + Set> 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> reservations = rule.getMechanismsToReserve(); + Set> 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 new file mode 100644 index 00000000..c88aadfe --- /dev/null +++ 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 + extends Mechanism { + 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 index 770a4efa..a6f1ad0c 100644 --- 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 extends Mechanism { - private ArrayList> submechanisms = new ArrayList<>(); +public abstract class Superstructure + extends Mechanism { + private ArrayList> submechanisms = new ArrayList<>(); @Override /* package */ void periodicInternal() { @@ -15,7 +16,7 @@ public abstract class Superstructure extends Mechanism { super.periodicInternal(); } - protected > M addMechanism(M submechanism) { + protected > 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 index f10dbf8f..6f67c1d5 100644 --- 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> checkSubsystemsAreMechanisms(Set requirements) { + private static Set> checkSubsystemsAreMechanisms(Set 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>) (Set) requirements; + return (Set>) (Set) requirements; } @Override diff --git a/src/test/java/com/team766/framework3/FakeMechanism.java b/src/test/java/com/team766/framework3/FakeMechanism.java index faf8ac3d..bafb0221 100644 --- 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 { - record FakeStatus(int currentState) implements Status {} +class FakeMechanism extends Mechanism { + 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 index 617d1a20..fb33a298 100644 --- 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> reservations) { + public FakeProcedure(int lifetime, Set> reservations) { super(reservations); this.lifetime = lifetime; } - public FakeProcedure(String name, int lifetime, Set> reservations) { + public FakeProcedure(String name, int lifetime, Set> 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 index ed7c429b..0ed0430c 100644 --- 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,42 @@ 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(); + // Status set from Initial request + assertEquals(new FakeStatus(-1), StatusBus.getStatusOrThrow(FakeStatus.class)); + assertEquals(new FakeStatus(-1), mech.getMechanismStatus()); + step(); + // Status set from Idle request + 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 { + class DummyMechanism extends Mechanism { @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 +141,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 +222,7 @@ protected FakeRequest getIdleRequest() { /// Test making a Mechanism part of a superstructure. @Test public void testSuperstructure() { - class TestSuperstructure extends Superstructure { + class TestSuperstructure extends Superstructure { // 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. @@ -212,14 +238,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 TestSuperstructure(); diff --git a/src/test/java/com/team766/framework3/RuleTest.java b/src/test/java/com/team766/framework3/RuleTest.java index 795b8395..f9c42f8a 100644 --- a/src/test/java/com/team766/framework3/RuleTest.java +++ b/src/test/java/com/team766/framework3/RuleTest.java @@ -123,9 +123,9 @@ public void testGetCancellation() { @Test public void testGetMechanismsToReserve() { - final Set> newlyMechanisms = + final Set> newlyMechanisms = Set.of(new FakeMechanism1(), new FakeMechanism2()); - final Set> finishedMechanisms = Set.of(new FakeMechanism()); + final Set> finishedMechanisms = Set.of(new FakeMechanism()); Rule duckDuckGooseGoose = getSingleElement(