From ac04ccc06233967ac94901a47d8d996b083100e1 Mon Sep 17 00:00:00 2001 From: Ryan Cahoon Date: Wed, 25 Dec 2024 00:53:50 -0800 Subject: [PATCH] Revert "Status handles" --- .../com/team766/framework3/Mechanism.java | 5 +- .../framework3/MultiFacetedMechanism.java | 12 +++- .../com/team766/framework3/StatusBus.java | 26 ++++--- .../com/team766/framework3/StatusHandle.java | 3 - .../com/team766/framework3/StatusSource.java | 13 ---- .../com/team766/framework3/StatusesMixin.java | 68 +++++++++---------- .../com/team766/library/ReflectionUtils.java | 17 ----- .../team766/robot/burro_elevator/Lights.java | 4 +- .../robot/common/procedures/FollowPath.java | 4 +- .../com/team766/robot/gatorade/Lights.java | 8 +-- .../java/com/team766/robot/gatorade/OI.java | 3 +- .../gatorade/procedures/GyroBalance.java | 6 +- .../java/com/team766/robot/reva/BoxOpOI.java | 2 +- .../java/com/team766/robot/reva/Lights.java | 2 +- .../reva/VisionUtil/VisionSpeakerHelper.java | 4 +- .../robot/reva/procedures/DriverShootNow.java | 2 +- .../DriverShootVelocityAndIntake.java | 2 +- .../robot/reva/procedures/IntakeUntilIn.java | 8 +-- .../robot/reva/procedures/PickupNote.java | 4 +- .../robot/reva/procedures/ShootNow.java | 2 +- .../procedures/ShootingProcedureStatus.java | 3 +- 21 files changed, 83 insertions(+), 115 deletions(-) delete mode 100644 src/main/java/com/team766/framework3/StatusHandle.java delete mode 100644 src/main/java/com/team766/framework3/StatusSource.java delete mode 100644 src/main/java/com/team766/library/ReflectionUtils.java diff --git a/src/main/java/com/team766/framework3/Mechanism.java b/src/main/java/com/team766/framework3/Mechanism.java index 499a42bf1..3579ac868 100644 --- a/src/main/java/com/team766/framework3/Mechanism.java +++ b/src/main/java/com/team766/framework3/Mechanism.java @@ -13,7 +13,7 @@ import java.util.Set; public abstract class Mechanism - implements Reservable, StatusSource, LoggingBase { + implements Reservable, LoggingBase { @FunctionalInterface public interface Directive { /** @@ -205,7 +205,6 @@ public final Set getReservableSubsystems() { return Set.of(subsystem); } - @Override public final S getStatus() { if (status == null) { throw new NoSuchElementException(getName() + " has not published a status yet"); @@ -216,7 +215,7 @@ public final S getStatus() { /* package */ void periodicInternal() { try { status = reportStatus(); - publishStatus(status); + StatusBus.getInstance().publishStatus(status); isRunningPeriodic = true; request.runDirective(); diff --git a/src/main/java/com/team766/framework3/MultiFacetedMechanism.java b/src/main/java/com/team766/framework3/MultiFacetedMechanism.java index 82a725907..f431fc97c 100644 --- a/src/main/java/com/team766/framework3/MultiFacetedMechanism.java +++ b/src/main/java/com/team766/framework3/MultiFacetedMechanism.java @@ -1,6 +1,7 @@ package com.team766.framework3; import java.util.HashSet; +import java.util.NoSuchElementException; import java.util.Objects; import java.util.Set; import com.team766.framework.StackTraceUtils; @@ -12,7 +13,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public abstract class MultiFacetedMechanism - implements StatusSource, Reservable, LoggingBase { + implements Reservable, LoggingBase { @SuppressWarnings("unused") private SubsystemBase outerSubsystem = new SubsystemBase() { @@ -61,6 +62,13 @@ 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() { return facetSubsystems; @@ -74,7 +82,7 @@ public Category getLoggerCategory() { /* package */ void periodicInternal() { try { status = reportStatus(); - publishStatus(status); + StatusBus.getInstance().publishStatus(status); run(); } catch (Exception ex) { diff --git a/src/main/java/com/team766/framework3/StatusBus.java b/src/main/java/com/team766/framework3/StatusBus.java index 1686f480e..100ccd05d 100644 --- a/src/main/java/com/team766/framework3/StatusBus.java +++ b/src/main/java/com/team766/framework3/StatusBus.java @@ -19,7 +19,7 @@ public class StatusBus { private static StatusBus s_instance = new StatusBus(); - private final Map>, Entry> statuses = new LinkedHashMap<>(); + private final Map, Entry> statuses = new LinkedHashMap<>(); /** * Get the Singleton instance of the {@link StatusBus}. @@ -51,10 +51,9 @@ public void clear() { * * This method also logs the Status to diagnostic logs. */ - public , S extends Record & Status> Entry publishStatus( - Class handle, S status) { + public Entry publishStatus(S status) { var entry = new Entry<>(status, RobotProvider.instance.getClock().getTime()); - statuses.put(handle, entry); + statuses.put(status.getClass(), entry); // TODO(MF3): also publish to data logs Logger.get(Category.FRAMEWORK) .logRaw( @@ -79,9 +78,8 @@ public , S extends Record & Status> Entry publishSt * Optional if the {@link Status} hasn't been published. */ @SuppressWarnings("unchecked") - public , S extends Status> Optional> getStatusEntry( - Class handle) { - return Optional.ofNullable((Entry) statuses.get(handle)); + public Optional> getStatusEntry(Class statusClass) { + return Optional.ofNullable((Entry) statuses.get(statusClass)); } /** @@ -96,8 +94,8 @@ public , S extends Status> Optional> getStatu * The latest published {@link Status} or an empty Optional if the {@link Status} hasn't * been published. */ - public , S extends Status> Optional getStatus(Class handle) { - return getStatusEntry(handle).map(Entry::status); + public Optional getStatus(Class statusClass) { + return getStatusEntry(statusClass).map(Entry::status); } /** @@ -111,8 +109,8 @@ public , S extends Status> Optional getStatus(Class * @return The latest published {@link Status} * @throws NoSuchElementException if the {@link Status} hasn't been published */ - public , S extends Status> S getStatusOrThrow(Class handle) { - return getStatus(handle).orElseThrow(); + public S getStatusOrThrow(Class statusClass) { + return getStatus(statusClass).orElseThrow(); } /** @@ -128,8 +126,8 @@ public , S extends Status> S getStatusOrThrow(Class * The result of the Function applied to the latest published {@link Status} or an empty * Optional if the {@link Status} hasn't been published. */ - public , S extends Status, V> Optional getStatusValue( - Class handle, Function getter) { - return getStatusEntry(handle).map(s -> getter.apply(s.status())); + public Optional getStatusValue( + Class statusClass, Function getter) { + return getStatusEntry(statusClass).map(s -> getter.apply(s.status())); } } diff --git a/src/main/java/com/team766/framework3/StatusHandle.java b/src/main/java/com/team766/framework3/StatusHandle.java deleted file mode 100644 index 94f9fc147..000000000 --- a/src/main/java/com/team766/framework3/StatusHandle.java +++ /dev/null @@ -1,3 +0,0 @@ -package com.team766.framework3; - -public interface StatusHandle {} diff --git a/src/main/java/com/team766/framework3/StatusSource.java b/src/main/java/com/team766/framework3/StatusSource.java deleted file mode 100644 index 88aea1d2c..000000000 --- a/src/main/java/com/team766/framework3/StatusSource.java +++ /dev/null @@ -1,13 +0,0 @@ -package com.team766.framework3; - -import com.team766.library.ReflectionUtils; - -public interface StatusSource extends StatusHandle { - default S getStatus() { - return StatusBus.getInstance().getStatusOrThrow(ReflectionUtils.getClass(this)); - } - - default void publishStatus(S status) { - StatusBus.getInstance().publishStatus(ReflectionUtils.getClass(this), status); - } -} diff --git a/src/main/java/com/team766/framework3/StatusesMixin.java b/src/main/java/com/team766/framework3/StatusesMixin.java index 6f3308627..404932f9c 100644 --- a/src/main/java/com/team766/framework3/StatusesMixin.java +++ b/src/main/java/com/team766/framework3/StatusesMixin.java @@ -6,58 +6,58 @@ import java.util.Optional; import java.util.function.Function; import java.util.function.Predicate; -import com.team766.library.ReflectionUtils; public interface StatusesMixin { - default > - StatusBus.Entry publishStatus(SH status) { - return StatusBus.getInstance().publishStatus(ReflectionUtils.getClass(status), status); + /** + * @see StatusBus#publishStatus(Record) + */ + default StatusBus.Entry publishStatus(S status) { + return StatusBus.getInstance().publishStatus(status); } /** * @see StatusBus#getStatusEntry(Class) */ - default , S extends Status> - Optional> getStatusEntry(Class handle) { - return StatusBus.getInstance().getStatusEntry(handle); + default Optional> getStatusEntry(Class statusClass) { + return StatusBus.getInstance().getStatusEntry(statusClass); } /** * @see StatusBus#getStatus(Class) */ - default , S extends Status> Optional getStatus(Class handle) { - return StatusBus.getInstance().getStatus(handle); + default Optional getStatus(Class statusClass) { + return StatusBus.getInstance().getStatus(statusClass); } /** * @see StatusBus#getStatusOrThrow(Class) */ - default , S extends Status> S getStatusOrThrow(Class handle) { - return StatusBus.getInstance().getStatusOrThrow(handle); + default S getStatusOrThrow(Class statusClass) { + return StatusBus.getInstance().getStatusOrThrow(statusClass); } /** * @see StatusBus#getStatusValue(Class, Function) */ - default , S extends Status, V> Optional getStatusValue( - Class handle, Function getter) { - return StatusBus.getInstance().getStatusValue(handle, getter); + default Optional getStatusValue( + Class statusClass, Function getter) { + return StatusBus.getInstance().getStatusValue(statusClass, getter); } /** * Predicate that checks whether or not a {@link Status} with the given class has been published */ - default , S extends Status> boolean checkForStatus(Class handle) { - return getStatusEntry(handle).isPresent(); + default boolean checkForStatus(Class statusClass) { + return getStatusEntry(statusClass).isPresent(); } /** * Predicate that checks whether or not the latest {@link Status} with the given class passes * the check provided by {@code predicate}. */ - default , S extends Status> boolean checkForStatusWith( - Class handle, Predicate predicate) { - return getStatusValue(handle, predicate::test).orElse(false); + default boolean checkForStatusWith( + Class statusClass, Predicate predicate) { + return getStatusValue(statusClass, predicate::test).orElse(false); } /** @@ -65,18 +65,18 @@ default , S extends Status> boolean checkForStatusWith * the check provided by {@code predicate}, including additional metadata about how the Status * was published. */ - default , S extends Status> boolean checkForStatusEntryWith( - Class handle, Predicate> predicate) { - return getStatusEntry(handle).map(predicate::test).orElse(false); + default boolean checkForStatusEntryWith( + Class statusClass, Predicate> predicate) { + return getStatusEntry(statusClass).map(predicate::test).orElse(false); } /** * Suspend the Procedure until a {@link Status} with the given class has been published, then * return that Status. */ - default , S extends Status> S waitForStatus( - Context context, Class handle) { - return waitForValue(context, () -> getStatus(handle)); + default S waitForStatus( + Context context, Class statusClass) { + return waitForValue(context, () -> getStatus(statusClass)); } /** @@ -84,18 +84,18 @@ default , S extends Status> S waitForStatus( * we've waited for at least {@code timeoutSeconds}. Returns an Optional containing the Status * if one was published, or return an empty Optional if the timeout was reached. */ - default , S extends Status> Optional waitForStatusOrTimeout( - Context context, Class handle, double timeoutSeconds) { - return waitForValueOrTimeout(context, () -> getStatus(handle), timeoutSeconds); + default Optional waitForStatusOrTimeout( + Context context, Class statusClass, double timeoutSeconds) { + return waitForValueOrTimeout(context, () -> getStatus(statusClass), timeoutSeconds); } /** * Suspend the Procedure until a {@link Status} with the given class has been published that * passes the check provided by {@code predicate}, then return that Status. */ - default , S extends Status> S waitForStatusWith( - Context context, Class handle, Predicate predicate) { - return waitForValue(context, () -> getStatus(handle).filter(predicate)); + default S waitForStatusWith( + Context context, Class statusClass, Predicate predicate) { + return waitForValue(context, () -> getStatus(statusClass).filter(predicate)); } /** @@ -104,9 +104,9 @@ default , S extends Status> S waitForStatusWith( * {@code timeoutSeconds}. Returns an Optional containing the Status if one was published, or * return an empty Optional if the timeout was reached. */ - default , S extends Status> Optional waitForStatusWithOrTimeout( - Context context, Class handle, Predicate predicate, double timeoutSeconds) { + default Optional waitForStatusWithOrTimeout( + Context context, Class statusClass, Predicate predicate, double timeoutSeconds) { return waitForValueOrTimeout( - context, () -> getStatus(handle).filter(predicate), timeoutSeconds); + context, () -> getStatus(statusClass).filter(predicate), timeoutSeconds); } } diff --git a/src/main/java/com/team766/library/ReflectionUtils.java b/src/main/java/com/team766/library/ReflectionUtils.java deleted file mode 100644 index 6362ab287..000000000 --- a/src/main/java/com/team766/library/ReflectionUtils.java +++ /dev/null @@ -1,17 +0,0 @@ -package com.team766.library; - -public class ReflectionUtils { - /** - * Works the same as {@code object.getClass()}, but the return type has the full, unerased type. - * - * For example, {@code getClass(new List())} will return a - * {@code Class>}, but {@code new List().getClass()} will return - * a {@code Class}. - * - * See https://stackoverflow.com/a/65020013 for discussion. - */ - @SuppressWarnings("unchecked") - public static Class getClass(T object) { - return (Class) object.getClass(); - } -} diff --git a/src/main/java/com/team766/robot/burro_elevator/Lights.java b/src/main/java/com/team766/robot/burro_elevator/Lights.java index 8d74c0fb7..0801c8b79 100644 --- a/src/main/java/com/team766/robot/burro_elevator/Lights.java +++ b/src/main/java/com/team766/robot/burro_elevator/Lights.java @@ -31,7 +31,7 @@ public Lights() { "Elevator top", () -> checkForStatusWith( - Elevator.class, + Elevator.ElevatorStatus.class, s -> s.position() >= Elevator.TOP_POSITION)) .withOnTriggeringProcedure(ONCE_AND_HOLD, Set.of(), () -> elevatorAtTop())); @@ -40,7 +40,7 @@ public Lights() { "Elevator bottom", () -> checkForStatusWith( - Elevator.class, + Elevator.ElevatorStatus.class, s -> s.position() <= Elevator.BOTTOM_POSITION)) .withOnTriggeringProcedure( ONCE_AND_HOLD, Set.of(), () -> elevatorAtBottom())); diff --git a/src/main/java/com/team766/robot/common/procedures/FollowPath.java b/src/main/java/com/team766/robot/common/procedures/FollowPath.java index 7f3b6d54e..afcc0d9a7 100644 --- a/src/main/java/com/team766/robot/common/procedures/FollowPath.java +++ b/src/main/java/com/team766/robot/common/procedures/FollowPath.java @@ -46,7 +46,7 @@ public void run(Context context) { // intitialization - var driveStatus = waitForStatus(context, SwerveDrive.class); + var driveStatus = waitForStatus(context, SwerveDrive.DriveStatus.class); Pose2d curPose = driveStatus.currentPosition(); ChassisSpeeds currentSpeeds = driveStatus.chassisSpeeds(); @@ -67,7 +67,7 @@ public void run(Context context) { while (!timer.hasElapsed(generatedTrajectory.getTotalTimeSeconds())) { double currentTime = timer.get(); PathPlannerTrajectory.State targetState = generatedTrajectory.sample(currentTime); - driveStatus = waitForStatus(context, SwerveDrive.class); + driveStatus = waitForStatus(context, SwerveDrive.DriveStatus.class); curPose = driveStatus.currentPosition(); currentSpeeds = driveStatus.chassisSpeeds(); diff --git a/src/main/java/com/team766/robot/gatorade/Lights.java b/src/main/java/com/team766/robot/gatorade/Lights.java index deb86b1a1..2d3578d62 100644 --- a/src/main/java/com/team766/robot/gatorade/Lights.java +++ b/src/main/java/com/team766/robot/gatorade/Lights.java @@ -23,12 +23,12 @@ public Lights() { addRule( Rule.create( "OI State Updated", - () -> checkForStatusEntryWith(OI.class, s -> s.age() < 1.3)) + () -> checkForStatusEntryWith(OI.OIStatus.class, s -> s.age() < 1.3)) .withOnTriggeringProcedure( REPEATEDLY, Set.of(), () -> { - final OI.OIStatus status = getStatusOrThrow(OI.class); + final OI.OIStatus status = getStatusOrThrow(OI.OIStatus.class); setLightsForGamePiece(status.gamePieceType()); setLightsForPlacement( status.placementPosition(), status.gamePieceType()); @@ -43,12 +43,12 @@ public Lights() { .withOnTriggeringProcedure(ONCE_AND_HOLD, Set.of(), () -> rainbow())); addRule( - Rule.create("Default display", () -> checkForStatus(OI.class)) + Rule.create("Default display", () -> checkForStatus(OI.OIStatus.class)) .withOnTriggeringProcedure( REPEATEDLY, Set.of(), () -> { - final OI.OIStatus status = getStatusOrThrow(OI.class); + final OI.OIStatus status = getStatusOrThrow(OI.OIStatus.class); setLightsForGamePiece(status.gamePieceType()); setLightsForPlacement( status.placementPosition(), status.gamePieceType()); diff --git a/src/main/java/com/team766/robot/gatorade/OI.java b/src/main/java/com/team766/robot/gatorade/OI.java index eb751398f..ade11c178 100644 --- a/src/main/java/com/team766/robot/gatorade/OI.java +++ b/src/main/java/com/team766/robot/gatorade/OI.java @@ -5,7 +5,6 @@ import com.team766.framework3.Rule; import com.team766.framework3.RuleEngine; import com.team766.framework3.Status; -import com.team766.framework3.StatusSource; import com.team766.hal.JoystickReader; import com.team766.hal.RobotProvider; import com.team766.robot.common.DriverOI; @@ -20,7 +19,7 @@ * This class is the glue that binds the controls on the physical operator interface to the code * that allow control of the robot. */ -public class OI extends RuleEngine implements StatusSource { +public class OI extends RuleEngine { public record OIStatus(GamePieceType gamePieceType, PlacementPosition placementPosition) implements Status {} diff --git a/src/main/java/com/team766/robot/gatorade/procedures/GyroBalance.java b/src/main/java/com/team766/robot/gatorade/procedures/GyroBalance.java index 8a99666fa..90809e277 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/GyroBalance.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/GyroBalance.java @@ -51,8 +51,8 @@ public GyroBalance(Alliance alliance, SwerveDrive drive, Arm arm) { } private double getAbsoluteTilt() { - final double pitch = getStatusOrThrow(SwerveDrive.class).pitch(); - final double roll = getStatusOrThrow(SwerveDrive.class).roll(); + final double pitch = getStatusOrThrow(SwerveDrive.DriveStatus.class).pitch(); + final double roll = getStatusOrThrow(SwerveDrive.DriveStatus.class).roll(); return Math.toDegrees( Math.acos(Math.cos(Math.toRadians(roll) * Math.cos(Math.toRadians(pitch))))); } @@ -62,7 +62,7 @@ public void run(Context context) { waitForRequest(context, arm.requestExtendedToMid()); // initialY is robot y position when balancing starts - final double initialY = getStatusOrThrow(SwerveDrive.class).currentPosition().getY(); + final double initialY = getStatusOrThrow(SwerveDrive.DriveStatus.class).currentPosition().getY(); // Sets movement direction towards desired charge station. switch (alliance) { case Red: diff --git a/src/main/java/com/team766/robot/reva/BoxOpOI.java b/src/main/java/com/team766/robot/reva/BoxOpOI.java index bb8ebbe57..89758d44f 100644 --- a/src/main/java/com/team766/robot/reva/BoxOpOI.java +++ b/src/main/java/com/team766/robot/reva/BoxOpOI.java @@ -107,7 +107,7 @@ public BoxOpOI( "Spin shooter for assist shot", () -> checkForStatusWith( - Shooter.class, + Shooter.ShooterStatus.class, s -> s .targetSpeed() diff --git a/src/main/java/com/team766/robot/reva/Lights.java b/src/main/java/com/team766/robot/reva/Lights.java index c1e10f531..5662493ac 100644 --- a/src/main/java/com/team766/robot/reva/Lights.java +++ b/src/main/java/com/team766/robot/reva/Lights.java @@ -19,7 +19,7 @@ public class Lights extends RuleEngine { public Lights() { final BooleanSupplier isCameraMissing = - () -> !checkForStatusWith(ForwardApriltagCamera.class, s -> s.isCameraConnected()); + () -> !checkForStatusWith(ForwardApriltagCamera.ApriltagCameraStatus.class, s -> s.isCameraConnected()); addRule( Rule.create("Robot Disabled", () -> DriverStation.isDisabled()) diff --git a/src/main/java/com/team766/robot/reva/VisionUtil/VisionSpeakerHelper.java b/src/main/java/com/team766/robot/reva/VisionUtil/VisionSpeakerHelper.java index 1691448a6..d09156cd4 100644 --- a/src/main/java/com/team766/robot/reva/VisionUtil/VisionSpeakerHelper.java +++ b/src/main/java/com/team766/robot/reva/VisionUtil/VisionSpeakerHelper.java @@ -94,8 +94,8 @@ private void updateRelativeTranslation2d() { } public void update() { - cameraStatus = getStatusOrThrow(ForwardApriltagCamera.class); - driveStatus = getStatusOrThrow(SwerveDrive.class); + cameraStatus = getStatusOrThrow(ForwardApriltagCamera.ApriltagCameraStatus.class); + driveStatus = getStatusOrThrow(SwerveDrive.DriveStatus.class); updateAlliance(); updateTarget(); diff --git a/src/main/java/com/team766/robot/reva/procedures/DriverShootNow.java b/src/main/java/com/team766/robot/reva/procedures/DriverShootNow.java index e3afbec54..ca0f3ba09 100644 --- a/src/main/java/com/team766/robot/reva/procedures/DriverShootNow.java +++ b/src/main/java/com/team766/robot/reva/procedures/DriverShootNow.java @@ -105,7 +105,7 @@ public void run(Context context) { } private Optional getTransform3dOfRobotToTag() { - return getStatus(ForwardApriltagCamera.class) + return getStatus(ForwardApriltagCamera.ApriltagCameraStatus.class) .flatMap(s -> s.speakerTagTransform()); } } diff --git a/src/main/java/com/team766/robot/reva/procedures/DriverShootVelocityAndIntake.java b/src/main/java/com/team766/robot/reva/procedures/DriverShootVelocityAndIntake.java index 4b56035f1..7f0d5633d 100644 --- a/src/main/java/com/team766/robot/reva/procedures/DriverShootVelocityAndIntake.java +++ b/src/main/java/com/team766/robot/reva/procedures/DriverShootVelocityAndIntake.java @@ -15,7 +15,7 @@ public DriverShootVelocityAndIntake(Intake intake) { public void run(Context context) { waitForStatusWithOrTimeout( - context, Shooter.class, s -> s.isCloseToTargetSpeed(), 1); + context, Shooter.ShooterStatus.class, s -> s.isCloseToTargetSpeed(), 1); intake.requestIn(); diff --git a/src/main/java/com/team766/robot/reva/procedures/IntakeUntilIn.java b/src/main/java/com/team766/robot/reva/procedures/IntakeUntilIn.java index af8637f57..f9a255439 100644 --- a/src/main/java/com/team766/robot/reva/procedures/IntakeUntilIn.java +++ b/src/main/java/com/team766/robot/reva/procedures/IntakeUntilIn.java @@ -3,11 +3,9 @@ import com.team766.framework3.Context; import com.team766.framework3.Procedure; import com.team766.framework3.Status; -import com.team766.framework3.StatusSource; import com.team766.robot.reva.mechanisms.Intake; -public class IntakeUntilIn extends Procedure - implements StatusSource { +public class IntakeUntilIn extends Procedure { public record IntakeUntilInStatus(boolean noteInIntake) implements Status {} private final Intake intake; @@ -19,8 +17,8 @@ public IntakeUntilIn(Intake intake) { public void run(Context context) { publishStatus(new IntakeUntilInStatus(false)); intake.requestPowerForSensorDistance(); - waitForStatusWith(context, Intake.class, s -> s.hasNoteInIntake()); + waitForStatusWith(context, Intake.IntakeStatus.class, s -> s.hasNoteInIntake()); publishStatus(new IntakeUntilInStatus(true)); - waitForStatusWith(context, Intake.class, s -> s.hasNoteInIntake()); + waitForStatusWith(context, Intake.IntakeStatus.class, s -> s.hasNoteInIntake()); } } diff --git a/src/main/java/com/team766/robot/reva/procedures/PickupNote.java b/src/main/java/com/team766/robot/reva/procedures/PickupNote.java index 0ca00f1c7..3973cfb6a 100644 --- a/src/main/java/com/team766/robot/reva/procedures/PickupNote.java +++ b/src/main/java/com/team766/robot/reva/procedures/PickupNote.java @@ -30,8 +30,8 @@ public void run(Context context) { // Run intake the whole time intake.requestPowerForSensorDistance(); - while (!checkForStatusWith(Intake.class, s -> s.hasNoteInIntake())) { - Optional yawInDegrees = getStatus(NoteCamera.class).flatMap(s -> s.yawOfRing()); + while (!checkForStatusWith(Intake.IntakeStatus.class, s -> s.hasNoteInIntake())) { + Optional yawInDegrees = getStatus(NoteCamera.NoteCameraStatus.class).flatMap(s -> s.yawOfRing()); if (!yawInDegrees.isPresent()) { break; } diff --git a/src/main/java/com/team766/robot/reva/procedures/ShootNow.java b/src/main/java/com/team766/robot/reva/procedures/ShootNow.java index 755fb880b..7aa757f22 100644 --- a/src/main/java/com/team766/robot/reva/procedures/ShootNow.java +++ b/src/main/java/com/team766/robot/reva/procedures/ShootNow.java @@ -111,6 +111,6 @@ public void run(Context context) { } private Optional getTransform3dOfRobotToTag() { - return getStatus(ForwardApriltagCamera.class).flatMap(s -> s.speakerTagTransform()); + return getStatus(ForwardApriltagCamera.ApriltagCameraStatus.class).flatMap(s -> s.speakerTagTransform()); } } diff --git a/src/main/java/com/team766/robot/reva/procedures/ShootingProcedureStatus.java b/src/main/java/com/team766/robot/reva/procedures/ShootingProcedureStatus.java index 8e178ceef..e04243158 100644 --- a/src/main/java/com/team766/robot/reva/procedures/ShootingProcedureStatus.java +++ b/src/main/java/com/team766/robot/reva/procedures/ShootingProcedureStatus.java @@ -1,9 +1,8 @@ package com.team766.robot.reva.procedures; import com.team766.framework3.Status; -import com.team766.framework3.StatusHandle; -public record ShootingProcedureStatus(ShootingProcedureStatus.Status status) implements Status, StatusHandle { +public record ShootingProcedureStatus(ShootingProcedureStatus.Status status) implements Status { public enum Status { RUNNING, OUT_OF_RANGE,