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 10285a1..afed453 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 @@ -118,6 +118,7 @@ public class CometBotDriveV2 extends OpMode { moveToPark(); break; case 99: + wrist.toTravelPosition(); break; default: telemetry.addLine("default"); @@ -133,7 +134,6 @@ public class CometBotDriveV2 extends OpMode { public void moveToPark() { if (runtime.seconds() > 26) { path6.moveToBasketPath6(follower); - wrist.toTravelPosition(); state = 99; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath6.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath6.java index f342229..b61c773 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath6.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/HighBasketPath6.java @@ -22,9 +22,10 @@ public class HighBasketPath6 { builder .addPath( // Line 1 - new BezierLine( - new Point(27.000, 128.500, Point.CARTESIAN), - new Point(104.00, 100.00, Point.CARTESIAN) + new BezierCurve( + new Point(27.000, 128.000, Point.CARTESIAN), + new Point(80.000, 130.000, Point.CARTESIAN), + new Point(100.000, 118.000, Point.CARTESIAN) ) ) .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270));