Almost Working Speciem hanging code!

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/SpecimenTest.java

@@ -29,6 +29,10 @@
 
 package org.firstinspires.ftc.teamcode.cometbots.tests;
 
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.Action;
 import com.acmerobotics.roadrunner.SequentialAction;
 import com.acmerobotics.roadrunner.SleepAction;
 import com.acmerobotics.roadrunner.ftc.Actions;
@@ -38,6 +42,11 @@ import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
 import org.firstinspires.ftc.teamcode.subsystem.ArmActionsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
@@ -49,9 +58,29 @@ public class SpecimenTest extends LinearOpMode {
 
     private final ElapsedTime runtime = new ElapsedTime();
 
+    private PathChain path;
+
+    private final Pose startPose = new Pose(36, 72);
+
+    public class MoveToPath implements Action {
+
+        private Follower actionRobot;
+        private PathChain pathChain;
+
+        private MoveToPath(PathChain path, Follower robot) {
+            this.actionRobot = robot;
+            this.pathChain = path;
+        }
+        @Override
+        public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+            return false;
+        }
+    }
+
     @Override
     public void runOpMode() {
 
+
         /*
          * Instantiate Lift
          */
@@ -61,18 +90,33 @@ public class SpecimenTest extends LinearOpMode {
         ClawActionsSubsystem claw = new ClawActionsSubsystem(hardwareMap);
         Follower robot = new Follower(hardwareMap);
 
+        /*
+          Robot stuff
+         */
+        robot.setStartingPose(startPose);
+        path = robot.pathBuilder() .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(36.000, 72.000, Point.CARTESIAN),
+                                new Point(37.500, 72.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0)).build();
+
         /*
          * Instantiate gamepad state holders
          */
         Gamepad currentGamepad1 = new Gamepad();
         Gamepad previousGamepad1 = new Gamepad();
+        Gamepad currentGamepad2 = new Gamepad();
+        Gamepad previousGamepad2 = new Gamepad();
 
         lift.init();
         wrist.init();
         arm.init();
         claw.init();
         robot.setMaxPower(.75);
-        robot.startTeleopDrive();
+        robot.followPath(path);
 
         waitForStart();
         runtime.reset();
@@ -84,6 +128,10 @@ public class SpecimenTest extends LinearOpMode {
             previousGamepad1.copy(currentGamepad1);
             currentGamepad1.copy(gamepad1);
 
+            previousGamepad2.copy(currentGamepad2);
+            currentGamepad2.copy(gamepad2);
+
+
             robot.setTeleOpMovementVectors(-currentGamepad1.left_stick_y, -currentGamepad1.left_stick_x, -currentGamepad1.right_stick_x);
             //robot.update();
 
@@ -104,6 +152,7 @@ public class SpecimenTest extends LinearOpMode {
                                 lift.toHighRung(),
                                 wrist.toSpeciemenBar(),
                                 lift.dropToHighRung()
+
                         )
                 );
 
@@ -136,6 +185,15 @@ public class SpecimenTest extends LinearOpMode {
                 lift.setPosition(lift.getPosition() - 25);
             }
 
+            if (currentGamepad2.x && !previousGamepad2.x){
+                while(true) {
+                    robot.update();
+                    if (!robot.isBusy()) {
+                        break;
+                    }
+                }
+            }
+
 
             // Show the elapsed game time and wheel power.
             telemetry.addData("Status", "Run Time: " + runtime.toString());