In-flight untested red-side code

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAuto.java

@@ -80,14 +80,14 @@ public class BlueBasketAuto extends OpMode {
                         // Line 4
                         new BezierLine(
                                 new Point(28.000, 121.500, Point.CARTESIAN),
-                                new Point(18.000, 130.179, Point.CARTESIAN)
+                                new Point(15.750, 126.321, Point.CARTESIAN)
                         )
                 )
                 .setConstantHeadingInterpolation(Math.toRadians(0))
                 .addPath(
                         // Line 5
                         new BezierCurve(
-                                new Point(18.000, 130.179, Point.CARTESIAN),
+                                new Point(15.750, 126.321, Point.CARTESIAN),
                                 new Point(59.000, 102.500, Point.CARTESIAN),
                                 new Point(68.700, 130.500, Point.CARTESIAN)
                         )
@@ -97,14 +97,14 @@ public class BlueBasketAuto extends OpMode {
                         // Line 6
                         new BezierLine(
                                 new Point(68.700, 130.500, Point.CARTESIAN),
-                                new Point(18.000, 130.339, Point.CARTESIAN)
+                                new Point(22.661, 130.821, Point.CARTESIAN)
                         )
                 )
                 .setConstantHeadingInterpolation(Math.toRadians(0))
                 .addPath(
                         // Line 7
                         new BezierCurve(
-                                new Point(18.000, 130.339, Point.CARTESIAN),
+                                new Point(22.661, 130.821, Point.CARTESIAN),
                                 new Point(49.018, 121.179, Point.CARTESIAN),
                                 new Point(63.804, 135.321, Point.CARTESIAN)
                         )
@@ -114,29 +114,14 @@ public class BlueBasketAuto extends OpMode {
                         // Line 8
                         new BezierLine(
                                 new Point(63.804, 135.321, Point.CARTESIAN),
-                                new Point(53.036, 135.161, Point.CARTESIAN)
+                                new Point(26.679, 135.964, Point.CARTESIAN)
                         )
                 )
                 .setConstantHeadingInterpolation(Math.toRadians(0))
                 .addPath(
                         // Line 9
                         new BezierLine(
-                                new Point(53.036, 135.161, Point.CARTESIAN),
+                                new Point(26.679, 135.964, Point.CARTESIAN),
-                                new Point(18.643, 135.000, Point.CARTESIAN)
-                        )
-                )
-                .setConstantHeadingInterpolation(Math.toRadians(0))
-                .addPath(
-                        // Line 10
-                        new BezierLine(
-                                new Point(18.643, 135.000, Point.CARTESIAN),
-                                new Point(72.300, 97.400, Point.CARTESIAN)
-                        )
-                )
-                .addPath(
-                        // Line 9
-                        new BezierLine(
-                                new Point(18.643, 135.000, Point.CARTESIAN),
                                 new Point(83.250, 95.464, Point.CARTESIAN)
                         )
                 )

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedBasketAuto.java

@@ -0,0 +1,161 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+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.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+/**
+ * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
+ * a circle, but some Bezier curves that have control points set essentially in a square. However,
+ * it turns enough to tune your centripetal force correction and some of your heading. Some lag in
+ * heading is to be expected.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/12/2024
+ */
+@Config
+@Autonomous(name = "BlueBasketAuto", group = "Autonomous Pathing Tuning")
+public class RedBasketAuto extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(
+            135.6
+            , 48.05);
+
+    /**
+     * This initializes the Follower and creates the PathChain for the "circle". Additionally, this
+     * initializes the FTC Dashboard telemetry.
+     */
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+
+        follower.setMaxPower(.45);
+
+        follower.setStartingPose(startPose);
+
+        path = follower.pathBuilder()
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(136.286, 48.536, Point.CARTESIAN),
+                                new Point(102.375, 32.946, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 2
+                        new BezierLine(
+                                new Point(102.375, 32.946, Point.CARTESIAN),
+                                new Point(80.679, 31.982, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 3
+                        new BezierLine(
+                                new Point(80.679, 31.982, Point.CARTESIAN),
+                                new Point(80.679, 23.304, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 4
+                        new BezierLine(
+                                new Point(80.679, 23.304, Point.CARTESIAN),
+                                new Point(131.143, 21.536, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 5
+                        new BezierLine(
+                                new Point(131.143, 21.536, Point.CARTESIAN),
+                                new Point(81.000, 23.304, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 6
+                        new BezierLine(
+                                new Point(81.000, 23.304, Point.CARTESIAN),
+                                new Point(81.161, 13.179, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 7
+                        new BezierLine(
+                                new Point(81.161, 13.179, Point.CARTESIAN),
+                                new Point(124.554, 13.018, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 8
+                        new BezierLine(
+                                new Point(124.554, 13.018, Point.CARTESIAN),
+                                new Point(81.000, 13.018, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 9
+                        new BezierLine(
+                                new Point(81.000, 13.018, Point.CARTESIAN),
+                                new Point(80.839, 8.800, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 10
+                        new BezierLine(
+                                new Point(80.839, 8.800, Point.CARTESIAN),
+                                new Point(120.054, 8.518, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(180))
+                .addPath(
+                        // Line 11
+                        new BezierCurve(
+                                new Point(120.054, 8.518, Point.CARTESIAN),
+                                new Point(49.821, 27.643, Point.CARTESIAN),
+                                new Point(60.429, 48.214, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(90)).build();
+        follower.followPath(path);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.update();
+    }
+
+    /**
+     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+     * the Telemetry, as well as the FTC Dashboard.
+     */
+    @Override
+    public void loop() {
+        follower.update();
+        if (follower.atParametricEnd()) {
+            follower.followPath(path);
+        }
+        follower.telemetryDebug(telemetryA);
+    }
+}