refactor(superstructure): Improve checking at goal.

Author: haydenheroux

src/main/java/frc/robot/superstructure/Superstructure.java

@@ -58,23 +58,21 @@ public static Superstructure getInstance() {
 
   @Override
   public void periodic() {
-    updateMeasurement();
+    measurement = new SuperstructureState(arm.getState(), intake.getState(), shooter.getState());
+
+    SuperstructureMechanism.getInstance().updateSuperstructure(measurement);
   }
 
   @Override
   public void addToShuffleboard(ShuffleboardTab tab) {
     addStateToShuffleboard(tab, "Measurement", () -> measurement);
     addStateToShuffleboard(tab, "Goal", () -> goal);
 
-    ShuffleboardLayout state = Telemetry.addColumn(tab, "State");
-
-    state.addString(
+    Telemetry.addColumn(tab, "State").addString(
         "State",
         () -> this.getCurrentCommand() != null ? this.getCurrentCommand().getName() : "NONE");
 
-    ShuffleboardLayout at = Telemetry.addColumn(tab, "At Goal?");
-
-    at.addBoolean("At Goal?", () -> at(goal));
+    Telemetry.addColumn(tab, "At Goal?").addBoolean("At Goal?", this::atGoal);
   }
 
   private void addStateToShuffleboard(
@@ -105,16 +103,6 @@ private void addStateToShuffleboard(
         () -> state.get().shooterState().serializerVelocityRotationsPerSecond());
   }
 
-  private void updateMeasurement() {
-    measurement = new SuperstructureState(arm.getState(), intake.getState(), shooter.getState());
-
-    SuperstructureMechanism.getInstance().updateSuperstructure(measurement);
-  }
-
-  public SuperstructureState getState() {
-    return measurement;
-  }
-
   public void setPosition(SuperstructureState state) {
     arm.setPosition(state.armState());
   }
@@ -127,18 +115,18 @@ public void setGoal(SuperstructureState goal) {
     shooter.setGoal(goal.shooterState());
   }
 
-  public boolean at(SuperstructureState goal) {
-    updateMeasurement();
-
-    return measurement.atGoal(goal);
+  public boolean atGoal() {
+    return arm.atGoal()
+        && intake.atGoal()
+        && shooter.atGoal();
   }
 
   private Command hold(SuperstructureState goal) {
     return run(() -> setGoal(goal));
   }
 
   private Command to(SuperstructureState goal) {
-    return run(() -> setGoal(goal)).until(() -> at(goal));
+    return run(() -> setGoal(goal)).until(this::atGoal);
   }
 
   public Command stow() {

src/main/java/frc/robot/superstructure/SuperstructureState.java

@@ -1,10 +1,7 @@
 package frc.robot.superstructure;
 
-import frc.robot.arm.Arm;
 import frc.robot.arm.ArmState;
-import frc.robot.intake.Intake;
 import frc.robot.intake.IntakeState;
-import frc.robot.shooter.Shooter;
 import frc.robot.shooter.ShooterState;
 import java.util.Objects;
 
@@ -51,16 +48,4 @@ public record SuperstructureState(
     Objects.requireNonNull(intakeState);
     Objects.requireNonNull(shooterState);
   }
-
-  /**
-   * Returns true if at the superstructure goal.
-   *
-   * @param goal
-   * @return true if at the superstructure goal.
-   */
-  public boolean atGoal(SuperstructureState goal) {
-    return Arm.getInstance().atGoal()
-        && Intake.getInstance().atGoal()
-        && Shooter.getInstance().atGoal();
-  }
 }