Multiple usability patches

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java

@@ -1,19 +1,19 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.follower;
 
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFFeedForward;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFSwitch;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.forwardZeroPowerAcceleration;
-import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch;
-import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.drivePIDFFeedForward;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFFeedForward;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.headingPIDFSwitch;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.lateralZeroPowerAcceleration;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightRearMotorName;
-import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFFeedForward;
-import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.lateralZeroPowerAcceleration;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryDrivePIDFFeedForward;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryHeadingPIDFFeedForward;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.secondaryTranslationalPIDFFeedForward;
+import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFFeedForward;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.translationalPIDFSwitch;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryDrivePID;
 import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.useSecondaryHeadingPID;
@@ -365,6 +365,25 @@ public class Follower {
         closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1);
     }
 
+    /**
+     * This holds a Point.
+     *
+     * @param point   the Point to stay at.
+     * @param heading the heading to face.
+     */
+    public void holdPoint(Point point, double heading) {
+        holdPoint(new BezierPoint(point), heading);
+    }
+
+    /**
+     * This holds a Point.
+     *
+     * @param pose the Point (as a Pose) to stay at.
+     */
+    public void holdPoint(Pose pose) {
+        holdPoint(new Point(pose), pose.getHeading());
+    }
+
     /**
      * This follows a Path.
      * This also makes the Follower hold the last Point on the Path.
@@ -426,15 +445,22 @@ public class Follower {
     }
 
     /**
-     * This calls an update to the PoseUpdater, which updates the robot's current position estimate.
+     * Calls an update to the PoseUpdater, which updates the robot's current position estimate.
-     * This also updates all the Follower's PIDFs, which updates the motor powers.
      */
-    public void update() {
+    public void updatePose() {
         poseUpdater.update();
 
         if (drawOnDashboard) {
             dashboardPoseTracker.update();
         }
+    }
+
+    /**
+     * This calls an update to the PoseUpdater, which updates the robot's current position estimate.
+     * This also updates all the Follower's PIDFs, which updates the motor powers.
+     */
+    public void update() {
+        updatePose();
 
         if (!teleopDrive) {
             if (currentPath != null) {
@@ -515,7 +541,7 @@ public class Follower {
      *                     movement, this is the x-axis.
      * @param lateralDrive determines the lateral drive vector for the robot in teleop. In field centric
      *                     movement, this is the y-axis.
-     * @param heading determines the heading vector for the robot in teleop.
+     * @param heading      determines the heading vector for the robot in teleop.
      */
     public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, double heading) {
         setTeleOpMovementVectors(forwardDrive, lateralDrive, heading, true);
@@ -528,7 +554,7 @@ public class Follower {
      *                     movement, this is the x-axis.
      * @param lateralDrive determines the lateral drive vector for the robot in teleop. In field centric
      *                     movement, this is the y-axis.
-     * @param heading determines the heading vector for the robot in teleop.
+     * @param heading      determines the heading vector for the robot in teleop.
      * @param robotCentric sets if the movement will be field or robot centric
      */
     public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, double heading, boolean robotCentric) {
@@ -719,7 +745,7 @@ public class Follower {
         Vector velocityErrorVector = MathFunctions.addVectors(forwardVelocityError, lateralVelocityError);
 
         previousRawDriveError = rawDriveError;
-        rawDriveError =  velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector()));
+        rawDriveError = velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector()));
 
         double projection = 2 * driveErrors[1] - driveErrors[0];
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Pose.java

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.localization;
 
+import androidx.annotation.NonNull;
+
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
 
@@ -44,7 +46,7 @@ public class Pose {
      * This creates a new Pose with no inputs and 0 for all values.
      */
     public Pose() {
-        this(0,0,0);
+        this(0, 0, 0);
     }
 
     /**
@@ -208,4 +210,10 @@ public class Pose {
     public Pose copy() {
         return new Pose(getX(), getY(), getHeading());
     }
+
+    @NonNull
+    @Override
+    public String toString() {
+        return "(" + getX() + ", " + getY() + ", " + Math.toDegrees(getHeading()) + ")";
+    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java

@@ -27,16 +27,16 @@ public class MathFunctions {
         double denom = 1;
 
         // this multiplies up the numerator of the nCr function
-        for (int i = n; i > n-r; i--) {
+        for (int i = n; i > n - r; i--) {
             num *= i;
         }
 
         // this multiplies up the denominator of the nCr function
-        for (int i = 1; i <=r; i++) {
+        for (int i = 1; i <= r; i++) {
             denom *= i;
         }
 
-        return num/denom;
+        return num / denom;
     }
 
     /**
@@ -67,7 +67,7 @@ public class MathFunctions {
 
     /**
      * This normalizes an angle to be between 0 and 2 pi radians, inclusive.
-     *
+     * <p>
      * IMPORTANT NOTE: This method operates in radians.
      *
      * @param angleRadians the angle to be normalized.
@@ -75,8 +75,8 @@ public class MathFunctions {
      */
     public static double normalizeAngle(double angleRadians) {
         double angle = angleRadians;
-        while (angle<0) angle += 2*Math.PI;
+        while (angle < 0) angle += 2 * Math.PI;
-        while (angle>2*Math.PI) angle -= 2*Math.PI;
+        while (angle > 2 * Math.PI) angle -= 2 * Math.PI;
         return angle;
     }
 
@@ -88,7 +88,7 @@ public class MathFunctions {
      * @return returns the smallest angle.
      */
     public static double getSmallestAngleDifference(double one, double two) {
-        return Math.min(MathFunctions.normalizeAngle(one-two), MathFunctions.normalizeAngle(two-one));
+        return Math.min(MathFunctions.normalizeAngle(one - two), MathFunctions.normalizeAngle(two - one));
     }
 
     /**
@@ -98,7 +98,7 @@ public class MathFunctions {
      * @return returns the turn direction.
      */
     public static double getTurnDirection(double startHeading, double endHeading) {
-        if (MathFunctions.normalizeAngle(endHeading-startHeading) >= 0 && MathFunctions.normalizeAngle(endHeading-startHeading) <= Math.PI) {
+        if (MathFunctions.normalizeAngle(endHeading - startHeading) >= 0 && MathFunctions.normalizeAngle(endHeading - startHeading) <= Math.PI) {
             return 1; // counter clock wise
         }
         return -1; // clock wise
@@ -112,7 +112,7 @@ public class MathFunctions {
      * @return returns the distance between the two.
      */
     public static double distance(Pose pose, Point point) {
-        return Math.sqrt(Math.pow(pose.getX()-point.getX(), 2) + Math.pow(pose.getY()-point.getY(), 2));
+        return Math.sqrt(Math.pow(pose.getX() - point.getX(), 2) + Math.pow(pose.getY() - point.getY(), 2));
     }
 
     /**
@@ -123,7 +123,7 @@ public class MathFunctions {
      * @return returns the distance between the two.
      */
     public static double distance(Pose one, Pose two) {
-        return Math.sqrt(Math.pow(one.getX()-two.getX(), 2) + Math.pow(one.getY()-two.getY(), 2));
+        return Math.sqrt(Math.pow(one.getX() - two.getX(), 2) + Math.pow(one.getY() - two.getY(), 2));
     }
 
     /**
@@ -175,8 +175,8 @@ public class MathFunctions {
     /**
      * This rotates the given pose by the given theta,
      *
-     * @param pose          the Pose to rotate.
+     * @param pose the Pose to rotate.
-     * @param theta         the angle to rotate by.
+     * @param theta the angle to rotate by.
      * @param rotateHeading whether to adjust the Pose heading too.
      * @return the rotated Pose.
      */
@@ -196,7 +196,7 @@ public class MathFunctions {
      * @return returns the scaled Point.
      */
     public static Point scalarMultiplyPoint(Point point, double scalar) {
-        return new Point(point.getX()*scalar, point.getY()*scalar, Point.CARTESIAN);
+        return new Point(point.getX() * scalar, point.getY() * scalar, Point.CARTESIAN);
     }
 
     /**
@@ -229,7 +229,7 @@ public class MathFunctions {
      * @return returns the scaled Vector.
      */
     public static Vector scalarMultiplyVector(Vector vector, double scalar) {
-        return new Vector(vector.getMagnitude()*scalar, vector.getTheta());
+        return new Vector(vector.getMagnitude() * scalar, vector.getTheta());
     }
 
     /**
@@ -243,7 +243,7 @@ public class MathFunctions {
         if (vector.getMagnitude() == 0) {
             return new Vector(0.0, vector.getTheta());
         } else {
-            return new Vector(vector.getMagnitude()/Math.abs(vector.getMagnitude()), vector.getTheta());
+            return new Vector(vector.getMagnitude() / Math.abs(vector.getMagnitude()), vector.getTheta());
         }
     }
 
@@ -256,7 +256,7 @@ public class MathFunctions {
      */
     public static Vector addVectors(Vector one, Vector two) {
         Vector returnVector = new Vector();
-        returnVector.setOrthogonalComponents(one.getXComponent()+two.getXComponent(), one.getYComponent()+two.getYComponent());
+        returnVector.setOrthogonalComponents(one.getXComponent() + two.getXComponent(), one.getYComponent() + two.getYComponent());
         return returnVector;
     }
 
@@ -270,7 +270,7 @@ public class MathFunctions {
      */
     public static Vector subtractVectors(Vector one, Vector two) {
         Vector returnVector = new Vector();
-        returnVector.setOrthogonalComponents(one.getXComponent()-two.getXComponent(), one.getYComponent()-two.getYComponent());
+        returnVector.setOrthogonalComponents(one.getXComponent() - two.getXComponent(), one.getYComponent() - two.getYComponent());
         return returnVector;
     }
 
@@ -282,7 +282,7 @@ public class MathFunctions {
      * @return returns the dot product of the two Vectors.
      */
     public static double dotProduct(Vector one, Vector two) {
-        return one.getXComponent()*two.getXComponent() + one.getYComponent()*two.getYComponent();
+        return one.getXComponent() * two.getXComponent() + one.getYComponent() * two.getYComponent();
     }
 
     /**
@@ -294,7 +294,7 @@ public class MathFunctions {
      * @return returns the cross product of the two Vectors.
      */
     public static double crossProduct(Vector one, Vector two) {
-        return one.getXComponent()*two.getYComponent() - one.getYComponent()*two.getXComponent();
+        return one.getXComponent() * two.getYComponent() - one.getYComponent() * two.getXComponent();
     }
 
     /**
@@ -302,7 +302,7 @@ public class MathFunctions {
      * specified accuracy amount.
      *
      * @param one first number specified.
-     * @param two second number specified.
+     * @param two Second number specified.
      * @param accuracy the level of accuracy specified.
      * @return returns if the two numbers are within the specified accuracy of each other.
      */

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java

@@ -51,16 +51,47 @@ public class PathBuilder {
         return this;
     }
 
+    /**
+     * This adds a default Path defined by a specified BezierCurve to the PathBuilder.
+     *
+     * @param controlPoints This is the specified control points that define the BezierCurve.
+     * @return This returns itself with the updated data.
+     */
+    public PathBuilder addBezierCurve(Point... controlPoints) {
+        return addPath(new BezierCurve(controlPoints));
+    }
+
+    /**
+     * This adds a default Path defined by a specified BezierCurve to the PathBuilder.
+     *
+     * @param controlPoints This is the specified control points that define the BezierCurve.
+     * @return This returns itself with the updated data.
+     */
+    public PathBuilder addBezierCurve(ArrayList<Point> controlPoints) {
+        return addPath(new BezierCurve(controlPoints));
+    }
+
+    /**
+     * This adds a default Path defined by a specified BezierLine to the PathBuilder.
+     *
+     * @param startPoint start point of the line.
+     * @param endPoint end point of the line.
+     * @return This returns itself with the updated data.
+     */
+    public PathBuilder addBezierLine(Point startPoint, Point endPoint) {
+        return addPath(new BezierLine(startPoint, endPoint));
+    }
+
     /**
      * This sets a linear heading interpolation on the last Path added to the PathBuilder.
      *
      * @param startHeading The start of the linear heading interpolation.
      * @param endHeading The end of the linear heading interpolation.
-     *                   This will be reached at the end of the Path if no end time is specified.
+     *         This will be reached at the end of the Path if no end time is specified.
      * @return This returns itself with the updated data.
      */
     public PathBuilder setLinearHeadingInterpolation(double startHeading, double endHeading) {
-        this.paths.get(paths.size()-1).setLinearHeadingInterpolation(startHeading, endHeading);
+        this.paths.get(paths.size() - 1).setLinearHeadingInterpolation(startHeading, endHeading);
         return this;
     }
 
@@ -69,13 +100,13 @@ public class PathBuilder {
      *
      * @param startHeading The start of the linear heading interpolation.
      * @param endHeading The end of the linear heading interpolation.
-     *                   This will be reached at the end of the Path if no end time is specified.
+     *         This will be reached at the end of the Path if no end time is specified.
      * @param endTime The end time on the Path that the linear heading interpolation will end.
-     *                This value goes from [0, 1] since Bezier curves are parametric functions.
+     *         This value goes from [0, 1] since Bezier curves are parametric functions.
      * @return This returns itself with the updated data.
      */
     public PathBuilder setLinearHeadingInterpolation(double startHeading, double endHeading, double endTime) {
-        this.paths.get(paths.size()-1).setLinearHeadingInterpolation(startHeading, endHeading, endTime);
+        this.paths.get(paths.size() - 1).setLinearHeadingInterpolation(startHeading, endHeading, endTime);
         return this;
     }
 
@@ -86,7 +117,7 @@ public class PathBuilder {
      * @return This returns itself with the updated data.
      */
     public PathBuilder setConstantHeadingInterpolation(double setHeading) {
-        this.paths.get(paths.size()-1).setConstantHeadingInterpolation(setHeading);
+        this.paths.get(paths.size() - 1).setConstantHeadingInterpolation(setHeading);
         return this;
     }
 
@@ -94,20 +125,21 @@ public class PathBuilder {
      * This sets a reversed or tangent heading interpolation on the last Path added to the PathBuilder.
      *
      * @param set This sets the heading to reversed tangent following if this parameter is true.
-     *            Conversely, this sets a tangent following if this parameter is false.
+     *         Conversely, this sets a tangent following if this parameter is false.
      * @return This returns itself with the updated data.
      */
     public PathBuilder setReversed(boolean set) {
-        this.paths.get(paths.size()-1).setReversed(set);
+        this.paths.get(paths.size() - 1).setReversed(set);
         return this;
     }
+
     /**
      * This sets the heading interpolation to tangential on the last Path added to the PathBuilder.
      * There really shouldn't be a reason to use this since the default heading interpolation is
      * tangential but it's here.
      */
     public PathBuilder setTangentHeadingInterpolation() {
-        this.paths.get(paths.size()-1).setTangentHeadingInterpolation();
+        this.paths.get(paths.size() - 1).setTangentHeadingInterpolation();
         return this;
     }
 
@@ -118,7 +150,7 @@ public class PathBuilder {
      * @return This returns itself with the updated data.
      */
     public PathBuilder setZeroPowerAccelerationMultiplier(double set) {
-        this.paths.get(paths.size()-1).setZeroPowerAccelerationMultiplier(set);
+        this.paths.get(paths.size() - 1).setZeroPowerAccelerationMultiplier(set);
         return this;
     }
 
@@ -129,7 +161,7 @@ public class PathBuilder {
      * @return This returns itself with the updated data.
      */
     public PathBuilder setPathEndVelocityConstraint(double set) {
-        this.paths.get(paths.size()-1).setPathEndVelocityConstraint(set);
+        this.paths.get(paths.size() - 1).setPathEndVelocityConstraint(set);
         return this;
     }
 
@@ -140,7 +172,7 @@ public class PathBuilder {
      * @return This returns itself with the updated data.
      */
     public PathBuilder setPathEndTranslationalConstraint(double set) {
-        this.paths.get(paths.size()-1).setPathEndTranslationalConstraint(set);
+        this.paths.get(paths.size() - 1).setPathEndTranslationalConstraint(set);
         return this;
     }
 
@@ -151,7 +183,7 @@ public class PathBuilder {
      * @return This returns itself with the updated data.
      */
     public PathBuilder setPathEndHeadingConstraint(double set) {
-        this.paths.get(paths.size()-1).setPathEndHeadingConstraint(set);
+        this.paths.get(paths.size() - 1).setPathEndHeadingConstraint(set);
         return this;
     }
 
@@ -162,7 +194,7 @@ public class PathBuilder {
      * @return This returns itself with the updated data.
      */
     public PathBuilder setPathEndTValueConstraint(double set) {
-        this.paths.get(paths.size()-1).setPathEndTValueConstraint(set);
+        this.paths.get(paths.size() - 1).setPathEndTValueConstraint(set);
         return this;
     }
 
@@ -173,7 +205,7 @@ public class PathBuilder {
      * @return This returns itself with the updated data.
      */
     public PathBuilder setPathEndTimeoutConstraint(double set) {
-        this.paths.get(paths.size()-1).setPathEndTimeoutConstraint(set);
+        this.paths.get(paths.size() - 1).setPathEndTimeoutConstraint(set);
         return this;
     }
 
@@ -182,12 +214,12 @@ public class PathBuilder {
      * This callback is set to run at a specified number of milliseconds after the start of the path.
      *
      * @param time This sets the number of milliseconds of wait between the start of the Path and
-     *             the calling of the callback.
+     *         the calling of the callback.
      * @param runnable This sets the code for the callback to run. Use lambda statements for this.
      * @return This returns itself with the updated data.
      */
     public PathBuilder addTemporalCallback(double time, Runnable runnable) {
-        this.callbacks.add(new PathCallback(time, runnable, PathCallback.TIME, paths.size()-1));
+        this.callbacks.add(new PathCallback(time, runnable, PathCallback.TIME, paths.size() - 1));
         return this;
     }
 
@@ -200,7 +232,7 @@ public class PathBuilder {
      * @return This returns itself with the updated data.
      */
     public PathBuilder addParametricCallback(double t, Runnable runnable) {
-        this.callbacks.add(new PathCallback(t, runnable, PathCallback.PARAMETRIC, paths.size()-1));
+        this.callbacks.add(new PathCallback(t, runnable, PathCallback.PARAMETRIC, paths.size() - 1));
         return this;
     }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/Point.java

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration;
 
+import androidx.annotation.NonNull;
+
 import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
 
 /**
@@ -31,13 +33,13 @@ public class Point {
      * This creates a new Point with coordinate inputs and a specified coordinate system.
      *
      * @param rOrX Depending on the coordinate system specified, this is either the r or x value.
-     *             In polar coordinates, the r value is the distance from the origin.
+     *         In polar coordinates, the r value is the distance from the origin.
-     *             In Cartesian coordinates, the x value is the distance left/right from the origin.
+     *         In Cartesian coordinates, the x value is the distance left/right from the origin.
      * @param thetaOrY Depending on the coordinate system specified, this is either the theta or
-     *                 y value.
+     *         y value.
-     *                 In polar coordinates, the theta value is the angle from the positive x-axis.
+     *         In polar coordinates, the theta value is the angle from the positive x-axis.
-     *                 Increasing theta moves in the counter-clockwise direction.
+     *         Increasing theta moves in the counter-clockwise direction.
-     *                 In Cartesian coordinates, the y value is the distance up/down from the origin.
+     *         In Cartesian coordinates, the y value is the distance up/down from the origin.
      * @param identifier this specifies what coordinate system the coordinate inputs are in.
      */
     public Point(double rOrX, double thetaOrY, int identifier) {
@@ -53,17 +55,27 @@ public class Point {
         setCoordinates(pose.getX(), pose.getY(), CARTESIAN);
     }
 
+    /**
+     * This creates a new Point from a X and Y value.
+     *
+     * @param setX the X value.
+     * @param setY the Y value.
+     */
+    public Point(double setX, double setY) {
+        setCoordinates(setX, setY, CARTESIAN);
+    }
+
     /**
      * This sets the coordinates of the Point using the specified coordinate system.
      *
      * @param rOrX Depending on the coordinate system specified, this is either the r or x value.
-     *             In polar coordinates, the r value is the distance from the origin.
+     *         In polar coordinates, the r value is the distance from the origin.
-     *             In Cartesian coordinates, the x value is the distance left/right from the origin.
+     *         In Cartesian coordinates, the x value is the distance left/right from the origin.
      * @param thetaOrY Depending on the coordinate system specified, this is either the theta or
-     *                 y value.
+     *         y value.
-     *                 In polar coordinates, the theta value is the angle from the positive x-axis.
+     *         In polar coordinates, the theta value is the angle from the positive x-axis.
-     *                 Increasing theta moves in the counter-clockwise direction.
+     *         Increasing theta moves in the counter-clockwise direction.
-     *                 In Cartesian coordinates, the y value is the distance up/down from the origin.
+     *         In Cartesian coordinates, the y value is the distance up/down from the origin.
      * @param identifier this specifies what coordinate system to use when setting values.
      */
     public void setCoordinates(double rOrX, double thetaOrY, int identifier) {
@@ -78,9 +90,9 @@ public class Point {
                 theta = setOtherCoordinates[1];
                 break;
             default:
-                if (rOrX<0) {
+                if (rOrX < 0) {
                     r = -rOrX;
-                    theta = MathFunctions.normalizeAngle(thetaOrY+Math.PI);
+                    theta = MathFunctions.normalizeAngle(thetaOrY + Math.PI);
                 } else {
                     r = rOrX;
                     theta = MathFunctions.normalizeAngle(thetaOrY);
@@ -99,7 +111,7 @@ public class Point {
      * @return returns the distance between the two Points.
      */
     public double distanceFrom(Point otherPoint) {
-        return Math.sqrt(Math.pow(otherPoint.getX()-x, 2) + Math.pow(otherPoint.getY()-y, 2));
+        return Math.sqrt(Math.pow(otherPoint.getX() - x, 2) + Math.pow(otherPoint.getY() - y, 2));
     }
 
     /**
@@ -110,7 +122,7 @@ public class Point {
      * @return this returns the x and y values, in that order, in an Array of doubles.
      */
     public static double[] polarToCartesian(double r, double theta) {
-        return new double[] {r * Math.cos(theta), r * Math.sin(theta)};
+        return new double[]{r * Math.cos(theta), r * Math.sin(theta)};
     }
 
     /**
@@ -123,17 +135,17 @@ public class Point {
     public static double[] cartesianToPolar(double x, double y) {
         if (x == 0) {
             if (y > 0) {
-                return new double[] {Math.abs(y), Math.PI/2};
+                return new double[]{Math.abs(y), Math.PI / 2};
             } else {
-                return new double[] {Math.abs(y), (3 * Math.PI) / 2};
+                return new double[]{Math.abs(y), (3 * Math.PI) / 2};
             }
         }
-        double r = Math.sqrt(x*x+y*y);
+        double r = Math.sqrt(x * x + y * y);
-        if (x < 0) return new double[] {r, Math.PI+Math.atan(y/x)};
+        if (x < 0) return new double[]{r, Math.PI + Math.atan(y / x)};
         if (y > 0) {
             return new double[]{r, Math.atan(y / x)};
         } else {
-            return new double[]{r, (2*Math.PI) + Math.atan(y / x)};
+            return new double[]{r, (2 * Math.PI) + Math.atan(y / x)};
         }
     }
 
@@ -182,4 +194,10 @@ public class Point {
     public Point copy() {
         return new Point(getX(), getY(), CARTESIAN);
     }
+
+    @NonNull
+    @Override
+    public String toString() {
+        return "(" + getX() + ", " + getY() + ")";
+    }
 }

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

@@ -2,11 +2,11 @@ package org.firstinspires.ftc.teamcode.pedroPathing.tuning;
 
 import com.acmerobotics.dashboard.config.Config;
 
-import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomFilteredPIDFCoefficients;
-import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomPIDFCoefficients;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
+import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomFilteredPIDFCoefficients;
+import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomPIDFCoefficients;
 import org.firstinspires.ftc.teamcode.pedroPathing.util.KalmanFilterParameters;
 
 /**
@@ -33,7 +33,7 @@ public class FollowerConstants {
     private static double xMovement = 81.34056;
     private static double yMovement = 65.43028;
     private static double[] convertToPolar = Point.cartesianToPolar(xMovement, -yMovement);
-    public static Vector frontLeftVector = MathFunctions.normalizeVector(new Vector(convertToPolar[0],convertToPolar[1]));
+    public static Vector frontLeftVector = MathFunctions.normalizeVector(new Vector(convertToPolar[0], convertToPolar[1]));
 
 
     // Translational PIDF coefficients (don't use integral)
@@ -182,7 +182,7 @@ public class FollowerConstants {
 
 
     // the limit at which the heading PIDF switches between the main and secondary heading PIDFs
-    public static double headingPIDFSwitch = Math.PI/20;
+    public static double headingPIDFSwitch = Math.PI / 20;
 
     // Secondary heading error PIDF coefficients
     public static CustomPIDFCoefficients secondaryHeadingPIDFCoefficients = new CustomPIDFCoefficients(

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

@@ -40,7 +40,7 @@ import java.util.List;
  * @version 1.0, 3/13/2024
  */
 @Config
-@Autonomous (name = "Forward Velocity Tuner", group = "Autonomous Pathing Tuning")
+@Autonomous(name = "Forward Velocity Tuner", group = "Autonomous Pathing Tuning")
 public class ForwardVelocityTuner extends OpMode {
     private ArrayList<Double> velocities = new ArrayList<>();
 
@@ -122,6 +122,10 @@ public class ForwardVelocityTuner extends OpMode {
     @Override
     public void loop() {
         if (gamepad1.cross || gamepad1.a) {
+            for (DcMotorEx motor : motors) {
+                motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+                motor.setPower(0);
+            }
             requestOpModeStop();
         }
 

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

@@ -40,7 +40,7 @@ import java.util.List;
  * @version 1.0, 3/13/2024
  */
 @Config
-@Autonomous (name = "Forward Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning")
+@Autonomous(name = "Forward Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning")
 public class ForwardZeroPowerAccelerationTuner extends OpMode {
     private ArrayList<Double> accelerations = new ArrayList<>();
 
@@ -120,6 +120,10 @@ public class ForwardZeroPowerAccelerationTuner extends OpMode {
     @Override
     public void loop() {
         if (gamepad1.cross || gamepad1.a) {
+            for (DcMotorEx motor : motors) {
+                motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+                motor.setPower(0);
+            }
             requestOpModeStop();
         }
 
@@ -149,7 +153,7 @@ public class ForwardZeroPowerAccelerationTuner extends OpMode {
             for (Double acceleration : accelerations) {
                 average += acceleration;
             }
-            average /= (double)accelerations.size();
+            average /= (double) accelerations.size();
 
             telemetryA.addData("forward zero power acceleration (deceleration):", average);
             telemetryA.update();

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

@@ -40,7 +40,7 @@ import java.util.List;
  * @version 1.0, 3/13/2024
  */
 @Config
-@Autonomous (name = "Lateral Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning")
+@Autonomous(name = "Lateral Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning")
 public class LateralZeroPowerAccelerationTuner extends OpMode {
     private ArrayList<Double> accelerations = new ArrayList<>();
 
@@ -120,11 +120,15 @@ public class LateralZeroPowerAccelerationTuner extends OpMode {
     @Override
     public void loop() {
         if (gamepad1.cross || gamepad1.a) {
+            for (DcMotorEx motor : motors) {
+                motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+                motor.setPower(0);
+            }
             requestOpModeStop();
         }
 
         poseUpdater.update();
-        Vector heading = new Vector(1.0, poseUpdater.getPose().getHeading() - Math.PI/2);
+        Vector heading = new Vector(1.0, poseUpdater.getPose().getHeading() - Math.PI / 2);
         if (!end) {
             if (!stopping) {
                 if (MathFunctions.dotProduct(poseUpdater.getVelocity(), heading) > VELOCITY) {
@@ -149,7 +153,7 @@ public class LateralZeroPowerAccelerationTuner extends OpMode {
             for (Double acceleration : accelerations) {
                 average += acceleration;
             }
-            average /= (double)accelerations.size();
+            average /= (double) accelerations.size();
 
             telemetryA.addData("lateral zero power acceleration (deceleration):", average);
             telemetryA.update();

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

@@ -40,7 +40,7 @@ import java.util.List;
  * @version 1.0, 3/13/2024
  */
 @Config
-@Autonomous (name = "Strafe Velocity Tuner", group = "Autonomous Pathing Tuning")
+@Autonomous(name = "Strafe Velocity Tuner", group = "Autonomous Pathing Tuning")
 public class StrafeVelocityTuner extends OpMode {
     private ArrayList<Double> velocities = new ArrayList<>();
 
@@ -120,6 +120,10 @@ public class StrafeVelocityTuner extends OpMode {
     @Override
     public void loop() {
         if (gamepad1.cross || gamepad1.a) {
+            for (DcMotorEx motor : motors) {
+                motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+                motor.setPower(0);
+            }
             requestOpModeStop();
         }
 
@@ -132,7 +136,7 @@ public class StrafeVelocityTuner extends OpMode {
                     motor.setPower(0);
                 }
             } else {
-                double currentVelocity = Math.abs(MathFunctions.dotProduct(poseUpdater.getVelocity(), new Vector(1, Math.PI/2)));
+                double currentVelocity = Math.abs(MathFunctions.dotProduct(poseUpdater.getVelocity(), new Vector(1, Math.PI / 2)));
                 velocities.add(currentVelocity);
                 velocities.remove(0);
             }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/util/Drawing.java

@@ -20,11 +20,14 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
  * @version 1.0, 4/22/2024
  */
 public class Drawing {
+    public static final double ROBOT_RADIUS = 9;
+
     private static TelemetryPacket packet;
 
     /**
      * This draws everything that will be used in the Follower's telemetryDebug() method. This takes
      * a Follower as an input, so an instance of the DashbaordDrawingHandler class is not needed.
+     *
      * @param follower
      */
     public static void drawDebug(Follower follower) {
@@ -35,6 +38,7 @@ public class Drawing {
         }
         drawPoseHistory(follower.getDashboardPoseTracker(), "#4CAF50");
         drawRobot(follower.getPose(), "#4CAF50");
+
         sendPacket();
     }
 
@@ -114,12 +118,10 @@ public class Drawing {
      * @param t the Point to draw at
      */
     public static void drawRobotOnCanvas(Canvas c, Point t) {
-        final double ROBOT_RADIUS = 9;
-
         c.setStrokeWidth(1);
         c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS);
 
-        Vector halfv = new Vector(0.5*ROBOT_RADIUS, t.getTheta());
+        Vector halfv = new Vector(0.5 * ROBOT_RADIUS, t.getTheta());
         Vector p1 = MathFunctions.addVectors(halfv, new Vector(t.getR(), t.getTheta()));
         Vector p2 = MathFunctions.addVectors(p1, halfv);
         c.strokeLine(p1.getXComponent(), p1.getYComponent(), p2.getXComponent(), p2.getYComponent());
@@ -133,8 +135,6 @@ public class Drawing {
      * @param t the Pose to draw at
      */
     public static void drawRobotOnCanvas(Canvas c, Pose t) {
-        final double ROBOT_RADIUS = 9;
-
         c.strokeCircle(t.getX(), t.getY(), ROBOT_RADIUS);
         Vector v = t.getHeadingVector();
         v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS);