Lots of minor API Updates/cleanup

Path/src/main/java/com/technototes/path/subsystem/PathingMecanumDrivebaseSubsystem.java

@@ -123,10 +123,10 @@ public PathingMecanumDrivebaseSubsystem(
             c.getDouble(WheelBase.class),
             c.getDouble(LateralMult.class)
         );
-        leftFront = fl.getDevice();
-        leftRear = rl.getDevice();
-        rightRear = rr.getDevice();
-        rightFront = fr.getDevice();
+        leftFront = fl.getRawMotor(DcMotorEx.class);
+        leftRear = rl.getRawMotor(DcMotorEx.class);
+        rightRear = rr.getRawMotor(DcMotorEx.class);
+        rightFront = fr.getRawMotor(DcMotorEx.class);
 
         motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
 

Path/src/main/java/com/technototes/path/subsystem/TwoDeadWheelLocalizer.java

@@ -0,0 +1,104 @@
+package com.technototes.path.subsystem;
+
+import static com.technototes.path.subsystem.DeadWheelConstants.EncoderOverflow;
+import static com.technototes.path.subsystem.DeadWheelConstants.ForwardOffset;
+import static com.technototes.path.subsystem.DeadWheelConstants.GearRatio;
+import static com.technototes.path.subsystem.DeadWheelConstants.LateralDistance;
+import static com.technototes.path.subsystem.DeadWheelConstants.TicksPerRev;
+import static com.technototes.path.subsystem.DeadWheelConstants.WheelRadius;
+
+import androidx.annotation.NonNull;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
+import com.acmerobotics.roadrunner.localization.TwoTrackingWheelLocalizer;
+import com.technototes.library.hardware.sensor.IMU;
+import com.technototes.library.hardware.sensor.encoder.MotorEncoder;
+import com.technototes.library.subsystem.Subsystem;
+import java.util.Arrays;
+import java.util.List;
+import java.util.function.DoubleSupplier;
+
+/*
+ * Sample tracking wheel localizer implementation assuming a standard configuration:
+ *
+ *    /---------------\
+ *    |      ____     |
+ *    |      ----     |
+ *    | ||      ^     |
+ *    | ||<- lr |     |
+ *    |         fb    |
+ *    |               |
+ *    \---------------/
+ *
+ * COMPLETELY UNTESTED!!
+ */
+public class TwoDeadWheelLocalizer extends TwoTrackingWheelLocalizer implements Subsystem {
+
+    protected MotorEncoder leftrightEncoder, frontbackEncoder;
+    protected DoubleSupplier headingSupplier;
+    protected double lateralDistance, forwardOffset, gearRatio, wheelRadius, ticksPerRev;
+
+    protected boolean encoderOverflow;
+
+    public TwoDeadWheelLocalizer(IMU imu, MotorEncoder lr, MotorEncoder fb, DeadWheelConstants constants) {
+        super(
+            Arrays.asList(
+                new Pose2d(0, constants.getDouble(LateralDistance.class), 0), // left
+                new Pose2d(constants.getDouble(ForwardOffset.class), 0, Math.toRadians(90)) // front
+            )
+        );
+        leftrightEncoder = lr;
+        frontbackEncoder = fb;
+        headingSupplier = () -> imu.gyroHeading();
+
+        lateralDistance = constants.getDouble(LateralDistance.class);
+        forwardOffset = constants.getDouble(ForwardOffset.class);
+        encoderOverflow = constants.getBoolean(EncoderOverflow.class);
+        gearRatio = constants.getDouble(GearRatio.class);
+        ticksPerRev = constants.getDouble(TicksPerRev.class);
+        wheelRadius = constants.getDouble(WheelRadius.class);
+    }
+
+    public double encoderTicksToInches(double ticks) {
+        return ((getWheelRadius() * 2 * Math.PI * getGearRatio() * ticks) / getTicksPerRev());
+    }
+
+    @NonNull
+    @Override
+    public List<Double> getWheelPositions() {
+        return Arrays.asList(
+            encoderTicksToInches(leftrightEncoder.getCurrentPosition()),
+            encoderTicksToInches(frontbackEncoder.getCurrentPosition())
+        );
+    }
+
+    @NonNull
+    @Override
+    public List<Double> getWheelVelocities() {
+        // TODO: If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other
+        //  competing magnetic encoders), change Encoder.getRawVelocity() to Encoder.getCorrectedVelocity() to enable a
+        //  compensation method
+
+        return Arrays.asList(
+            encoderTicksToInches(leftrightEncoder.getCorrectedVelocity()),
+            encoderTicksToInches(frontbackEncoder.getCorrectedVelocity())
+        );
+    }
+
+    public double getTicksPerRev() {
+        return ticksPerRev;
+    }
+
+    public double getWheelRadius() {
+        return wheelRadius;
+    }
+
+    public double getGearRatio() {
+        return gearRatio;
+    }
+
+    @Override
+    public double getHeading() {
+        return headingSupplier.getAsDouble();
+    }
+}

Path/src/main/java/com/technototes/path/trajectorysequence/TrajectoryPath.java

@@ -0,0 +1,176 @@
+package com.technototes.path.trajectorysequence;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.technototes.path.geometry.ConfigurablePose;
+import com.technototes.path.geometry.ConfigurablePoseD;
+import java.util.function.Function;
+
+public interface TrajectoryPath extends Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence> {
+    /**
+     * Create a spline between two poses
+     *
+     * @param start The beginning of a spline
+     * @param end The end of a spline
+     * @return A function that takes a 'start' location and returns a TrajectorySequenceBuilder
+     */
+    static TrajectoryPath splineTo(ConfigurablePose start, ConfigurablePose end) {
+        return b -> b.apply(start.toPose()).splineTo(end.toVec(), end.getHeading()).build();
+    }
+
+    /**
+     * Create a spline between two poses
+     *
+     * @param start The beginning of a spline
+     * @param end The end of a spline
+     * @return A function that takes a 'start' location and returns a TrajectorySequenceBuilder
+     */
+    static TrajectoryPath splineTo(ConfigurablePoseD start, ConfigurablePoseD end) {
+        return b -> b.apply(start.toPose()).splineTo(end.toVec(), end.getHeading()).build();
+    }
+
+    /**
+     * Create a spline between a list of poses
+     *
+     * @param start The beginning of a spline
+     * @param points The remaing points of the spline
+     * @return A function that takes a 'start' location and returns a TrajectorySequenceBuilder
+     */
+    static TrajectoryPath splinesTo(ConfigurablePoseD start, ConfigurablePoseD... points) {
+        return b -> {
+            TrajectorySequenceBuilder bld = b.apply(start.toPose());
+            for (ConfigurablePoseD d : points) {
+                bld = bld.splineTo(d.toVec(), d.getHeading());
+            }
+            return bld.build();
+        };
+    }
+
+    /**
+     * Create a spline between a list of poses
+     *
+     * @param start The beginning of a spline
+     * @param points The remaing points of the spline
+     * @return A function that takes a 'start' location and returns a TrajectorySequenceBuilder
+     */
+    static TrajectoryPath splinesTo(ConfigurablePose start, ConfigurablePose... points) {
+        return b -> {
+            TrajectorySequenceBuilder bld = b.apply(start.toPose());
+            for (ConfigurablePose d : points) {
+                bld = bld.splineTo(d.toVec(), d.getHeading());
+            }
+            return bld.build();
+        };
+    }
+
+    /**
+     * Create a line between two poses
+     *
+     * @param start The beginning of the line
+     * @param end The end of the line
+     * @return A function that takes a 'start' location and returns a TrajectorySequenceBuilder
+     */
+    static TrajectoryPath lineTo(ConfigurablePose start, ConfigurablePose end) {
+        return b -> b.apply(start.toPose()).lineTo(end.toPose().vec()).build();
+    }
+
+    /**
+     * Create a line between two poses
+     *
+     * @param start The beginning of the line
+     * @param end The end of the line
+     * @return A function that takes a 'start' location and returns a TrajectorySequenceBuilder
+     */
+    static TrajectoryPath lineTo(ConfigurablePoseD start, ConfigurablePoseD end) {
+        return b -> b.apply(start.toPose()).lineTo(end.toPose().vec()).build();
+    }
+
+    /**
+     * Create a line between a list of poses
+     *
+     * @param start The beginning of the line
+     * @param points The points of the line
+     * @return A function that takes a 'start' location and returns a TrajectorySequenceBuilder
+     */
+    static TrajectoryPath linesTo(ConfigurablePoseD start, ConfigurablePoseD... points) {
+        return b -> {
+            TrajectorySequenceBuilder bld = b.apply(start.toPose());
+            for (ConfigurablePoseD d : points) {
+                bld = bld.lineTo(d.toPose().vec());
+            }
+            return bld.build();
+        };
+    }
+
+    /**
+     * Create a line between a list of poses
+     *
+     * @param start The beginning of the line
+     * @param points The points of the line
+     * @return A function that takes a 'start' location and returns a TrajectorySequenceBuilder
+     */
+    static TrajectoryPath linesTo(ConfigurablePose start, ConfigurablePose... points) {
+        return b -> {
+            TrajectorySequenceBuilder bld = b.apply(start.toPose());
+            for (ConfigurablePose d : points) {
+                bld = bld.lineTo(d.toPose().vec());
+            }
+            return bld.build();
+        };
+    }
+
+    /**
+     * Create a line between two poses on a specific linear heading
+     *
+     * @param start The beginning of the line
+     * @param end The end of the line
+     * @return A function that takes a 'start' location and returns a TrajectorySequenceBuilder
+     */
+    static TrajectoryPath lineToLinearHeading(ConfigurablePose start, ConfigurablePose end) {
+        return b -> b.apply(start.toPose()).lineToLinearHeading(end.toPose()).build();
+    }
+
+    /**
+     * Create a line between two poses on a specific linear heading
+     *
+     * @param start The beginning of the line
+     * @param end The end of the line
+     * @return A function that takes a 'start' location and returns a TrajectorySequenceBuilder
+     */
+    static TrajectoryPath lineToLinearHeading(ConfigurablePoseD start, ConfigurablePoseD end) {
+        return b -> b.apply(start.toPose()).lineToLinearHeading(end.toPose()).build();
+    }
+
+    /**
+     * Create a set of lines between a list of  poses on a specific linear heading
+     *
+     * @param start The beginning of the line
+     * @param points The list of points in the line
+     * @return A function that takes a 'start' location and returns a TrajectorySequenceBuilder
+     */
+    static TrajectoryPath linesToLinearHeadings(ConfigurablePoseD start, ConfigurablePoseD... points) {
+        return b -> {
+            TrajectorySequenceBuilder bld = b.apply(start.toPose());
+            for (ConfigurablePoseD d : points) {
+                bld = bld.lineToLinearHeading(d.toPose());
+            }
+            return bld.build();
+        };
+    }
+
+    /**
+     * Create a set of lines between a list of  poses on a specific linear heading
+     *
+     * @param start The beginning of the line
+     * @param points The list of points in the line
+     * @return A function that takes a 'start' location and returns a TrajectorySequenceBuilder
+     */
+    static TrajectoryPath linesToLinearHeadings(ConfigurablePose start, ConfigurablePose... points) {
+        return b -> {
+            TrajectorySequenceBuilder bld = b.apply(start.toPose());
+            for (ConfigurablePose d : points) {
+                bld = bld.lineToLinearHeading(d.toPose());
+            }
+            return bld.build();
+        };
+    }
+}