From 93ff65ee5373f12f411769d000b96beeee3574ce Mon Sep 17 00:00:00 2001
From: robotics1 <troy@scds.org>
Date: Tue, 21 Jan 2025 16:32:18 -0800
Subject: [PATCH] Path one, backwards, works as expected

---
 .../cometbots/paths/CometBotDriveV2.java         | 16 +++-------------
 .../cometbots/paths/HighBasketPath1.java         | 13 +++++++------
 .../ftc/teamcode/configs/RobotConstants.java     |  4 ++--
 3 files changed, 12 insertions(+), 21 deletions(-)

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 7967cc4..3c1a9b7 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
@@ -65,7 +65,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 +77,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());
         }
@@ -127,18 +128,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;
-            }
-        }
+            state = 1;
     }
 
     public class SetStateAction implements Action {
@@ -148,7 +139,6 @@ public class CometBotDriveV2 extends OpMode {
         public SetStateAction(int value) {
             this.value = value;
         }
-
         @Override
         public boolean run(@NonNull TelemetryPacket telemetryPacket) {
             state = value;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath1.java
index 235802e..0fe012a 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath1.java
+++ b/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);
     }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java
index bbfae83..1696a7e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java
+++ b/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;