From 76d8c551a49a1d19e9b5f09418f1512493f71897 Mon Sep 17 00:00:00 2001 From: Ryan Cahoon Date: Tue, 1 Oct 2024 04:35:48 -0700 Subject: [PATCH] Superstructures --- .../com/team766/framework3/Mechanism.java | 65 ++++++++++++++++-- .../team766/framework3/Superstructure.java | 24 +++++++ .../com/team766/framework3/MechanismTest.java | 67 +++++++++++++++++++ 3 files changed, 152 insertions(+), 4 deletions(-) create mode 100644 src/main/java/com/team766/framework3/Superstructure.java diff --git a/src/main/java/com/team766/framework3/Mechanism.java b/src/main/java/com/team766/framework3/Mechanism.java index a9396114..d35c9895 100644 --- a/src/main/java/com/team766/framework3/Mechanism.java +++ b/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> 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; } } diff --git a/src/main/java/com/team766/framework3/Superstructure.java b/src/main/java/com/team766/framework3/Superstructure.java new file mode 100644 index 00000000..a2fddea3 --- /dev/null +++ b/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> extends Mechanism { + private ArrayList> submechanisms = new ArrayList<>(); + + @Override + /* package */ void periodicInternal() { + for (var m : submechanisms) { + m.periodicInternal(); + } + + super.periodicInternal(); + } + + protected > M addMechanism(M submechanism) { + Objects.requireNonNull(submechanism); + submechanism.setSuperstructure(this); + submechanisms.add(submechanism); + return submechanism; + } +} diff --git a/src/test/java/com/team766/framework3/MechanismTest.java b/src/test/java/com/team766/framework3/MechanismTest.java index 9a96885a..ed7c429b 100644 --- a/src/test/java/com/team766/framework3/MechanismTest.java +++ b/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 { + // 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"); + } }