Two Samples in bucket needs fine tuning

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComeBotDriveDevV2.java

@@ -47,18 +47,22 @@ public class ComeBotDriveDevV2 extends OpMode {
         follower.setMaxPower(.75);
         path1 = new HighBasketAutoPath1();
         path2 = new HighBasketAutoPath2();
+        path3 = new HighBasketAutoPath3();
         pathPark = new AutoPark();
 
         comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
         comp.initCloseClaw();
         hook = new SkyHookSubsystem(hardwareMap);
         state = 0;
-
     }
 
 
     public void loop() {
         telemetry.addData("state", state);
+        telemetry.addData("followingPath", followingPath);
+        if (runtime != null) {
+            telemetry.addData("Runtime (seconds)", runtime.seconds());
+        }
         switch (state) {
             case 0:
                 moveToPathOneAndHighBucket();
@@ -79,16 +83,16 @@ public class ComeBotDriveDevV2 extends OpMode {
                 theArmThing();
                 break;
             case 6:
-                moveToPickupAgainPath4();
+                //moveToPickupAgainPath4();
                 break;
             case 7:
-                doPickUpThingAgain();
+                //doPickUpThingAgain();
                 break;
             case 8:
                 //line five
                 break;
             case 9:
-                theArmThingAgain();
+                //theArmThingAgain();
                 break;
             case 10:
                 //line six
@@ -109,8 +113,8 @@ public class ComeBotDriveDevV2 extends OpMode {
         }
         if (runtime != null) {
             telemetry.addData("Runtime (seconds)", runtime.seconds());
-            if (follower.atParametricEnd() || runtime.seconds() > 6.0) {
+            if (follower.atParametricEnd() || runtime.seconds() > 4) {
-                state = 3;
+                state = 1;
                 followingPath = false;
             }
         }
@@ -141,9 +145,15 @@ public class ComeBotDriveDevV2 extends OpMode {
     }
 
     private void theArmThing() {
+        telemetry.addData("busy?", follower.isBusy());
+        telemetry.addData("end?", follower.atParametricEnd());
+        if (follower.atParametricEnd()){
+            follower.breakFollowing();
             comp.highBucketDrop();
             state = 6;
         }
+//        follower.breakFollowing();
+    }
 
     private void theArmThingAgain() {
         comp.highBucketDrop();
@@ -163,6 +173,7 @@ public class ComeBotDriveDevV2 extends OpMode {
             }
         }
     }
+
     private void moveToPickupAgainPath4() {
         if (!followingPath) {
             path4.moveToPickupAgainPath4(follower);
@@ -176,6 +187,7 @@ public class ComeBotDriveDevV2 extends OpMode {
             }
         }
     }
+
     private void moveToBasketPath3() {
         if (!followingPath) {
             path3.moveToBasketPath3(follower);
@@ -192,24 +204,25 @@ public class ComeBotDriveDevV2 extends OpMode {
 
     private void thePickUp() {
         Actions.runBlocking(new SequentialAction(
-         comp.arm.toFloorPosition(),
+                new SleepAction(.25),
+                comp.arm.toSubmarinePosition(),
                 new SleepAction(.5),
                 comp.wrist.toPickupPosition(),
-         new SleepAction(1),
+                new SleepAction(.75),
                 comp.claw.closeClaw(),
-         new SleepAction(1),
+                new SleepAction(.75),
                 comp.wrist.toFloorPosition(),
                 new SleepAction(0.5),
-         comp.arm.toParkPosition()
+                comp.arm.toParkPosition(),
+                new SleepAction(.25)
         ));
     }
 
     private void doPickUpThing() {
+        follower.breakFollowing();
         thePickUp();
-        if(!follower.isBusy()) {
         state = 4;
     }
-    }
 
     private void doPickUpThingAgain() {
         thePickUp();

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

@@ -49,7 +49,6 @@ 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.AutoLine1;
-import org.firstinspires.ftc.teamcode.subsystem.AutoLine2;
 import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine2.java

@@ -1,40 +0,0 @@
-package org.firstinspires.ftc.teamcode.subsystem;
-
-import androidx.annotation.NonNull;
-
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.Action;
-
-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.PathChain;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
-
-
-/*
-    AutoLine# - This file does something of a path......
-
- */
-public class AutoLine2 {
-
-
-    private PathChain pathChain;
-
-    private PathChain goToStore;
-
-    private Pose startPose = new Pose(37.5, 72);
-
-    public AutoLine2(Follower robot) {
-        robot.setStartingPose(startPose);
-        pathChain  = robot.pathBuilder().addPath(
-                new BezierLine(
-                        new Point(37.500, 72.000, Point.CARTESIAN),
-                        new Point(36.000, 72.000, Point.CARTESIAN)
-                )
-        ).setConstantHeadingInterpolation(Math.toRadians(0)).build();
-        robot.followPath(pathChain);
-    }
-}
-
-

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath1.java

@@ -29,7 +29,7 @@ public class HighBasketAutoPath1 {
                                 new Point(18.000, 126.000, Point.CARTESIAN)
                         )
                 )
-                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)).build();
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
         pathChain = builder.build();
         robot.followPath(pathChain);
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HighBasketAutoPath3.java

@@ -2,7 +2,7 @@ package org.firstinspires.ftc.teamcode.subsystem;
 
 
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
+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;
@@ -18,6 +18,7 @@ public class HighBasketAutoPath3 {
     public void moveToBasketPath3(Follower robot) {
         PathChain pathChain;
         PathBuilder builder = new PathBuilder();
+//        robot.setStartingPose( new Pose(29, 120));
         builder
                 .addPath(
                         // Line 1
@@ -27,7 +28,6 @@ public class HighBasketAutoPath3 {
                         )
                 )
                 .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
-
         pathChain = builder.build();
         robot.followPath(pathChain);
     }