IMUOp redesign (#34) and Cartesian drive revamp

src/main/java/org/murraybridgebunyips/bunyipslib/Direction.kt

@@ -2,6 +2,9 @@ package org.murraybridgebunyips.bunyipslib
 
 import com.acmerobotics.roadrunner.geometry.Pose2d
 import com.acmerobotics.roadrunner.geometry.Vector2d
+import org.murraybridgebunyips.bunyipslib.external.units.Angle
+import org.murraybridgebunyips.bunyipslib.external.units.Measure
+import org.murraybridgebunyips.bunyipslib.external.units.Units.Radians
 
 /**
  * Represents relative positions in 2D space.
@@ -39,11 +42,8 @@ enum class Direction(
         ANTICLOCKWISE(Pose2d(0.0, 0.0, Math.toRadians(90.0)));
     }
 
-    val degrees: Double
-        get() = Math.toDegrees(vector.angle())
-
-    val radians: Double
-        get() = vector.angle()
+    val angle: Measure<Angle>
+        get() = Radians.of(vector.angle())
 
     companion object {
         /**

src/main/java/org/murraybridgebunyips/bunyipslib/UserSelection.kt

@@ -15,7 +15,8 @@ import java.util.function.Consumer
  * Keep in mind this thread runs in the background so it is not guaranteed to be ready during any
  * specific phase of your init-cycle. It is recommended to check if this thread is running using
  * `Threads.isRunning(selector)` in your `onInitLoop()` to ensure BOM knows it has to wait for the
- * user to make a selection. If you do not do this, the OpMode will assume it is ready to run regardless.
+ * user to make a selection. Alternatively, you can set an init-task that is a `WaitForTask`.
+ * If you do not do this, the OpMode will assume it is ready to run regardless.
  *
  * The result of this thread will be stored in the `result` property, which you can access yourself
  * or you can attach a callback to the `callback` property to be run once the thread is complete.

src/main/java/org/murraybridgebunyips/bunyipslib/drive/CartesianFieldCentricMecanumDrive.java

@@ -1,20 +1,28 @@
 package org.murraybridgebunyips.bunyipslib.drive;
 
+import static org.murraybridgebunyips.bunyipslib.external.units.Units.Degrees;
+
 import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.IMU;
 
-import org.murraybridgebunyips.bunyipslib.Controls;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.murraybridgebunyips.bunyipslib.Direction;
-import org.murraybridgebunyips.bunyipslib.subsystems.IMUOp;
 
 /**
  * Variant of the Cartesian MecanumDrive that uses field-centric controls, accounting for robot heading.
+ * <p>
  * This system is deprecated by RoadRunner, but remains for legacy/non-RoadRunner purposes.
+ * Coordinates used in Cartesian drives include a Cartesian speed mapping, where X is the horizontal axis, and
+ * Y is the forward axis. This is rotated differently within RoadRunner-derived drive bases.
+ * <p>
+ * Note: CFCMD no longer relies on an IMUOp subsystem, and can simply be used with a standard IMU from your config.
  *
  * @author Lucas Bubner, 2023
  * @see MecanumDrive
  */
 public class CartesianFieldCentricMecanumDrive extends CartesianMecanumDrive {
-    private IMUOp imu;
+    private IMU imu;
+    private double offsetDeg;
 
     /**
      * Constructs a new CartesianFieldCentricMecanumDrive.
@@ -27,59 +35,24 @@ public class CartesianFieldCentricMecanumDrive extends CartesianMecanumDrive {
      * @param invalidatePreviousHeading whether to invalidate the previous heading of the IMU
      * @param startingDirection         the starting direction of the robot
      */
-    public CartesianFieldCentricMecanumDrive(DcMotor frontLeft, DcMotor frontRight, DcMotor backLeft, DcMotor backRight, IMUOp imu, boolean invalidatePreviousHeading, Direction startingDirection) {
+    public CartesianFieldCentricMecanumDrive(DcMotor frontLeft, DcMotor frontRight, DcMotor backLeft, DcMotor backRight, IMU imu, boolean invalidatePreviousHeading, Direction startingDirection) {
         super(frontLeft, frontRight, backLeft, backRight);
         if (!assertParamsNotNull(imu)) return;
 
         this.imu = imu;
 
         // Invalidate any previous readings
         if (invalidatePreviousHeading)
-            imu.resetHeading();
+            imu.resetYaw();
 
         // Current vector will be the robot's starting vector, must offset the IMU to align straight
-        imu.setOffset(-startingDirection.getDegrees());
-    }
-
-    /**
-     * Set a speed at which the Mecanum drive assembly should move using controller input.
-     * This method will also account for the robot's current heading and adjust the vectors
-     * accordingly. Note that the IMU will be internally ticked by this method, so imu.tick()
-     * does not need to be called.
-     * The difference is that the vectors will be rotated by 90 degrees clockwise to account for
-     * the strange non-Cartesian coordinate system of the gamepad.
-     *
-     * @param left_stick_x  X value of the controller, will be interpreted as relative to the field
-     * @param left_stick_y  Y value of the controller, will be interpreted as relative to the field
-     * @param right_stick_x R value of the controller, will be interpreted as relative to the field
-     * @see Controls#Companion
-     */
-    @Override
-    public CartesianFieldCentricMecanumDrive setSpeedUsingController(double left_stick_x, double left_stick_y, double right_stick_x) {
-        imu.update();
-
-        // Account for the rotated vector of the gamepad
-        double heading = imu.getRawHeading() - 90;
-        double x = -left_stick_x;
-
-        double sin = Math.sin(Math.toRadians(heading));
-        double cos = Math.cos(Math.toRadians(heading));
-
-        // Transform the x and y values to be relative to the field
-        // This is done by calculating the current heading to the field then rotating the x
-        // and y vectors to be relative to the field, then updating the motor powers as normal
-        speedY = x * cos + left_stick_y * sin;
-        speedX = x * sin - left_stick_y * cos;
-        speedR = right_stick_x;
-
-        return this;
+        offsetDeg = startingDirection.getAngle().in(Degrees);
     }
 
     /**
      * Set a speed at which the Mecanum drive assembly should move.
      * This method will also account for the robot's current heading and adjust the vectors
-     * accordingly. Note that the IMU will be internally ticked by this method, so imu.tick()
-     * does not need to be called.
+     * accordingly.
      *
      * @param x The speed at which the robot should move in the x direction relative to the field.
      *          Positive is right, negative is left.
@@ -93,44 +66,12 @@ public CartesianFieldCentricMecanumDrive setSpeedUsingController(double left_sti
      */
     @Override
     public CartesianFieldCentricMecanumDrive setSpeedXYR(double x, double y, double r) {
-        imu.update();
-
-        double heading = imu.getRawHeading();
-        double xn = -x;
-
-        double sin = Math.sin(Math.toRadians(heading));
-        double cos = Math.cos(Math.toRadians(heading));
-
-        speedX = -(xn * cos + y * sin);
-        speedY = -(xn * sin - y * cos);
-        speedR = r;
-
-        return this;
-    }
-
-    /**
-     * Set a polar speed at which the Mecanum drive assembly should move.
-     * This method will also account for the robot's current heading and adjust the vectors
-     * accordingly. Note that the IMU will be internally ticked by this method, so imu.tick()
-     * does not need to be called.
-     *
-     * @param speed             speed at which the motors will operate
-     * @param direction_degrees direction at which the motors will move toward relative to the field
-     * @param rSpeed            rotation speed - positive: clockwise
-     */
-    @Override
-    public CartesianFieldCentricMecanumDrive setSpeedPolarR(double speed, double direction_degrees, double rSpeed) {
-        imu.update();
-
-        double heading = imu.getRawHeading();
-        double direction = Math.toRadians(direction_degrees);
+        double botHeading = Math.toRadians(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) - offsetDeg);
 
-        double sin = Math.sin(Math.toRadians(heading));
-        double cos = Math.cos(Math.toRadians(heading));
+        double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading);
+        double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading);
 
-        speedX = -(speed * Math.cos(direction) * cos + speed * Math.sin(direction) * sin);
-        speedY = -(speed * Math.cos(direction) * sin - speed * Math.sin(direction) * cos);
-        speedR = rSpeed;
+        super.setSpeedXYR(rotX, rotY, r);
 
         return this;
     }

src/main/java/org/murraybridgebunyips/bunyipslib/drive/CartesianMecanumDrive.java

@@ -1,35 +1,41 @@
 package org.murraybridgebunyips.bunyipslib.drive;
 
+import static org.murraybridgebunyips.bunyipslib.external.units.Units.Radians;
+
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.util.Range;
 
 import org.murraybridgebunyips.bunyipslib.BunyipsSubsystem;
 import org.murraybridgebunyips.bunyipslib.Controls;
+import org.murraybridgebunyips.bunyipslib.external.units.Angle;
+import org.murraybridgebunyips.bunyipslib.external.units.Measure;
 
 import java.util.Locale;
 
 /**
- * Base class for a Cartesian drive system that uses Mecanum wheels.
+ * Class for a Cartesian drive system that uses Mecanum wheels.
  * Includes all the common math done across all Mecanum drive systems.
+ * <p>
  * This system is deprecated by RoadRunner, but remains for legacy/non-RoadRunner purposes.
+ * Coordinates used in Cartesian drives include a Cartesian speed mapping, where X is the horizontal axis, and
+ * Y is the forward axis. This is rotated differently within RoadRunner-derived drive bases.
  *
  * @author Lucas Bubner, 2023
  * @see MecanumDrive
  */
 public class CartesianMecanumDrive extends BunyipsSubsystem {
-
     /**
      * Horizontal speed of the robot.
      */
-    public double speedX;
+    private double speedX;
     /**
      * Vertical speed of the robot.
      */
-    public double speedY;
+    private double speedY;
     /**
      * Rotational speed of the robot.
      */
-    public double speedR;
+    private double speedR;
     private DcMotor frontLeft;
 
     // Axial translation speeds
@@ -80,9 +86,7 @@ public CartesianMecanumDrive swapPriority() {
      * @see Controls#Companion
      */
     public CartesianMecanumDrive setSpeedUsingController(double left_stick_x, double left_stick_y, double right_stick_x) {
-        speedX = Range.clip(left_stick_x, -1.0, 1.0);
-        speedY = Range.clip(-left_stick_y, -1.0, 1.0);
-        speedR = Range.clip(right_stick_x, -1.0, 1.0);
+        setSpeedXYR(left_stick_x, -left_stick_y, right_stick_x);
         return this;
     }
 
@@ -110,16 +114,17 @@ public CartesianMecanumDrive setSpeedXYR(double x, double y, double r) {
     /**
      * Set a polar speed at which the Mecanum drive assembly should move.
      *
-     * @param speed             speed at which the motors will operate
-     * @param direction_degrees direction at which the motors will move toward
-     * @param rSpeed            rotation speed - positive: clockwise
+     * @param speed     speed at which the motors will operate
+     * @param direction direction at which the motors will move toward
+     * @param rSpeed    rotation speed - positive: clockwise
      * @return this
      */
-    public CartesianMecanumDrive setSpeedPolarR(double speed, double direction_degrees, double rSpeed) {
-        double radians = Math.toRadians(direction_degrees);
-        speedX = Range.clip(speed * Math.cos(radians), -1.0, 1.0);
-        speedY = Range.clip(speed * Math.sin(radians), -1.0, 1.0);
-        speedR = Range.clip(rSpeed, -1.0, 1.0);
+    public CartesianMecanumDrive setSpeedPolarR(double speed, Measure<Angle> direction, double rSpeed) {
+        setSpeedXYR(
+                speed * Math.cos(direction.in(Radians)),
+                speed * Math.sin(direction.in(Radians)),
+                rSpeed
+        );
         return this;
     }
 

src/main/java/org/murraybridgebunyips/bunyipslib/example/examplerobot/debug/ExampleDebug.java

@@ -25,7 +25,9 @@ protected void onInit() {
 
     @Override
     protected void activeLoop() {
-        telemetry.add("Heading: " + imu.getHeading());
+        telemetry.add("Heading: " + imu.yaw);
         // Alternatively the custom format string method, where type can be omitted: telemetry.add("Heading: %", imu.getHeading());
+
+        imu.update();
     }
 }

src/main/java/org/murraybridgebunyips/bunyipslib/roadrunner/drive/DriveConstants.java

@@ -321,7 +321,7 @@ public Builder setMaxAngAccel(Measure<Velocity<Velocity<Angle>>> maxAngAccel) {
          * Set the admissible error for the PIDVA controller for trajectory following. Usually, you won't have
          * to call this method, as the default error is adequate.
          *
-         * @param admissibleError admissible/satisfactory pose error at the end of each move (default 0.5in 5deg)
+         * @param admissibleError   admissible/satisfactory pose error at the end of each move (default 0.5in 5deg)
          * @param admissibleTimeout max time to wait for the error to be admissible (default 0.5 sec)
          * @return this
          */