Fixed for SDK 8.1.1, hopefully (new IMU). Possibly a breaking change?

technototes · Dec 15, 2022 · 769896b · 769896b
1 parent 7b9f89f
commit 769896b
Show file tree

Hide file tree

Showing 4 changed files with 64 additions and 61 deletions.
diff --git a/RobotLibrary/src/main/java/com/technototes/library/hardware/sensor/IMU.java b/RobotLibrary/src/main/java/com/technototes/library/hardware/sensor/IMU.java
@@ -1,19 +1,16 @@
 package com.technototes.library.hardware.sensor;
 
-import com.qualcomm.hardware.bosch.BNO055IMU;
-import com.qualcomm.hardware.bosch.BNO055IMUImpl;
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
 import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
 import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
 
 /**
- * Class for the BNO055 (Inertial Movement Units) that implements the IGyro interface
+ * Class for the IMU (Inertial Movement Units) that implements the IGyro interface
  */
 @SuppressWarnings("unused")
-public class IMU extends Sensor<BNO055IMUImpl> implements IGyro {
-
-    private double angleOffset = 0;
+public class IMU extends Sensor<com.qualcomm.robotcore.hardware.IMU> implements IGyro {
 
     /**
      * The direction of the axes signs when remapping the axes
@@ -67,16 +64,27 @@ public enum AxesSigns {
         }
     }
 
-    private BNO055IMU.Parameters parameters;
+    private com.qualcomm.robotcore.hardware.IMU.Parameters parameters;
+    // This is the preferred units (Degrees/radians)
+    private AngleUnit preferred;
+    // This is an offset from 0 so we can zero the IMU without needing to be facing forward
+    private double angleOffset;
+    private AxesOrder axesOrder;
+    private AxesSigns axesSigns;
 
     /**
      * Make an imu
      *
      * @param device The imu device
      */
-    public IMU(BNO055IMUImpl device) {
+    public IMU(com.qualcomm.robotcore.hardware.IMU device) {
         super(device);
-        parameters = new BNO055IMU.Parameters();
+        angleOffset = 0.0;
+
+        // TODO: Validate this on hardware!
+        axesOrder = AxesOrder.ZXY;
+        axesSigns = AxesSigns.PPP;
+
         degrees();
         initialize();
     }
@@ -88,7 +96,11 @@ public IMU(BNO055IMUImpl device) {
      */
     public IMU(String deviceName) {
         super(deviceName);
-        parameters = new BNO055IMU.Parameters();
+        angleOffset = 0.0;
+        // TODO: Validate this on hardware!
+        axesOrder = AxesOrder.ZXY;
+        axesSigns = AxesSigns.PPP;
+
         degrees();
         initialize();
     }
@@ -99,7 +111,9 @@ public IMU(String deviceName) {
      * @return this
      */
     public IMU degrees() {
-        parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
+        // Convert the angle offset!
+        angleOffset = AngleUnit.DEGREES.fromUnit(preferred, angleOffset);
+        preferred = AngleUnit.DEGREES;
         return this;
     }
 
@@ -109,7 +123,9 @@ public IMU degrees() {
      * @return this (for chaining)
      */
     public IMU radians() {
-        parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
+        // Convert the angle offset!
+        angleOffset = AngleUnit.RADIANS.fromUnit(preferred, angleOffset);
+        preferred = AngleUnit.RADIANS;
         return this;
     }
 
@@ -119,28 +135,35 @@ public IMU radians() {
      * @return the IMU (for chaining)
      */
     public IMU initialize() {
-        getDevice().initialize(parameters);
+        // TODO: fix this?
+        // getDevice().initialize(parameters);
         return this;
     }
 
+    // Gets the Yaw (orientation along the z-axis) with no adjustment
+    private double getRawYaw(AngleUnit unit) {
+        // TODO: Deal with the AxesSign thing properly. Cuz it does nothing here, AFAICT
+        return getDevice().getRobotYawPitchRollAngles().getYaw(unit);
+    }
+
     /**
      * Get gyro heading
      *
-     * @return The gyro heading
+     * @return The gyro heading (in preferred units, from -180/pi to +180/pi
      */
     @Override
     public double gyroHeading() {
-        return getAngularOrientation().firstAngle;
+        return preferred.normalize(getRawYaw(preferred) - angleOffset);
     }
 
     /**
      * Get the gyro heading in the provided units
      *
      * @param unit the unit desired
-     * @return The heading in 'unit' units
+     * @return The heading in 'unit' units from -180/pi to +180/pi
      */
     public double gyroHeading(AngleUnit unit) {
-        return unit.fromUnit(device.getAngularOrientation().angleUnit, gyroHeading() - angleOffset);
+        return unit.normalize(getRawYaw(unit) - unit.fromUnit(preferred, angleOffset));
     }
 
     /**
@@ -170,7 +193,7 @@ public double gyroHeadingInRadians() {
      */
     @Override
     public void setHeading(double newHeading) {
-        angleOffset = gyroHeading() - newHeading;
+        angleOffset = getRawYaw(preferred) - newHeading;
     }
 
     /**
@@ -181,38 +204,8 @@ public void setHeading(double newHeading) {
      * @return this (for chaining)
      */
     public IMU remapAxes(AxesOrder order, AxesSigns signs) {
-        try {
-            // the indices correspond with the 2-bit encodings specified in the datasheet
-            int[] indices = order.indices();
-            int axisMapConfig = 0;
-            axisMapConfig |= (indices[0] << 4);
-            axisMapConfig |= (indices[1] << 2);
-            axisMapConfig |= (indices[2]);
-
-            // the BNO055 driver flips the first orientation vector so we also flip here
-            int axisMapSign = signs.bVal ^ (0b100 >> indices[0]);
-
-            // Enter CONFIG mode
-            getDevice()
-                .write8(BNO055IMU.Register.OPR_MODE, BNO055IMU.SensorMode.CONFIG.bVal & 0x0F);
-
-            Thread.sleep(100);
-
-            // Write the AXIS_MAP_CONFIG register
-            getDevice().write8(BNO055IMU.Register.AXIS_MAP_CONFIG, axisMapConfig & 0x3F);
-
-            // Write the AXIS_MAP_SIGN register
-            getDevice().write8(BNO055IMU.Register.AXIS_MAP_SIGN, axisMapSign & 0x07);
-
-            // Switch back to the previous mode
-            getDevice()
-                .write8(BNO055IMU.Register.OPR_MODE, getDevice().getParameters().mode.bVal & 0x0F);
-
-            Thread.sleep(100);
-        } catch (InterruptedException e) {
-            Thread.currentThread().interrupt();
-        }
-        parameters = getDevice().getParameters();
+        axesSigns = signs;
+        axesOrder = order;
         return this;
     }
 
@@ -222,7 +215,7 @@ public IMU remapAxes(AxesOrder order, AxesSigns signs) {
      * @return The Angular Velocity
      */
     public AngularVelocity getAngularVelocity() {
-        return device.getAngularVelocity();
+        return device.getRobotAngularVelocity(preferred);
     }
 
     /**
@@ -231,6 +224,16 @@ public AngularVelocity getAngularVelocity() {
      * @return the Orientation of the IMU
      */
     public Orientation getAngularOrientation() {
-        return device.getAngularOrientation();
+        Orientation res = getDevice().getRobotOrientation(AxesReference.INTRINSIC, axesOrder, preferred);
+        if ((axesSigns.bVal & AxesSigns.NPP.bVal) == AxesSigns.NPP.bVal) {
+            res.firstAngle = -res.firstAngle;
+        }
+        if ((axesSigns.bVal & AxesSigns.PNP.bVal) == AxesSigns.PNP.bVal) {
+            res.secondAngle = -res.secondAngle;
+        }
+        if ((axesSigns.bVal & AxesSigns.PPN.bVal) == AxesSigns.PPN.bVal) {
+            res.thirdAngle = -res.thirdAngle;
+        }
+        return res;
     }
 }
diff --git a/RobotLibrary/src/main/java/com/technototes/library/hardware/sensor/Rev2MDistanceSensor.java b/RobotLibrary/src/main/java/com/technototes/library/hardware/sensor/Rev2MDistanceSensor.java
@@ -4,7 +4,8 @@
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 
 /**
- * Class for range sensors
+ * Class for the Rev '2m' range sensors
+ * Note for users: The Rev 2m range sensor is actually only useful at about 1.1 - 1.2m :/
  *
  * @author Alex Stedman
  */

diff --git a/RobotLibrary/src/test/java/com/technototes/library/opmode/OpModeTest.java b/RobotLibrary/src/test/java/com/technototes/library/opmode/OpModeTest.java
@@ -2,23 +2,22 @@
 
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.hardware.Gamepad;
-import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.technototes.library.structure.CommandOpMode;
 
 public class OpModeTest {
 
     private OpMode opMode;
-    private HardwareMap hardwareMap;
+    // SDK 8.1.1 doesn't let us make a hardware map with a null appContext
+    // private HardwareMap hardwareMap;
     private Gamepad g1, g2;
 
     public void setup() {
         opMode = new TestOpMode();
-        hardwareMap = new HardwareMap(null);
         g1 = new Gamepad();
         g2 = new Gamepad();
         opMode.gamepad1 = g1;
         opMode.gamepad2 = g2;
-        opMode.hardwareMap = hardwareMap;
+        opMode.hardwareMap = null;
     }
 
     public void run() {}

diff --git a/build.dependencies.gradle b/build.dependencies.gradle
@@ -6,10 +6,10 @@ repositories {
 }
 
 dependencies {
-    implementation 'org.firstinspires.ftc:RobotCore:8.0.0'
-    implementation 'org.firstinspires.ftc:RobotServer:8.0.0'
-    implementation 'org.firstinspires.ftc:Hardware:8.0.0'
-    implementation 'org.firstinspires.ftc:FtcCommon:8.0.0'
+    implementation 'org.firstinspires.ftc:RobotCore:8.1.1'
+    implementation 'org.firstinspires.ftc:RobotServer:8.1.1'
+    implementation 'org.firstinspires.ftc:Hardware:8.1.1'
+    implementation 'org.firstinspires.ftc:FtcCommon:8.1.1'
     implementation 'androidx.appcompat:appcompat:1.3.1'
 }