From 2cb8ce41dd9ecc05aa124793fab92f9178b7aae9 Mon Sep 17 00:00:00 2001
From: robotics1 <troy@scds.org>
Date: Tue, 21 Jan 2025 17:10:30 -0800
Subject: [PATCH] Path two working to pick up 1st specimen

---
 .../cometbots/paths/CometBotDriveV2.java      | 38 ++++++++++---------
 .../cometbots/paths/HighBasketPath2.java      | 36 ++++++++++++++++++
 2 files changed, 57 insertions(+), 17 deletions(-)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath2.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java
index 3c1a9b7..c24f3f8 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotDriveV2.java
+++ b/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);
@@ -88,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;
@@ -181,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);
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath2.java
new file mode 100644
index 0000000..ad5c2a4
--- /dev/null
+++ b/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);
+    }
+
+}
+