WithStatus class hierarchy

Team766 · Dec 27, 2024 · 04cb8ae · 04cb8ae
1 parent 9357aa7
commit 04cb8ae
Show file tree

Hide file tree

Showing 10 changed files with 101 additions and 76 deletions.
diff --git 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<? extends MechanismSubsystem> 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<? extends MechanismSubsystem> 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
@@ -1,9 +1,9 @@
 package com.team766.framework3;
 
-import com.team766.logging.LoggerExceptionUtils;
 import java.util.NoSuchElementException;
 
-public abstract class MechanismWithStatus<S extends Record & Status> extends Mechanism {
+public abstract class MechanismWithStatus<S extends Record & Status> extends Mechanism
+        implements StatusSource<S> {
     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
@@ -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<S extends Record & Status>
-        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,20 +31,18 @@ public final void periodic() {
     @SuppressWarnings("unused")
     private final MechanismSubsystem outerSubsystem = new ProxySubsystem();
 
-    private S status = null;
+    private final ArrayList<Mechanism> facets = new ArrayList<>();
+    private final HashSet<MechanismSubsystem> facetSubsystems = new HashSet<>();
 
-    private ArrayList<Mechanism> facets = new ArrayList<>();
-    private HashSet<MechanismSubsystem> facetSubsystems = new HashSet<>();
-
-    protected <M extends Mechanism> M addFacet(M facet) {
+    protected final <M extends Mechanism> M addFacet(M facet) {
         Objects.requireNonNull(facet);
         facet.setContainer(this);
         addFacet(facet);
         facetSubsystems.addAll(facet.getReservableSubsystems());
         return facet;
     }
 
-    protected <M extends MultiFacetedMechanism<S>> Request<M> requestOfFacets(
+    protected final <M extends MultiFacetedMechanism> Request<M> 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<? extends MechanismSubsystem> getReservableSubsystems() {
+    public final Set<? extends MechanismSubsystem> 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
@@ -0,0 +1,23 @@
+package com.team766.framework3;
+
+import java.util.NoSuchElementException;
+
+public abstract class MultiFacetedMechanismWithStatus<S extends Record & Status>
+        extends MultiFacetedMechanism implements StatusSource<S> {
+    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
@@ -0,0 +1,5 @@
+package com.team766.framework3;
+
+public interface StatusSource<S extends Status> {
+    S getStatus();
+}
diff --git 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<S extends Record & Status> extends MechanismWithStatus<S> {
-    private ArrayList<Mechanism> submechanisms = new ArrayList<>();
+public abstract class Superstructure extends Mechanism {
+    private final ArrayList<Mechanism> 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 extends Mechanism> M addMechanism(M submechanism) {
+    protected final <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/SuperstructureWithStatus.java 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<S extends Record & Status> extends Superstructure
+        implements StatusSource<S> {
+    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
@@ -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<SwerveDrive.DriveStatus> {
+public class SwerveDrive extends MultiFacetedMechanismWithStatus<SwerveDrive.DriveStatus> {
     /**
      * @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
@@ -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<Arm.ArmStatus> {
-    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<Arm> requestExtended(PlacementPosition position, GamePieceType ga
     protected Request<Arm> startIdleRequest() {
         return requestHoldPosition();
     }
-
-    @Override
-    protected ArmStatus reportStatus() {
-        return new ArmStatus();
-    }
 }