Path two working to pick up 1st specimen

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java

@@ -37,6 +37,8 @@ public class CometBotDriveV2 extends OpMode {
     private int state;
 
     private HighBasketPath1 path1;
+    private HighBasketPath2 path2;
+    private HighBasketPath3 path3;
 
 
 
@@ -55,7 +57,8 @@ public class CometBotDriveV2 extends OpMode {
         follower.setMaxPower(.75);
 
         path1 = new HighBasketPath1();
-
+        path2 = new HighBasketPath2();
+        path3 = new HighBasketPath3();
         lift = new DualMotorSliderSubsystem(hardwareMap);
         arm = new ArmSubsystem(hardwareMap);
         wrist = new WristSubsystem(hardwareMap);
@@ -65,7 +68,7 @@ public class CometBotDriveV2 extends OpMode {
 
         lift.init();
         arm.initAuto();
-        wrist.initTeleOp();
+        wrist.InitAuto();
         claw.init();
 
         //comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
@@ -77,6 +80,7 @@ public class CometBotDriveV2 extends OpMode {
     public void loop() {
         telemetry.addData("state", state);
         telemetry.addData("followingPath", followingPath);
+        telemetry.addData("busy", follower.isBusy());
         if (runtime != null) {
             telemetry.addData("Runtime (seconds)", runtime.seconds());
         }
@@ -87,10 +91,11 @@ public class CometBotDriveV2 extends OpMode {
                 break;
             case 1:
 //                doArmThing2();
+                state = 2;
+                break;
+            case 2:
+                moveToPathTwoAndPickSampleUp();
                 break;
-//            case 2:
-//                moveToPathTwoAndPickSampleUp();
-//                break;
 //            case 3:
 //                doPickUpThing();
 //                break;
@@ -127,18 +132,8 @@ public class CometBotDriveV2 extends OpMode {
     }
 
     private void moveToPathOneAndHighBucket() {
-        if (!followingPath) {
-            runtime = new ElapsedTime();
             path1.moveToPath1(follower);
-            followingPath = true;
-        }
-        if (runtime != null) {
-            telemetry.addData("Runtime (seconds)", runtime.seconds());
-            if (runtime.seconds() > 4) {
             state = 1;
-                followingPath = false;
-            }
-        }
     }
 
     public class SetStateAction implements Action {
@@ -148,7 +143,6 @@ public class CometBotDriveV2 extends OpMode {
         public SetStateAction(int value) {
             this.value = value;
         }
-
         @Override
         public boolean run(@NonNull TelemetryPacket telemetryPacket) {
             state = value;
@@ -191,20 +185,20 @@ public class CometBotDriveV2 extends OpMode {
 //        state = 10;
 //    }
 
-//    private void moveToPathTwoAndPickSampleUp() {
-//        if (!followingPath) {
-//            path2.moveToPath2(follower);
-//            followingPath = true;
-//        }
-//        if (runtime != null) {
-//            telemetry.addData("Runtime (seconds)", runtime.seconds());
-//            if (follower.atParametricEnd() || runtime.seconds() > 22.0) {
-//                state = 3;
-//                followingPath = false;
-//            }
-//        }
-//    }
+    private void moveToPathTwoAndPickSampleUp() {
+        if (!follower.isBusy()) {
+            path2.moveToPath2(follower);
+            state = 3;
+        }
+    }
 //
+    private void moveToPathBasketPath3() {
+        if (!follower.isBusy()) {
+            path3.moveToBasketPath3(follower);
+            state = 3;
+
+    }   }
+
 //    private void moveToPickupAgainPath4() {
 //        if (!followingPath) {
 //            path4.moveToPickupAgainPath4(follower);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath1.java

@@ -4,6 +4,7 @@ package org.firstinspires.ftc.teamcode.cometbots.paths;
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
 import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
@@ -15,7 +16,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
  */
 public class HighBasketPath1 {
 
-    private final Pose startPose = new Pose(8, 89);
+    private final Pose startPose = new Pose(10, 89);
 
     public void moveToPath1(Follower robot) {
         PathChain pathChain;
@@ -23,13 +24,13 @@ public class HighBasketPath1 {
         PathBuilder builder = new PathBuilder();
         builder
                 .addPath(
-                        new BezierCurve(
-                                new Point(8.000, 89.000, Point.CARTESIAN),
-                                new Point(24.000, 96.000, Point.CARTESIAN),
-                                new Point(18.000, 126.000, Point.CARTESIAN)
+                        // Line 1
+                        new BezierLine(
+                                new Point(10.000, 89.000, Point.CARTESIAN),
+                                new Point(20.000, 130.000, Point.CARTESIAN)
                         )
                 )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(320));
         pathChain = builder.build();
         robot.followPath(pathChain);
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath2.java

@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+
+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.PathBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+
+/*
+    AutoLine# - This file does something of a path......
+
+ */
+public class HighBasketPath2 {
+
+
+    public void moveToPath2(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(20.000, 130.000, Point.CARTESIAN),
+                                new Point(26.000, 117.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(0));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java

@@ -17,8 +17,8 @@ public class RobotConstants {
     public final static double armHangBlueberrySkyhook = 0.6;
     public final static double wristHangBlueberrySkyhook = 0.3;
     public final static double armBucket = 0.45;
-    public final static double armInit = 0.125;
-    public final static double wristInit = 0.0;
+    public final static double armInit = 0.135;
+    public final static double wristInit = 0.25;
     public final static double wristPickup = 0.475;
     public final static double wristBucket = 0.56;
     public final static double wristFloor = 0.75;