From 04cb8ae0f5ffe0b2317b38a50ffd23ef213c5755 Mon Sep 17 00:00:00 2001 From: Ryan Cahoon Date: Fri, 27 Dec 2024 03:55:20 -0800 Subject: [PATCH] WithStatus class hierarchy --- .../com/team766/framework3/Mechanism.java | 36 +++++++++++------- .../framework3/MechanismWithStatus.java | 17 +++------ .../framework3/MultiFacetedMechanism.java | 37 +++++++------------ .../MultiFacetedMechanismWithStatus.java | 23 ++++++++++++ .../com/team766/framework3/StatusSource.java | 5 +++ .../team766/framework3/Superstructure.java | 12 +++--- .../framework3/SuperstructureWithStatus.java | 23 ++++++++++++ .../robot/common/mechanisms/SwerveDrive.java | 4 +- .../robot/gatorade/mechanisms/Arm.java | 10 +---- .../robot/reva/mechanisms/ArmAndClimber.java | 10 +---- 10 files changed, 101 insertions(+), 76 deletions(-) create mode 100644 src/main/java/com/team766/framework3/MultiFacetedMechanismWithStatus.java create mode 100644 src/main/java/com/team766/framework3/StatusSource.java create mode 100644 src/main/java/com/team766/framework3/SuperstructureWithStatus.java diff --git a/src/main/java/com/team766/framework3/Mechanism.java b/src/main/java/com/team766/framework3/Mechanism.java index 25b2fcac..0bf89a1f 100644 --- a/src/main/java/com/team766/framework3/Mechanism.java +++ b/src/main/java/com/team766/framework3/Mechanism.java @@ -12,7 +12,7 @@ import java.util.Set; import java.util.StringJoiner; -class requestAllOf_must_have_at_least_one_argument {} +final class requestAllOf_must_have_at_least_one_argument {} public abstract class Mechanism implements Reservable, LoggingBase { @FunctionalInterface @@ -45,11 +45,11 @@ default String getProvenance() { */ @DoNotCall @Deprecated - protected static requestAllOf_must_have_at_least_one_argument requestAllOf() { + protected static final requestAllOf_must_have_at_least_one_argument requestAllOf() { return null; } - protected static Directive requestAllOf(Directive... directives) { + protected static final Directive requestAllOf(Directive... directives) { var sj = new StringJoiner("; ", "{", "}"); for (int i = 0; i < directives.length; ++i) { sj.add(directives[i].getProvenance()); @@ -74,7 +74,7 @@ public String getProvenance() { }; } - protected static Directive dontWaitFor(Directive directive) { + protected static final Directive dontWaitFor(Directive directive) { return new Directive() { @Override public boolean update() { @@ -126,7 +126,7 @@ Reservable getMechanism() { } } - private class ProxySubsystem extends SubsystemBase implements MechanismSubsystem { + private final class ProxySubsystem extends SubsystemBase implements MechanismSubsystem { @Override public Reservable getMechanism() { return Mechanism.this; @@ -158,8 +158,8 @@ public final void periodic() { private boolean isRunningPeriodic = false; - private Superstructure superstructure = null; - private MultiFacetedMechanism container = null; + private Superstructure superstructure = null; + private MultiFacetedMechanism container = null; private MechanismRequest request = null; @@ -211,7 +211,7 @@ public Category getLoggerCategory() { * * @param superstructure The superstructure this Mechanism is part of. */ - /* package */ void setSuperstructure(Superstructure superstructure) { + /* package */ final void setSuperstructure(Superstructure superstructure) { Objects.requireNonNull(superstructure); if (this.superstructure != null) { throw new IllegalStateException("Mechanism is already part of a superstructure"); @@ -231,7 +231,7 @@ public Category getLoggerCategory() { /** * Indicate that this Mechanism is part of a MultiFacetedMechanism. */ - /* package */ void setContainer(MultiFacetedMechanism container) { + /* package */ final void setContainer(MultiFacetedMechanism container) { Objects.requireNonNull(container); if (this.superstructure != null) { throw new IllegalStateException("Mechanism is already part of a Superstructure"); @@ -266,12 +266,12 @@ protected Request startIdleRequest() { return null; } - /* package */ boolean isRunningPeriodic() { + /* package */ final boolean isRunningPeriodic() { return isRunningPeriodic; } @Override - public void checkContextReservation() { + public final void checkContextReservation() { if (isRunningPeriodic()) { return; } @@ -299,8 +299,12 @@ public final Set getReservableSubsystems() { return Set.of(subsystem); } - /* package */ void periodicInternal() { + /* package */ final void periodicInternal() { try { + runSubmechanisms(); + + publishStatus(); + isRunningPeriodic = true; request.runDirective(); } catch (Exception ex) { @@ -311,8 +315,14 @@ public final Set getReservableSubsystems() { } } + // Overridden in MechanismWithStatus + /* package */ void publishStatus() {} + + // Overridden in Superstructure + /* package */ void runSubmechanisms() {} + @Override - public String toString() { + public final String toString() { return getName(); } } diff --git a/src/main/java/com/team766/framework3/MechanismWithStatus.java b/src/main/java/com/team766/framework3/MechanismWithStatus.java index 21ff8060..3c2286ce 100644 --- a/src/main/java/com/team766/framework3/MechanismWithStatus.java +++ b/src/main/java/com/team766/framework3/MechanismWithStatus.java @@ -1,9 +1,9 @@ package com.team766.framework3; -import com.team766.logging.LoggerExceptionUtils; import java.util.NoSuchElementException; -public abstract class MechanismWithStatus extends Mechanism { +public abstract class MechanismWithStatus extends Mechanism + implements StatusSource { private S status = null; public final S getStatus() { @@ -14,16 +14,9 @@ public final S getStatus() { } @Override - /* package */ void periodicInternal() { - try { - status = reportStatus(); - StatusBus.getInstance().publishStatus(status); - } catch (Exception ex) { - ex.printStackTrace(); - LoggerExceptionUtils.logException(ex); - return; - } - super.periodicInternal(); + /* package */ final void publishStatus() { + status = reportStatus(); + StatusBus.getInstance().publishStatus(status); } protected abstract S reportStatus(); diff --git a/src/main/java/com/team766/framework3/MultiFacetedMechanism.java b/src/main/java/com/team766/framework3/MultiFacetedMechanism.java index 59ab5dc3..b9cd75ea 100644 --- a/src/main/java/com/team766/framework3/MultiFacetedMechanism.java +++ b/src/main/java/com/team766/framework3/MultiFacetedMechanism.java @@ -5,13 +5,11 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.ArrayList; import java.util.HashSet; -import java.util.NoSuchElementException; import java.util.Objects; import java.util.Set; -public abstract class MultiFacetedMechanism - implements Reservable, LoggingBase { - private class ProxySubsystem extends SubsystemBase implements MechanismSubsystem { +public abstract class MultiFacetedMechanism implements Reservable, LoggingBase { + private final class ProxySubsystem extends SubsystemBase implements MechanismSubsystem { @Override public Reservable getMechanism() { return MultiFacetedMechanism.this; @@ -33,12 +31,10 @@ public final void periodic() { @SuppressWarnings("unused") private final MechanismSubsystem outerSubsystem = new ProxySubsystem(); - private S status = null; + private final ArrayList facets = new ArrayList<>(); + private final HashSet facetSubsystems = new HashSet<>(); - private ArrayList facets = new ArrayList<>(); - private HashSet facetSubsystems = new HashSet<>(); - - protected M addFacet(M facet) { + protected final M addFacet(M facet) { Objects.requireNonNull(facet); facet.setContainer(this); addFacet(facet); @@ -46,7 +42,7 @@ protected M addFacet(M facet) { return facet; } - protected > Request requestOfFacets( + protected final Request requestOfFacets( Request... facetRequests) { for (var facetRequest : facetRequests) { if (!facets.contains(facetRequest.getMechanism())) { @@ -85,7 +81,7 @@ Reservable getMechanism() { }; } - public void checkContextReservation() { + public final void checkContextReservation() { if (facetSubsystems.isEmpty()) { throw new IllegalStateException("MultiFacetedMechanism does not have any Facets"); } @@ -94,15 +90,8 @@ public void checkContextReservation() { } } - public final S getStatus() { - if (status == null) { - throw new NoSuchElementException(getName() + " has not published a status yet"); - } - return status; - } - @Override - public Set getReservableSubsystems() { + public final Set getReservableSubsystems() { if (facetSubsystems.isEmpty()) { throw new IllegalStateException("MultiFacetedMechanism does not have any Facets"); } @@ -114,14 +103,13 @@ public Category getLoggerCategory() { return Category.MECHANISMS; } - /* package */ void periodicInternal() { + /* package */ final void periodicInternal() { for (var m : facets) { m.periodicInternal(); } try { - status = reportStatus(); - StatusBus.getInstance().publishStatus(status); + publishStatus(); run(); } catch (Exception ex) { @@ -130,12 +118,13 @@ public Category getLoggerCategory() { } } - protected abstract S reportStatus(); + // Overridden in MechanismWithStatus + /* package */ void publishStatus() {} protected abstract void run(); @Override - public String toString() { + public final String toString() { return getName(); } } diff --git a/src/main/java/com/team766/framework3/MultiFacetedMechanismWithStatus.java b/src/main/java/com/team766/framework3/MultiFacetedMechanismWithStatus.java new file mode 100644 index 00000000..e014524d --- /dev/null +++ b/src/main/java/com/team766/framework3/MultiFacetedMechanismWithStatus.java @@ -0,0 +1,23 @@ +package com.team766.framework3; + +import java.util.NoSuchElementException; + +public abstract class MultiFacetedMechanismWithStatus + extends MultiFacetedMechanism implements StatusSource { + private S status = null; + + public final S getStatus() { + if (status == null) { + throw new NoSuchElementException(getName() + " has not published a status yet"); + } + return status; + } + + @Override + /* package */ final void publishStatus() { + status = reportStatus(); + StatusBus.getInstance().publishStatus(status); + } + + protected abstract S reportStatus(); +} diff --git a/src/main/java/com/team766/framework3/StatusSource.java b/src/main/java/com/team766/framework3/StatusSource.java new file mode 100644 index 00000000..ea6076dd --- /dev/null +++ b/src/main/java/com/team766/framework3/StatusSource.java @@ -0,0 +1,5 @@ +package com.team766.framework3; + +public interface StatusSource { + S getStatus(); +} diff --git a/src/main/java/com/team766/framework3/Superstructure.java b/src/main/java/com/team766/framework3/Superstructure.java index eef775a0..85015832 100644 --- a/src/main/java/com/team766/framework3/Superstructure.java +++ b/src/main/java/com/team766/framework3/Superstructure.java @@ -13,19 +13,17 @@ * Superstructure should set requests on its constituent Mechanisms (in its * {@link #run(R, boolean)} method). */ -public abstract class Superstructure extends MechanismWithStatus { - private ArrayList submechanisms = new ArrayList<>(); +public abstract class Superstructure extends Mechanism { + private final ArrayList submechanisms = new ArrayList<>(); @Override - /* package */ void periodicInternal() { + /* package */ final void runSubmechanisms() { for (var m : submechanisms) { m.periodicInternal(); } - - super.periodicInternal(); } - protected Directive requestOfSubmechanism(Request submechanismRequest) { + protected final Directive requestOfSubmechanism(Request submechanismRequest) { if (!submechanisms.contains(submechanismRequest.getMechanism())) { throw new IllegalArgumentException( "Request is for " @@ -46,7 +44,7 @@ public String getProvenance() { }; } - protected M addMechanism(M submechanism) { + protected final M addMechanism(M submechanism) { Objects.requireNonNull(submechanism); submechanism.setSuperstructure(this); submechanisms.add(submechanism); diff --git a/src/main/java/com/team766/framework3/SuperstructureWithStatus.java b/src/main/java/com/team766/framework3/SuperstructureWithStatus.java new file mode 100644 index 00000000..ef9594d5 --- /dev/null +++ b/src/main/java/com/team766/framework3/SuperstructureWithStatus.java @@ -0,0 +1,23 @@ +package com.team766.framework3; + +import java.util.NoSuchElementException; + +public abstract class SuperstructureWithStatus extends Superstructure + implements StatusSource { + private S status = null; + + public final S getStatus() { + if (status == null) { + throw new NoSuchElementException(getName() + " has not published a status yet"); + } + return status; + } + + @Override + /* package */ final void publishStatus() { + status = reportStatus(); + StatusBus.getInstance().publishStatus(status); + } + + protected abstract S reportStatus(); +} diff --git a/src/main/java/com/team766/robot/common/mechanisms/SwerveDrive.java b/src/main/java/com/team766/robot/common/mechanisms/SwerveDrive.java index c7606abe..3051e082 100644 --- a/src/main/java/com/team766/robot/common/mechanisms/SwerveDrive.java +++ b/src/main/java/com/team766/robot/common/mechanisms/SwerveDrive.java @@ -9,7 +9,7 @@ import com.team766.config.ConfigFileReader; import com.team766.controllers.PIDController; import com.team766.framework3.Mechanism; -import com.team766.framework3.MultiFacetedMechanism; +import com.team766.framework3.MultiFacetedMechanismWithStatus; import com.team766.framework3.Request; import com.team766.framework3.Status; import com.team766.hal.GyroReader; @@ -33,7 +33,7 @@ import java.util.Optional; import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; -public class SwerveDrive extends MultiFacetedMechanism { +public class SwerveDrive extends MultiFacetedMechanismWithStatus { /** * @param heading current heading in degrees */ diff --git a/src/main/java/com/team766/robot/gatorade/mechanisms/Arm.java b/src/main/java/com/team766/robot/gatorade/mechanisms/Arm.java index 58322670..dfbba027 100644 --- a/src/main/java/com/team766/robot/gatorade/mechanisms/Arm.java +++ b/src/main/java/com/team766/robot/gatorade/mechanisms/Arm.java @@ -1,14 +1,11 @@ package com.team766.robot.gatorade.mechanisms; import com.team766.framework3.Request; -import com.team766.framework3.Status; import com.team766.framework3.Superstructure; import com.team766.robot.gatorade.PlacementPosition; import com.team766.robot.gatorade.mechanisms.Intake.GamePieceType; -public class Arm extends Superstructure { - public record ArmStatus() implements Status {} - +public class Arm extends Superstructure { private final Shoulder shoulder; private final Elevator elevator; private final Wrist wrist; @@ -175,9 +172,4 @@ public Request requestExtended(PlacementPosition position, GamePieceType ga protected Request startIdleRequest() { return requestHoldPosition(); } - - @Override - protected ArmStatus reportStatus() { - return new ArmStatus(); - } } diff --git a/src/main/java/com/team766/robot/reva/mechanisms/ArmAndClimber.java b/src/main/java/com/team766/robot/reva/mechanisms/ArmAndClimber.java index 8fa17d38..c84d5ef7 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/ArmAndClimber.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/ArmAndClimber.java @@ -1,12 +1,9 @@ package com.team766.robot.reva.mechanisms; import com.team766.framework3.Request; -import com.team766.framework3.Status; import com.team766.framework3.Superstructure; -public class ArmAndClimber extends Superstructure { - public record SuperstructureStatus() implements Status {} - +public class ArmAndClimber extends Superstructure { public Request requestShoulderPosition(double targetPosition) { return startRequest( () -> { @@ -109,9 +106,4 @@ public void resetClimberPositions() { climber.resetLeftPosition(); climber.resetRightPosition(); } - - @Override - protected SuperstructureStatus reportStatus() { - return new SuperstructureStatus(); - } }