Adapter for Road Runner added for easier transition to Pedro Pathing localizer, as well as adding previously omitted files

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

@@ -0,0 +1,34 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.localization;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
+
+/**
+ * This is the Localizer class. It is an abstract superclass of all localizers used in Pedro Pathing,
+ * so it contains abstract methods that will have a concrete implementation in the subclasses. Any
+ * method that all localizers will need will be in this class.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @version 1.0, 4/2/2024
+ */
+public abstract class Localizer {
+
+    public abstract Pose getPose();
+
+    public abstract Pose getVelocity();
+
+    public abstract Vector getVelocityVector();
+
+    public abstract void setStartPose(Pose setStart);
+
+    public abstract void setPose(Pose setPose);
+
+    public abstract void update();
+
+    public abstract double getTotalHeading();
+
+    public abstract double getForwardMultiplier();
+
+    public abstract double getLateralMultiplier();
+
+    public abstract double getTurningMultiplier();
+}

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

@@ -0,0 +1,109 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.localization;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
+
+/**
+ * This is the Pose class. It defines poses in 2D space, like the Pose2D class in Road Runner except
+ * in the Pedro Pathing code so I don't have to import the Road Runner library. A Pose consists of
+ * two coordinates defining a position and a third value for the heading, so basically just defining
+ * any position and orientation the robot can be at, unless your robot can fly for whatever reason.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @version 1.0, 4/2/2024
+ */
+public class Pose {
+    private double x;
+    private double y;
+    private double heading;
+
+    public Pose(double setX, double setY, double setHeading) {
+        setX(setX);
+        setY(setY);
+        setHeading(setHeading);
+    }
+
+    public Pose(double setX, double setY) {
+        this(setX, setY, 0);
+    }
+
+    public Pose() {
+        this(0,0,0);
+    }
+
+    public void setX(double set) {
+        x = set;
+    }
+
+    public void setY(double set) {
+        y = set;
+    }
+
+    public void setHeading(double set) {
+        heading = MathFunctions.normalizeAngle(set);
+    }
+
+    public double getX() {
+        return x;
+    }
+
+    public double getY() {
+        return y;
+    }
+
+    public double getHeading() {
+        return heading;
+    }
+
+    public Vector getVector() {
+        Vector returnVector = new Vector();
+        returnVector.setOrthogonalComponents(x, y);
+        return returnVector;
+    }
+
+    public Vector getHeadingVector() {
+        return new Vector(1, heading);
+    }
+
+    public void add(Pose pose) {
+        setX(x + pose.getX());
+        setY(y + pose.getY());
+        setHeading(heading + pose.getHeading());
+    }
+
+    public void subtract(Pose pose) {
+        setX(x - pose.getX());
+        setY(y - pose.getY());
+        setHeading(heading - pose.getHeading());
+    }
+
+    public void scalarMultiply(double scalar) {
+        setX(x * scalar);
+        setY(y * scalar);
+        setHeading(heading * scalar);
+    }
+
+    public void scalarDivide(double scalar) {
+        setX(x / scalar);
+        setY(y / scalar);
+        setHeading(heading / scalar);
+    }
+
+    public void flipSigns() {
+        setX(-x);
+        setY(-y);
+        setHeading(-heading);
+    }
+
+    public boolean roughlyEquals(Pose pose, double accuracy) {
+        return MathFunctions.roughlyEquals(x, pose.getX(), accuracy) && MathFunctions.roughlyEquals(y, pose.getY(), accuracy) && MathFunctions.roughlyEquals(MathFunctions.getSmallestAngleDifference(heading, pose.getHeading()), 0, accuracy);
+    }
+
+    public boolean roughlyEquals(Pose pose) {
+        return roughlyEquals(pose, 0.0001);
+    }
+
+    public Pose copy() {
+        return new Pose(getX(), getY(), getHeading());
+    }
+}

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

@@ -0,0 +1,93 @@
+//package org.firstinspires.ftc.teamcode.pedroPathing.localization;
+//
+//import com.acmerobotics.roadrunner.geometry.Pose2d;
+//import com.qualcomm.robotcore.hardware.HardwareMap;
+//
+//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
+//import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
+//
+//import java.util.ArrayList;
+//import java.util.List;
+//
+///**
+// * This is the RRToPedroThreeWheelLocalizer class. This class extends the Localizer superclass and
+// * is intended to adapt the old Road Runner three wheel odometry localizer to the new Pedro Pathing
+// * localizer system.
+// *
+// * @author Anyi Lin - 10158 Scott's Bots
+// * @version 1.0, 5/9/2024
+// */
+//public class RRToPedroThreeWheelLocalizer extends Localizer {
+//    private RoadRunnerThreeWheelLocalizer localizer;
+//    private double totalHeading;
+//    private Pose startPose;
+//    private Pose previousPose;
+//
+//    public RRToPedroThreeWheelLocalizer(HardwareMap hardwareMap) {
+//        List<Integer> lastTrackingEncPositions = new ArrayList<>();
+//        List<Integer> lastTrackingEncVels = new ArrayList<>();
+//
+//        localizer = new RoadRunnerThreeWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels);
+//
+//        startPose = new Pose();
+//        previousPose = new Pose();
+//    }
+//    @Override
+//    public Pose getPose() {
+//        Pose2d pose = localizer.getPoseEstimate();
+//        return new Pose(pose.getX(), pose.getY(), pose.getHeading());
+//    }
+//
+//    @Override
+//    public Pose getVelocity() {
+//        Pose2d pose = localizer.getPoseVelocity();
+//        return new Pose(pose.getX(), pose.getY(), pose.getHeading());
+//    }
+//
+//    @Override
+//    public Vector getVelocityVector() {
+//        Pose2d pose = localizer.getPoseVelocity();
+//        Vector returnVector = new Vector();
+//        returnVector.setOrthogonalComponents(pose.getX(), pose.getY());
+//        return returnVector;
+//    }
+//
+//    @Override
+//    public void setStartPose(Pose setStart) {
+//        Pose oldStart = startPose;
+//        startPose = setStart;
+//        Pose startDiff = MathFunctions.subtractPoses(startPose, oldStart);
+//        localizer.setPoseEstimate(new Pose2d(getPose().getX() + startDiff.getX(), getPose().getY() + startDiff.getY(), getPose().getHeading() + startDiff.getHeading()));
+//    }
+//
+//    @Override
+//    public void setPose(Pose setPose) {
+//        localizer.setPoseEstimate(new Pose2d(setPose.getX(), setPose.getY(), setPose.getHeading()));
+//    }
+//
+//    @Override
+//    public void update() {
+//        totalHeading += MathFunctions.getTurnDirection(previousPose.getHeading(), getPose().getHeading()) * MathFunctions.getSmallestAngleDifference(previousPose.getHeading(), getPose().getHeading());
+//        previousPose = getPose();
+//    }
+//
+//    @Override
+//    public double getTotalHeading() {
+//        return totalHeading;
+//    }
+//
+//    @Override
+//    public double getForwardMultiplier() {
+//        return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.X_MULTIPLIER;
+//    }
+//
+//    @Override
+//    public double getLateralMultiplier() {
+//        return RoadRunnerThreeWheelLocalizer.encoderTicksToInches(1) * RoadRunnerThreeWheelLocalizer.Y_MULTIPLIER;
+//    }
+//
+//    @Override
+//    public double getTurningMultiplier() {
+//        return (getForwardMultiplier() + getLateralMultiplier()) / 2;
+//    }
+//}