3 samples in high bucket

overtime

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

@@ -18,6 +18,8 @@ import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath1;
 import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath2;
 import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath3;
 import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath4;
+import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath5;
+import org.firstinspires.ftc.teamcode.subsystem.HighBasketAutoPath6;
 import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.SkyHookSubsystem;
 
@@ -32,6 +34,8 @@ public class ComeBotDriveDevV2 extends OpMode {
 
     private HighBasketAutoPath3 path3;
     private HighBasketAutoPath4 path4;
+    private HighBasketAutoPath5 path5;
+    private HighBasketAutoPath6 path6;
 
     private AutoPark pathPark;
     private SkyHookSubsystem hook;
@@ -48,6 +52,10 @@ public class ComeBotDriveDevV2 extends OpMode {
         path1 = new HighBasketAutoPath1();
         path2 = new HighBasketAutoPath2();
         path3 = new HighBasketAutoPath3();
+        path4 = new HighBasketAutoPath4();
+        path5 = new HighBasketAutoPath5();
+        path6 = new HighBasketAutoPath6();
+
         pathPark = new AutoPark();
 
         comp = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
@@ -83,16 +91,16 @@ public class ComeBotDriveDevV2 extends OpMode {
                 theArmThing();
                 break;
             case 6:
-                //moveToPickupAgainPath4();
+                moveToPickupAgainPath4();
                 break;
             case 7:
-                //doPickUpThingAgain();
+                doPickUpThingAgain();
                 break;
             case 8:
-                //line five
+                moveToPickupAgainPath5();
                 break;
             case 9:
-                //theArmThingAgain();
+                theArmThingAgain();
                 break;
             case 10:
                 //line six
@@ -156,6 +164,7 @@ public class ComeBotDriveDevV2 extends OpMode {
     }
 
     private void theArmThingAgain() {
+        follower.breakFollowing();
         comp.highBucketDrop();
         state = 10;
     }
@@ -181,13 +190,40 @@ public class ComeBotDriveDevV2 extends OpMode {
         }
         if (runtime != null) {
             telemetry.addData("Runtime (seconds)", runtime.seconds());
-            if (follower.atParametricEnd() || runtime.seconds() > 12.0) {
+            if (follower.atParametricEnd() || runtime.seconds() > 27.0) {
                 state = 7;
                 followingPath = false;
             }
         }
     }
 
+    private void moveToPickupAgainPath5() {
+        if (!followingPath) {
+            path5.moveToPickupAgainPath5(follower);
+            followingPath = true;
+        }
+        if (runtime != null) {
+            telemetry.addData("Runtime (seconds)", runtime.seconds());
+            if (follower.atParametricEnd() || runtime.seconds() > 36.0) {
+                state = 9;
+                followingPath = false;
+            }
+        }
+    }
+    private void moveToParkPath6() {
+        if (!followingPath) {
+            path6.moveToParkPath6(follower);
+            followingPath = true;
+        }
+        if (runtime != null) {
+            telemetry.addData("Runtime (seconds)", runtime.seconds());
+            if (follower.atParametricEnd() || runtime.seconds() > 48.0) {
+                state = 11;
+                followingPath = false;
+            }
+        }
+    }
+
     private void moveToBasketPath3() {
         if (!followingPath) {
             path3.moveToBasketPath3(follower);
@@ -225,6 +261,7 @@ public class ComeBotDriveDevV2 extends OpMode {
     }
 
     private void doPickUpThingAgain() {
+        follower.breakFollowing();
         thePickUp();
         state = 8;
     }

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

@@ -24,7 +24,7 @@ public class HighBasketAutoPath2 {
                         // Line 1
                         new BezierLine(
                                 new Point(18.000, 126.000, Point.CARTESIAN),
-                                new Point(29.000, 120.000, Point.CARTESIAN)
+                                new Point(25.500, 120.000, Point.CARTESIAN)
                         )
                 )
                 .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0));

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

@@ -18,7 +18,6 @@ public class HighBasketAutoPath3 {
     public void moveToBasketPath3(Follower robot) {
         PathChain pathChain;
         PathBuilder builder = new PathBuilder();
-//        robot.setStartingPose( new Pose(29, 120));
         builder
                 .addPath(
                         // Line 1

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

@@ -22,7 +22,7 @@ public class HighBasketAutoPath4 {
                         // Line 1
                         new BezierLine(
                                 new Point(18.000, 126.000, Point.CARTESIAN),
-                                new Point(29.000, 131.400, Point.CARTESIAN)
+                                new Point(25.000, 129.900, Point.CARTESIAN)
                         )
                 )
                 .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0));

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

@@ -0,0 +1,34 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+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 HighBasketAutoPath5 {
+    
+    public void moveToPickupAgainPath5(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(24.000, 131.000, Point.CARTESIAN),
+                                new Point(18.000, 126.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

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

@@ -0,0 +1,50 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+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 HighBasketAutoPath6 {
+    
+    public void moveToParkPath6(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(18.000, 126.000, Point.CARTESIAN),
+                                new Point(18.000, 126.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(270))
+                .addPath(
+                        // Line 2
+                        new BezierLine(
+                                new Point(18.000, 126.000, Point.CARTESIAN),
+                                new Point(81.000, 125.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(270), Math.toRadians(270))
+                .addPath(
+                        // Line 3
+                        new BezierLine(
+                                new Point(81.000, 125.000, Point.CARTESIAN),
+                                new Point(81.000, 100.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(270), Math.toRadians(270));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+