Migrate to IMU interface

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java

@@ -27,14 +27,14 @@ import com.acmerobotics.roadrunner.Twist2dDual;
 import com.acmerobotics.roadrunner.Twist2dIncrDual;
 import com.acmerobotics.roadrunner.Vector2d;
 import com.acmerobotics.roadrunner.VelConstraint;
-import com.qualcomm.hardware.bosch.BNO055IMU;
 import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.IMU;
 import com.qualcomm.robotcore.hardware.VoltageSensor;
 
-import org.firstinspires.ftc.teamcode.util.BNO055Wrapper;
 import org.firstinspires.ftc.teamcode.util.Encoder;
 import org.firstinspires.ftc.teamcode.util.Localizer;
 import org.firstinspires.ftc.teamcode.util.LynxFirmwareVersion;
@@ -95,7 +95,7 @@ public final class MecanumDrive {
 
     public final VoltageSensor voltageSensor;
 
-    public final BNO055Wrapper imu;
+    public final IMU imu;
 
     public final Localizer localizer;
     public Pose2d pose;
@@ -176,11 +176,11 @@ public final class MecanumDrive {
         rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
-        BNO055IMU baseImu = hardwareMap.get(BNO055IMU.class, "imu");
+        imu = hardwareMap.get(IMU.class, "imu");
-        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+        IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
-        parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
+                RevHubOrientationOnRobot.LogoFacingDirection.UP,
-        baseImu.initialize(parameters);
+                RevHubOrientationOnRobot.UsbFacingDirection.FORWARD));
-        imu = new BNO055Wrapper(baseImu);
+        imu.initialize(parameters);
 
         voltageSensor = hardwareMap.voltageSensor.iterator().next();
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java

@@ -29,14 +29,14 @@ import com.acmerobotics.roadrunner.Twist2dIncrDual;
 import com.acmerobotics.roadrunner.Vector2d;
 import com.acmerobotics.roadrunner.Vector2dDual;
 import com.acmerobotics.roadrunner.VelConstraint;
-import com.qualcomm.hardware.bosch.BNO055IMU;
 import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.IMU;
 import com.qualcomm.robotcore.hardware.VoltageSensor;
 
-import org.firstinspires.ftc.teamcode.util.BNO055Wrapper;
 import org.firstinspires.ftc.teamcode.util.Encoder;
 import org.firstinspires.ftc.teamcode.util.Localizer;
 import org.firstinspires.ftc.teamcode.util.LynxFirmwareVersion;
@@ -93,7 +93,7 @@ public final class TankDrive {
 
     public final List<DcMotorEx> leftMotors, rightMotors;
 
-    public final BNO055Wrapper imu;
+    public final IMU imu;
 
     public final VoltageSensor voltageSensor;
 
@@ -191,11 +191,11 @@ public final class TankDrive {
             m.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         }
 
-        BNO055IMU baseImu = hardwareMap.get(BNO055IMU.class, "imu");
+        imu = hardwareMap.get(IMU.class, "imu");
-        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+        IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
-        parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
+                RevHubOrientationOnRobot.LogoFacingDirection.UP,
-        baseImu.initialize(parameters);
+                RevHubOrientationOnRobot.UsbFacingDirection.FORWARD));
-        imu = new BNO055Wrapper(baseImu);
+        imu.initialize(parameters);
 
         voltageSensor = hardwareMap.voltageSensor.iterator().next();
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java

@@ -8,8 +8,9 @@ import com.acmerobotics.roadrunner.Twist2dIncrDual;
 import com.acmerobotics.roadrunner.Vector2dDual;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.IMU;
 
-import org.firstinspires.ftc.teamcode.util.BNO055Wrapper;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.teamcode.util.Encoder;
 import org.firstinspires.ftc.teamcode.util.Localizer;
 import org.firstinspires.ftc.teamcode.util.RawEncoder;
@@ -20,21 +21,21 @@ public final class TwoDeadWheelLocalizer implements Localizer {
     public static double PERP_X_TICKS = 0.0;
 
     public final Encoder par, perp;
-    public final BNO055Wrapper imu;
+    public final IMU imu;
 
     private int lastParPos, lastPerpPos;
     private Rotation2d lastHeading;
 
     private final double inPerTick;
 
-    public TwoDeadWheelLocalizer(HardwareMap hardwareMap, BNO055Wrapper imu, double inPerTick) {
+    public TwoDeadWheelLocalizer(HardwareMap hardwareMap, IMU imu, double inPerTick) {
         par = new RawEncoder(hardwareMap.get(DcMotorEx.class, "par"));
         perp = new RawEncoder(hardwareMap.get(DcMotorEx.class, "perp"));
         this.imu = imu;
 
         lastParPos = par.getPositionAndVelocity().position;
         lastPerpPos = perp.getPositionAndVelocity().position;
-        lastHeading = imu.getHeading();
+        lastHeading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
 
         this.inPerTick = inPerTick;
     }
@@ -42,13 +43,13 @@ public final class TwoDeadWheelLocalizer implements Localizer {
     public Twist2dIncrDual<Time> updateAndGetIncr() {
         Encoder.PositionVelocityPair parPosVel = par.getPositionAndVelocity();
         Encoder.PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
-        Rotation2d heading = imu.getHeading();
+        Rotation2d heading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
 
         int parPosDelta = parPosVel.position - lastParPos;
         int perpPosDelta = perpPosVel.position - lastPerpPos;
         double headingDelta = heading.minus(lastHeading);
 
-        double headingVel = imu.getHeadingVelocity();
+        double headingVel = imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate;
 
         Twist2dIncrDual<Time> twistIncr = new Twist2dIncrDual<>(
                 new Vector2dDual<>(

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/AngularRampLogger.java

@@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode.tuning;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
 import org.firstinspires.ftc.teamcode.util.Encoder;
 import org.firstinspires.ftc.teamcode.util.MidpointTimer;
@@ -119,7 +120,7 @@ public final class AngularRampLogger extends LinearOpMode {
             }
             data.encTimes.add(t.addSplit());
 
-            AngularVelocity av = view.imu.getAngularVelocity();
+            AngularVelocity av = view.imu.getRobotAngularVelocity(AngleUnit.RADIANS);
             data.angVels.get(0).add((double) av.xRotationRate);
             data.angVels.get(1).add((double) av.yRotationRate);
             data.angVels.get(2).add((double) av.zRotationRate);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveView.java

@@ -6,13 +6,13 @@ import com.acmerobotics.roadrunner.Twist2d;
 import com.qualcomm.robotcore.hardware.DcMotorController;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.IMU;
 import com.qualcomm.robotcore.hardware.VoltageSensor;
 
 import org.firstinspires.ftc.teamcode.MecanumDrive;
 import org.firstinspires.ftc.teamcode.TankDrive;
 import org.firstinspires.ftc.teamcode.ThreeDeadWheelLocalizer;
 import org.firstinspires.ftc.teamcode.TwoDeadWheelLocalizer;
-import org.firstinspires.ftc.teamcode.util.BNO055Wrapper;
 import org.firstinspires.ftc.teamcode.util.Encoder;
 import org.firstinspires.ftc.teamcode.util.Localizer;
 import org.firstinspires.ftc.teamcode.util.OverflowEncoder;
@@ -37,7 +37,7 @@ final class DriveView {
     public final List<RawEncoder> leftEncs, rightEncs, parEncs, perpEncs;
     public final List<RawEncoder> forwardEncs;
 
-    public final BNO055Wrapper imu;
+    public final IMU imu;
 
     public final VoltageSensor voltageSensor;
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/BNO055Wrapper.java

@@ -1,148 +0,0 @@
-package org.firstinspires.ftc.teamcode.util;
-
-import com.acmerobotics.roadrunner.Rotation2d;
-import com.qualcomm.hardware.bosch.BNO055IMU;
-
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
-import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
-
-public final class BNO055Wrapper {
-    private final BNO055IMU bno;
-
-    public BNO055Wrapper(BNO055IMU bno) {
-        this.bno = bno;
-    }
-
-    public Rotation2d getHeading() {
-        return Rotation2d.exp(bno.getAngularOrientation()
-                .toAngleUnit(AngleUnit.RADIANS)
-                .toAxesOrder(AxesOrder.ZYX)
-                .firstAngle);
-    }
-
-    public double getHeadingVelocity() {
-        return getAngularVelocity().zRotationRate;
-    }
-
-    public AngularVelocity getAngularVelocity() {
-        return bno.getAngularVelocity().toAngleUnit(AngleUnit.RADIANS);
-    }
-
-    /**
-     * A direction for an axis to be remapped to
-     */
-    public enum AxisDirection {
-        POS_X, NEG_X, POS_Y, NEG_Y, POS_Z, NEG_Z
-    }
-
-    /**
-     * IMU axes signs in the order XYZ (after remapping).
-     */
-    public enum AxesSigns {
-        PPP(0b000),
-        PPN(0b001),
-        PNP(0b010),
-        PNN(0b011),
-        NPP(0b100),
-        NPN(0b101),
-        NNP(0b110),
-        NNN(0b111);
-
-        public final int bVal;
-
-        AxesSigns(int bVal) {
-            this.bVal = bVal;
-        }
-    }
-
-    /**
-     * Remap BNO055 IMU axes and signs. For reference, the default order is {@link AxesOrder#XYZ}.
-     * Call after {@link BNO055IMU#initialize(BNO055IMU.Parameters)}. Although this isn't
-     * mentioned in the datasheet, the axes order appears to affect the onboard sensor fusion.
-     *
-     * Adapted from <a href="https://ftcforum.firstinspires.org/forum/ftc-technology/53812-mounting-the-revhub-vertically?p=56587#post56587">this post</a>.
-     * Reference the <a href="https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf">BNO055 Datasheet</a> for details.
-     *
-     * NOTE: Remapping axes can be somewhat confusing. Instead, use {@link #remapZAxis}, if
-     * appropriate.
-     *
-     * @param order axes order
-     * @param signs axes signs
-     */
-    public void swapThenFlipAxes(AxesOrder order, AxesSigns signs) {
-        try {
-            // the indices correspond with the 2-bit axis encodings specified in the datasheet
-            int[] indices = order.indices();
-            // AxesSign's values align with the datasheet
-            int axisMapSigns = signs.bVal;
-
-            if (indices[0] == indices[1] || indices[0] == indices[2] || indices[1] == indices[2]) {
-                throw new RuntimeException("Same axis cannot be included in AxesOrder twice");
-            }
-
-            // ensure right-handed coordinate system
-            boolean isXSwapped = indices[0] != 0;
-            boolean isYSwapped = indices[1] != 1;
-            boolean isZSwapped = indices[2] != 2;
-            boolean areTwoAxesSwapped = (isXSwapped || isYSwapped || isZSwapped)
-                    && (!isXSwapped || !isYSwapped || !isZSwapped);
-            boolean oddNumOfFlips = (((axisMapSigns >> 2) ^ (axisMapSigns >> 1) ^ axisMapSigns) & 1) == 1;
-            // != functions as xor
-            if (areTwoAxesSwapped != oddNumOfFlips) {
-                throw new RuntimeException("Coordinate system is left-handed");
-            }
-
-            // Bit:  7  6 |  5  4  |  3  2  |  1  0  |
-            //   reserved | z axis | y axis | x axis |
-            int axisMapConfig = indices[2] << 4 | indices[1] << 2 | indices[0];
-
-            // Enter CONFIG mode
-            bno.write8(BNO055IMU.Register.OPR_MODE, BNO055IMU.SensorMode.CONFIG.bVal & 0x0F);
-
-            Thread.sleep(100);
-
-            // Write the AXIS_MAP_CONFIG register
-            bno.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, axisMapConfig & 0x3F);
-
-            // Write the AXIS_MAP_SIGN register
-            bno.write8(BNO055IMU.Register.AXIS_MAP_SIGN, axisMapSigns & 0x07);
-
-            // Switch back to the previous mode
-            bno.write8(BNO055IMU.Register.OPR_MODE, bno.getParameters().mode.bVal & 0x0F);
-
-            Thread.sleep(100);
-        } catch (InterruptedException e) {
-            Thread.currentThread().interrupt();
-        }
-    }
-
-    /**
-     * Remaps the IMU coordinate system so that the remapped +Z faces the provided
-     * {@link AxisDirection}. See {@link #swapThenFlipAxes} for details about the remapping.
-     *
-     * @param direction axis direction
-     */
-    public void remapZAxis(AxisDirection direction) {
-        switch (direction) {
-            case POS_X:
-                swapThenFlipAxes(AxesOrder.ZYX, AxesSigns.NPP);
-                break;
-            case NEG_X:
-                swapThenFlipAxes(AxesOrder.ZYX, AxesSigns.PPN);
-                break;
-            case POS_Y:
-                swapThenFlipAxes(AxesOrder.XZY, AxesSigns.PNP);
-                break;
-            case NEG_Y:
-                swapThenFlipAxes(AxesOrder.XZY, AxesSigns.PPN);
-                break;
-            case POS_Z:
-                swapThenFlipAxes(AxesOrder.XYZ, AxesSigns.PPP);
-                break;
-            case NEG_Z:
-                swapThenFlipAxes(AxesOrder.XYZ, AxesSigns.PNN);
-                break;
-        }
-    }
-}