Rotate robot diagrams in localizer files to reduce confusion

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

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;
 
@@ -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/localization/localizers/OTOSLocalizer.java

@@ -21,18 +21,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
  *
  * forward on robot is the x positive direction
  *
+ *                         forward (x positive)
+ *                                △
+ *                                |
+ *                                |
  *                         /--------------\
+ *                         |              |
+ *                         |              |
+ *                         | ||        || |
+ *  left (y positive) <--- | ||        || |  
  *                         |     ____     |
  *                         |     ----     |
- *    | ||        || |
- *    | ||        || |  ----> left (y positive)
- *    |              |
- *    |              |
  *                         \--------------/
- *           |
- *           |
- *           V
- *    forward (x positive)
  *
  * @author Anyi Lin - 10158 Scott's Bots
  * @version 1.0, 7/20/2024
@@ -108,7 +108,10 @@ public class OTOSLocalizer extends Localizer {
      */
     @Override
     public Pose getPose() {
-        return MathFunctions.addPoses(startPose, new Pose(otosPose.x, otosPose.y, otosPose.h));
+        SparkFunOTOS.Pose2D rawPose = otos.getPosition();
+        Pose pose = new Pose(rawPose.x, rawPose.y, rawPose.h);
+
+        return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false));
     }
 
     /**

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

@@ -23,18 +23,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
          *
          * forward on robot is the x positive direction
          *
+         *                    forward (x positive)
+         *                                △
+         *                                |
+         *                                |
          *                         /--------------\
+         *                         |              |
+         *                         |              |
+         *                         |           || |
+         *  left (y positive) <--- |           || |  
          *                         |     ____     |
          *                         |     ----     |
-         *    | ||           |
-         *    | ||           |  ----> left (y positive)
-         *    |              |
-         *    |              |
          *                         \--------------/
-         *           |
-         *           |
-         *           V
-         *    forward (x positive)
          * With the pinpoint your readings will be used in mm
          * to use inches ensure to divide your mm value by 25.4
          * @author Logan Nash
@@ -96,8 +96,10 @@ public class PinpointLocalizer extends Localizer {
      */
     @Override
     public Pose getPose() {
-        Pose2D pose = odo.getPosition();
+        Pose2D rawPose = odo.getPosition();
-        return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), pose.getHeading(AngleUnit.RADIANS));
+        Pose pose = new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS));
+
+        return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false));
     }
 
     /**

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

@@ -26,18 +26,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
  *
  * forward on robot is the x positive direction
  *
+ *                         forward (x positive)
+ *                                △
+ *                                |
+ *                                |
  *                         /--------------\
+ *                         |              |
+ *                         |              |
+ *                         | ||        || |
+ *  left (y positive) <--- | ||        || |  
  *                         |     ____     |
  *                         |     ----     |
- *    | ||        || |
- *    | ||        || |  ----> left (y positive)
- *    |              |
- *    |              |
  *                         \--------------/
- *           |
- *           |
- *           V
- *    forward (x positive)
  *
  * @author Logan Nash
  * @author Anyi Lin - 10158 Scott's Bots

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

@@ -23,18 +23,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
  *
  * forward on robot is the x positive direction
  *
+ *                         forward (x positive)
+ *                                △
+ *                                |
+ *                                |
  *                         /--------------\
+ *                         |              |
+ *                         |              |
+ *                         | ||        || |
+ *  left (y positive) <--- | ||        || |  
  *                         |     ____     |
  *                         |     ----     |
- *    | ||        || |
- *    | ||        || |  ----> left (y positive)
- *    |              |
- *    |              |
  *                         \--------------/
- *           |
- *           |
- *           V
- *    forward (x positive)
  *
  * @author Anyi Lin - 10158 Scott's Bots
  * @version 1.0, 4/2/2024

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

@@ -26,18 +26,18 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.NanoTimer;
  *
  * forward on robot is the x positive direction
  *
+*                         forward (x positive)
+ *                                △
+ *                                |
+ *                                |
  *                         /--------------\
+ *                         |              |
+ *                         |              |
+ *                         |           || |
+ *  left (y positive) <--- |           || |  
  *                         |     ____     |
  *                         |     ----     |
- *    | ||        || |
- *    | ||        || |  ----> left (y positive)
- *    |              |
- *    |              |
  *                         \--------------/
- *           |
- *           |
- *           V
- *    forward (x positive)
  *
  * @author Anyi Lin - 10158 Scott's Bots
  * @version 1.0, 4/2/2024

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

@@ -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.
@@ -172,6 +172,22 @@ public class MathFunctions {
         return new Pose(one.getX() - two.getX(), one.getY() - two.getY(), one.getHeading() - two.getHeading());
     }
 
+    /**
+     * This rotates the given pose by the given theta,
+     *
+     * @param pose the Pose to rotate.
+     * @param theta the angle to rotate by.
+     * @param rotateHeading whether to adjust the Pose heading too.
+     * @return the rotated Pose.
+     */
+    public static Pose rotatePose(Pose pose, double theta, boolean rotateHeading) {
+        double x = pose.getX() * Math.cos(theta) - pose.getY() * Math.sin(theta);
+        double y = pose.getX() * Math.sin(theta) + pose.getY() * Math.cos(theta);
+        double heading = rotateHeading ? normalizeAngle(pose.getHeading() + theta) : pose.getHeading();
+
+        return new Pose(x, y, heading);
+    }
+
     /**
      * This multiplies a Point by a scalar and returns the result as a Point
      *
@@ -286,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,6 +51,37 @@ 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.
      *
@@ -101,6 +132,7 @@ public class PathBuilder {
         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

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;
 
 /**
@@ -53,6 +55,16 @@ 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.
      *
@@ -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;
 
 /**

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

@@ -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

@@ -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();
         }
 

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

@@ -120,6 +120,10 @@ 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();
         }
 

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

@@ -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();
         }
 

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,8 +118,6 @@ 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);
 
@@ -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);