From 15db2ded0168db221431ad35736a6e0c3ca2645a Mon Sep 17 00:00:00 2001 From: Lucas Bubner Date: Tue, 10 Sep 2024 12:48:36 +0930 Subject: [PATCH] Run Align To Point on drive --- .../bunyipslib/tasks/AlignToPointDriveTask.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/murraybridgebunyips/bunyipslib/tasks/AlignToPointDriveTask.java b/src/main/java/org/murraybridgebunyips/bunyipslib/tasks/AlignToPointDriveTask.java index f814fb625..7caa6ec88 100644 --- a/src/main/java/org/murraybridgebunyips/bunyipslib/tasks/AlignToPointDriveTask.java +++ b/src/main/java/org/murraybridgebunyips/bunyipslib/tasks/AlignToPointDriveTask.java @@ -7,6 +7,8 @@ import com.acmerobotics.roadrunner.geometry.Vector2d; import com.qualcomm.robotcore.hardware.Gamepad; +import org.murraybridgebunyips.bunyipslib.BunyipsSubsystem; +import org.murraybridgebunyips.bunyipslib.EmergencyStop; import org.murraybridgebunyips.bunyipslib.external.Mathf; import org.murraybridgebunyips.bunyipslib.external.pid.PIDFController; import org.murraybridgebunyips.bunyipslib.roadrunner.drive.RoadRunnerDrive; @@ -44,12 +46,15 @@ public class AlignToPointDriveTask extends ForeverTask { * * @param passThroughPoseX the pose X power to pass through to the drive * @param passThroughPoseY the pose Y power to pass through to the drive - * @param drive RoadRunner drive instance + * @param drive RoadRunner drive instance, must be a BunyipsSubsystem * @param rotationController rotation PID controller to use * @param point the point to align to in field space, will use the drive's pose estimate for current position */ public AlignToPointDriveTask(@Nullable Supplier passThroughPoseX, @Nullable Supplier passThroughPoseY, RoadRunnerDrive drive, PIDFController rotationController, Supplier point) { this.drive = drive; + if (!(drive instanceof BunyipsSubsystem)) + throw new EmergencyStop("AlignToPointDriveTask must be used with a BunyipsSubsystem extending drive"); + onSubsystem((BunyipsSubsystem) drive, false); pointSupplier = point; controller = rotationController; // Default tolerance is too low, will set minimum bound