From 308f301bd5c1070dee32d17d9a9eec36a194d25c Mon Sep 17 00:00:00 2001 From: robotics3 Date: Thu, 24 Oct 2024 16:49:40 -0700 Subject: [PATCH] Alex's initial path code --- .../ftc/teamcode/AutoExampleThree.java | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleThree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleThree.java index 47ca538..bbebecf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleThree.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleThree.java @@ -34,7 +34,7 @@ public class AutoExampleThree extends OpMode { private PathChain path; - private final Pose startPose = new Pose(10.0, 40, 90); + private final Pose startPose = new Pose(7.467869222096955, 59.74295377677565); /** * This initializes the Follower and creates the PathChain for the "circle". Additionally, this @@ -52,12 +52,21 @@ public class AutoExampleThree extends OpMode { .addPath( // Line 1 new BezierCurve( - new Point(10.000, 20.000, Point.CARTESIAN), - new Point(29.089, 61.232, Point.CARTESIAN), - new Point(48.054, 19.607, Point.CARTESIAN) + new Point(7.468, 59.743, Point.CARTESIAN), + new Point(67.860, 12.014, Point.CARTESIAN), + new Point(101.790, 36.041, Point.CARTESIAN), + new Point(68.347, 22.891, Point.CARTESIAN) ) ) - .setConstantHeadingInterpolation(Math.toRadians(90)).build(); + .setTangentHeadingInterpolation() + .addPath( + // Line 2 + new BezierLine( + new Point(68.347, 22.891, Point.CARTESIAN), + new Point(13.637, 24.352, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation().build(); follower.followPath(path);