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);