Merge remote-tracking branch 'origin/branch-rc-chassis-14493' into branch-rc-chassis-14493

TeamCode/src/main/java/ServoPractice.java

@@ -1,27 +0,0 @@
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Servo;
-
-public class ServoPractice extends OpMode {
-    //servo
-    Servo test_servo;
-
-
-    public void init(){
-        //hw map
-        test_servo = hardwareMap.get(Servo.class, "test_servo");
-    }
-    public void loop(){
-        //make it move
-        test_servo.setPosition(0);
-        try {
-            Thread.sleep(500);
-        } catch (InterruptedException e) {
-            throw new RuntimeException(e);
-        }
-        test_servo.setPosition(1);
-    }
-    public void stop(){
-
-    }
-}

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

@@ -0,0 +1,135 @@
+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 = "AutoExampleThree", group = "Autonomous Pathing Tuning")
+public class AsherPathV1 extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(10.0, 40, 90);
+
+    /**
+     * 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(.4);
+
+        follower.setStartingPose(startPose);
+
+        path = follower.pathBuilder()
+                /*
+                * Only update this path
+                 */
+                .addPath(
+                        // Line 1
+                        new BezierCurve(
+                                new Point(9.757, 84.983, Point.CARTESIAN),
+                                new Point(33.000, 105.000, Point.CARTESIAN),
+                                new Point(80.000, 118.000, Point.CARTESIAN),
+                                new Point(55.000, 120.000, Point.CARTESIAN)
+                        )
+                )
+                .addPath(
+                        // Line 2
+                        new BezierCurve(
+                                new Point(55.000, 120.000, Point.CARTESIAN),
+                                new Point(22.000, 106.000, Point.CARTESIAN),
+                                new Point(11.000, 131.000, Point.CARTESIAN)
+                        )
+                )
+                .addPath(
+                        // Line 3
+                        new BezierCurve(
+                                new Point(11.000, 131.000, Point.CARTESIAN),
+                                new Point(75.000, 95.000, Point.CARTESIAN),
+                                new Point(112.000, 132.000, Point.CARTESIAN),
+                                new Point(61.000, 131.000, Point.CARTESIAN)
+                        )
+                )
+                .addPath(
+                        // Line 4
+                        new BezierLine(
+                                new Point(61.000, 131.000, Point.CARTESIAN),
+                                new Point(11.000, 131.000, Point.CARTESIAN)
+                        )
+                )
+                .addPath(
+                        // Line 5
+                        new BezierCurve(
+                                new Point(11.000, 131.000, Point.CARTESIAN),
+                                new Point(100.000, 118.000, Point.CARTESIAN),
+                                new Point(103.000, 135.000, Point.CARTESIAN),
+                                new Point(61.000, 135.000, Point.CARTESIAN)
+                        )
+                )
+                .addPath(
+                        // Line 6
+                        new BezierLine(
+                                new Point(61.000, 135.000, Point.CARTESIAN),
+                                new Point(11.000, 131.000, Point.CARTESIAN)
+                        )
+                )
+                .addPath(
+                        // Line 7
+                        new BezierCurve(
+                                new Point(11.000, 131.000, Point.CARTESIAN),
+                                new Point(113.000, 95.000, Point.CARTESIAN),
+                                new Point(67.000, 95.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90)).build();
+                /*
+                * End of only update this path
+                 */
+
+        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);
+    }
+}

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

@@ -0,0 +1,142 @@
+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.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 = "AutoExampleSeason2025V1", group = "Autonomous Pathing Tuning")
+public class AutoExampleSeason2025V1 extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(15.0, 35, 90);
+
+    /**
+     * 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(.375);
+
+        follower.setStartingPose(startPose);
+
+        path = follower.pathBuilder()
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(15.000, 35.000, Point.CARTESIAN),
+                                new Point(60.000, 35.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 2
+                        new BezierLine(
+                                new Point(60.000, 35.000, Point.CARTESIAN),
+                                new Point(60.000, 25.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 3
+                        new BezierLine(
+                                new Point(60.000, 25.000, Point.CARTESIAN),
+                                new Point(15.000, 25.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 4
+                        new BezierLine(
+                                new Point(15.000, 25.000, Point.CARTESIAN),
+                                new Point(60.000, 25.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 5
+                        new BezierLine(
+                                new Point(60.000, 25.000, Point.CARTESIAN),
+                                new Point(60.000, 15.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 6
+                        new BezierLine(
+                                new Point(60.000, 15.000, Point.CARTESIAN),
+                                new Point(15.000, 15.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 7
+                        new BezierLine(
+                                new Point(15.000, 15.000, Point.CARTESIAN),
+                                new Point(60.000, 15.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 8
+                        new BezierLine(
+                                new Point(60.000, 15.000, Point.CARTESIAN),
+                                new Point(60.000, 8.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 9
+                        new BezierLine(
+                                new Point(60.000, 8.000, Point.CARTESIAN),
+                                new Point(15.000, 8.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(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);
+    }
+}

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

@@ -0,0 +1,89 @@
+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 = "AutoExampleThree", group = "Autonomous Pathing Tuning")
+public class AutoExampleThree extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(7.467869222096955, 59.74295377677565);
+
+    /**
+     * 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(.4);
+
+        follower.setStartingPose(startPose);
+
+        path = follower.pathBuilder()
+                .addPath(
+                        // Line 1
+                        new BezierCurve(
+                                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)
+                        )
+                )
+                .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);
+
+        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);
+    }
+}

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

@@ -29,11 +29,8 @@
 
 package org.firstinspires.ftc.teamcode;
 
-<<<<<<< HEAD
-=======
 import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER;
 import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION;
->>>>>>> branch-rc-chassis-14493
 import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
 import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
 import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
@@ -42,28 +39,20 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
 import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
 import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
 import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
-<<<<<<< HEAD
-=======
 import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER;
 import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION;
 import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
 import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
->>>>>>> branch-rc-chassis-14493
 
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotor;
-<<<<<<< HEAD
-import com.qualcomm.robotcore.util.ElapsedTime;
-
-=======
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
 import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
 
->>>>>>> branch-rc-chassis-14493
 /*
  * This file contains an example of a Linear "OpMode".
  * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match.
@@ -96,27 +85,13 @@ import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder;
 public class BasicOmniOpMode_Linear extends LinearOpMode {
 
     // Declare OpMode members for each of the 4 motors.
-<<<<<<< HEAD
-    private ElapsedTime runtime = new ElapsedTime();
-    private DcMotor leftFrontDrive = null;
-    private DcMotor leftBackDrive = null;
-    private DcMotor rightFrontDrive = null;
-    private DcMotor rightBackDrive = null;
-=======
     private final ElapsedTime runtime = new ElapsedTime();
->>>>>>> branch-rc-chassis-14493
 
     @Override
     public void runOpMode() {
 
         // Initialize the hardware variables. Note that the strings used here must correspond
         // to the names assigned during the robot configuration step on the DS or RC devices.
-<<<<<<< HEAD
-        leftFrontDrive  = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR);
-        leftBackDrive  = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR);
-        rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
-        rightBackDrive = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR);
-=======
         DcMotor leftFrontDrive = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR);
         DcMotor leftBackDrive = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR);
         DcMotor rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR);
@@ -133,7 +108,6 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
         leftEncoder.setDirection(LEFT_ENCODER_DIRECTION);
         rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION);
         strafeEncoder.setDirection(BACK_ENCODER_DIRECTION);
->>>>>>> branch-rc-chassis-14493
 
         // ########################################################################################
         // !!!            IMPORTANT Drive Information. Test your motor directions.            !!!!!
@@ -152,12 +126,9 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
 
         // Wait for the game to start (driver presses START)
         telemetry.addData("Status", "Initialized");
-<<<<<<< HEAD
-=======
         telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition());
         telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
         telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
->>>>>>> branch-rc-chassis-14493
         telemetry.update();
 
         waitForStart();
@@ -219,12 +190,9 @@ public class BasicOmniOpMode_Linear extends LinearOpMode {
             telemetry.addData("Status", "Run Time: " + runtime.toString());
             telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
             telemetry.addData("Back  left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
-<<<<<<< HEAD
-=======
             telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition());
             telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition());
             telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition());
->>>>>>> branch-rc-chassis-14493
             telemetry.update();
         }
     }}

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

@@ -0,0 +1,247 @@
+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 = "BluebAutoV1", group = "Autonomous Pathing Tuning")
+public class BluebAutoV1 extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(7.5, 72, 90);
+
+    /**
+     * 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(.4);
+
+        follower.setStartingPose(startPose);
+
+        path = follower.pathBuilder()
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(7.5, 72, Point.CARTESIAN),
+                                new Point(29.893, 38.250, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 2
+                        new BezierLine(
+                                new Point(29.893, 38.250, Point.CARTESIAN),
+                                new Point(65.250, 32.143, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 3
+                        new BezierLine(
+                                new Point(65.250, 32.143, Point.CARTESIAN),
+                                new Point(61.714, 24.429, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 4
+                        new BezierLine(
+                                new Point(61.714, 24.429, Point.CARTESIAN),
+                                new Point(13.821, 22.821, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 5
+                        new BezierLine(
+                                new Point(13.821, 22.821, Point.CARTESIAN),
+                                new Point(61.714, 24.429, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 6
+                        new BezierLine(
+                                new Point(61.714, 24.429, Point.CARTESIAN),
+                                new Point(60.750, 12.696, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 7
+                        new BezierLine(
+                                new Point(60.750, 12.696, Point.CARTESIAN),
+                                new Point(12.375, 13.179, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 8
+                        new BezierLine(
+                                new Point(12.375, 13.179, Point.CARTESIAN),
+                                new Point(60.750, 12.536, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 9
+                        new BezierLine(
+                                new Point(60.750, 12.536, Point.CARTESIAN),
+                                new Point(60.589, 9.321, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 10
+                        new BezierLine(
+                                new Point(60.589, 9.321, Point.CARTESIAN),
+                                new Point(12.536, 8.357, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 11
+                        new BezierLine(
+                                new Point(12.536, 8.357, Point.CARTESIAN),
+                                new Point(26.679, 8.679, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 12
+                        new BezierLine(
+                                new Point(26.679, 8.679, Point.CARTESIAN),
+                                new Point(22.821, 109.446, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 13
+                        new BezierLine(
+                                new Point(22.821, 109.446, Point.CARTESIAN),
+                                new Point(70.714, 109.446, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 14
+                        new BezierLine(
+                                new Point(70.714, 109.446, Point.CARTESIAN),
+                                new Point(71.036, 120.214, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 15
+                        new BezierLine(
+                                new Point(71.036, 120.214, Point.CARTESIAN),
+                                new Point(22.179, 120.214, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 16
+                        new BezierLine(
+                                new Point(22.179, 120.214, Point.CARTESIAN),
+                                new Point(11.089, 130.821, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 17
+                        new BezierLine(
+                                new Point(11.089, 130.821, Point.CARTESIAN),
+                                new Point(70.714, 112.018, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 18
+                        new BezierLine(
+                                new Point(70.714, 112.018, Point.CARTESIAN),
+                                new Point(70.714, 128.250, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 19
+                        new BezierLine(
+                                new Point(70.714, 128.250, Point.CARTESIAN),
+                                new Point(9.964, 130.018, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 20
+                        new BezierLine(
+                                new Point(9.964, 130.018, Point.CARTESIAN),
+                                new Point(70.554, 130.500, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 21
+                        new BezierLine(
+                                new Point(70.554, 130.500, Point.CARTESIAN),
+                                new Point(70.393, 135.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90))
+                .addPath(
+                        // Line 22
+                        new BezierLine(
+                                new Point(70.393, 135.000, Point.CARTESIAN),
+                                new Point(13.821, 134.839, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(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);
+    }
+}

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

@@ -0,0 +1,4 @@
+package org.firstinspires.ftc.teamcode;
+
+public class BluenbAutov1 {
+}

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

@@ -40,14 +40,13 @@ public class PedroConstants {
     public static final String BACK_ENCODER = "encoder back";
 
     // Robot encoder direction
-    public static final double LEFT_ENCODER_DIRECTION = Encoder.REVERSE;
+    public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD;
     public static final double RIGHT_ENCODER_DIRECTION = Encoder.FORWARD;
     public static final double BACK_ENCODER_DIRECTION = Encoder.FORWARD;
 
     /*
         Pedro's parameters
     */
-    // -0.0708
 
     // The weight of the robot in Kilograms
     public static final double ROBOT_WEIGHT_IN_KG = 10.5;

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

@@ -1,165 +0,0 @@
-package org.firstinspires.ftc.teamcode;
-
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Servo;
-import com.qualcomm.robotcore.util.ElapsedTime;
-
-import org.firstinspires.ftc.robotcontroller.external.samples.UtilityOctoQuadConfigMenu;
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-
-
-@TeleOp(name = "ArmControl")
-public class SlideArm extends OpMode {
-
-
-
-    DcMotor Slide;
-    Servo ClawServo;
-    Servo ArmServo;
-    Servo WristServo;
-    boolean xPressed;
-    boolean yPressed;
-    boolean bPressed;
-    double ticks = 753.2;
-
-
-    public void init(){
-        Slide = hardwareMap.get(DcMotor.class, "SlideMotor");
-        ArmServo = hardwareMap.get(Servo.class,"WristServo");
-        WristServo = hardwareMap.get(Servo.class, "ArmServo");
-        ClawServo = hardwareMap.get(Servo.class, "ClawServo");
-        xPressed = false;
-        yPressed = false;
-        bPressed = false;
-    }
-
-    double power = 0;
-    double position = 1;
-
-    public void loop() {
-        Slide.setPower(-gamepad2.left_stick_y);
-        Slide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        if (gamepad2.left_bumper) {
-            ClawServo.setPosition(0);
-        } else if (gamepad2.right_bumper) {
-            ClawServo.setPosition(1);
-        }
-
-        /*if(gamepad2.dpad_down){
-            WristServo.setPosition(0.78);
-            ArmServo.setPosition(0.55);
-        }
-        else if (gamepad2.dpad_up){
-            WristServo.setPosition(0.4);
-            ArmServo.setPosition(0.2);
-
-        }*/
-        if (gamepad2.a) {
-            Slide.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        }
-
-        if (gamepad2.dpad_down) {
-            SlideArm.ServoSteps(WristServo, 0.78, 4, 8);
-            try {
-                Thread.sleep(50);
-            } catch (InterruptedException e) {
-                throw new RuntimeException(e);
-            }
-            SlideArm.ServoSteps(ArmServo, 0.55, 10, 5);
-        } else if (gamepad2.dpad_up) {
-
-            WristServo.setPosition(0.4);
-            ArmServo.setPosition(0.2);
-
-        }
-
-        telemetry.addData("Slide Ticks", Slide.getCurrentPosition());
-        telemetry.update();
-        //limit = 6600;
-
-
-        if (gamepad2.x) {
-            xPressed = true;
-        }
-
-        while (xPressed && Slide.getCurrentPosition() < 6300) {
-            Slide.setPower(0.6);
-            Slide.setTargetPosition(6300);
-            Slide.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-            telemetry.addData("In While Loop:", Slide.getCurrentPosition());
-            telemetry.update();
-        }
-        if (xPressed && Slide.getCurrentPosition() >= 6300)
-        {
-            xPressed = false;
-        }
-
-        Slide.setPower(0);
-
-        if (gamepad2.y) {
-            yPressed = false;
-        }
-
-        while (yPressed && Slide.getCurrentPosition() < 3150) {
-            Slide.setPower(0.6);
-            Slide.setTargetPosition(3150);
-            Slide.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-            telemetry.addData("In While Loop:", Slide.getCurrentPosition());
-            telemetry.update();
-        }
-        Slide.setPower(0);
-        if (yPressed && Slide.getCurrentPosition() >= 3150)
-        {
-            yPressed = false;
-        }
-        if (gamepad2.b) {
-            bPressed = !bPressed;
-        }
-
-        while (bPressed && Slide.getCurrentPosition() > 0) {
-            Slide.setPower(0.6);
-            Slide.setTargetPosition(0);
-            Slide.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-            telemetry.addData("In While Loop:", Slide.getCurrentPosition());
-            telemetry.update();
-        }
-        Slide.setPower(0);
-        if (xPressed && Slide.getCurrentPosition() >= 0)
-        {
-            bPressed = false;
-        } }
-
-    /**
-     * This function takes 4 parameters and makes your servo move in multiple steps for precision
-     * @param servo the servo you want to move
-     * @param targetPosition the position you want to go to
-     * @param Steps how many steps you want to move before reaching targetPosition
-     * @param millis how much to sleep between steps
-     */
-    public static void ServoSteps (Servo servo, double targetPosition, int Steps, long millis){
-        double startingPosition = servo.getPosition();
-        double howFarToMove = targetPosition-startingPosition;
-        double Increment = howFarToMove/Steps;
-        double currentPosition = startingPosition;
-        for(int i = 0; i < Steps; i ++) {
-            servo.setPosition(currentPosition + Increment);
-            currentPosition += Increment;
-            try {
-                Thread.sleep(millis);
-            } catch (InterruptedException e) {
-                throw new RuntimeException(e);
-            }
-        }
-
-    }
-
-
-
-    public void stop(){
-
-    }
-
-
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java

@@ -58,10 +58,10 @@ public class FollowerConstants {
 
     // Heading error PIDF coefficients
     public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients(
-            2,
+            1,
             0,
-            .075,
+            .1,
-            -.03125);
+            0);
 
     // Feed forward constant added on to the heading PIDF
     public static double headingPIDFFeedForward = 0.01;