From a01ac85ef7712335adcc191f04250d902f2402b2 Mon Sep 17 00:00:00 2001 From: Ryan Brott Date: Sun, 24 Dec 2023 13:30:20 -0800 Subject: [PATCH] Better spline test --- .../ftc/teamcode/tuning/SplineTest.java | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java index 21f66567a32e..d70825e829b6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java @@ -11,25 +11,26 @@ public final class SplineTest extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { + Pose2d beginPose = new Pose2d(0, 0, 0); if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { - MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + MecanumDrive drive = new MecanumDrive(hardwareMap, beginPose); waitForStart(); Actions.runBlocking( - drive.actionBuilder(drive.pose) + drive.actionBuilder(beginPose) .splineTo(new Vector2d(30, 30), Math.PI / 2) - .splineTo(new Vector2d(60, 0), Math.PI) + .splineTo(new Vector2d(0, 60), Math.PI) .build()); } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) { - TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0)); + TankDrive drive = new TankDrive(hardwareMap, beginPose); waitForStart(); Actions.runBlocking( - drive.actionBuilder(drive.pose) + drive.actionBuilder(beginPose) .splineTo(new Vector2d(30, 30), Math.PI / 2) - .splineTo(new Vector2d(60, 0), Math.PI) + .splineTo(new Vector2d(0, 60), Math.PI) .build()); } else { throw new RuntimeException();