From 5f32fb194c4c4376f60ab093a79093a66c97215d 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 | 62 +++++++++++++++++ .../com/team766/framework3/MechanismTest.java | 69 +++++++++++++++++++ 2 files changed, 131 insertions(+) diff --git a/src/main/java/com/team766/framework3/Mechanism.java b/src/main/java/com/team766/framework3/Mechanism.java index 3b2a1263..68df7344 100644 --- a/src/main/java/com/team766/framework3/Mechanism.java +++ b/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> 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> 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,6 +124,22 @@ 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); } @@ -97,7 +147,19 @@ protected void checkContextReservation() { 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()); diff --git a/src/test/java/com/team766/framework3/MechanismTest.java b/src/test/java/com/team766/framework3/MechanismTest.java index 9a96885a..1b82b47f 100644 --- a/src/test/java/com/team766/framework3/MechanismTest.java +++ b/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 { + // 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"); + } }