From 626bfd6de8fef351be808c5a95ccce14102065f0 Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Wed, 2 Oct 2024 13:49:11 -0400
Subject: [PATCH 01/27] refactor(hardware/gyro): separate IMUs and single axis
 gyros

---
 .../hardware/gyro/SingleAxisGyroscope.java    | 45 ----------
 .../librobot/hardware/gyro/imu/IMU.java       | 82 +++++++++++++++++++
 .../gyro/single/SingleAxisGyroscope.java      | 10 +++
 3 files changed, 92 insertions(+), 45 deletions(-)
 delete mode 100644 src/main/java/net/frc5183/librobot/hardware/gyro/SingleAxisGyroscope.java
 create mode 100644 src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
 create mode 100644 src/main/java/net/frc5183/librobot/hardware/gyro/single/SingleAxisGyroscope.java

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/SingleAxisGyroscope.java b/src/main/java/net/frc5183/librobot/hardware/gyro/SingleAxisGyroscope.java
deleted file mode 100644
index 35fd712..0000000
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/SingleAxisGyroscope.java
+++ /dev/null
@@ -1,45 +0,0 @@
-package net.frc5183.librobot.hardware.gyro;
-
-public abstract class SingleAxisGyroscope {
-    /**
-     * @return the angle in degrees
-     */
-    public abstract double getAngle();
-
-    /**
-     * @return the Rotation2d of the gyro.
-     */
-    public abstract Rotation2d getRotation2d();
-
-    /**
-     * Calibrates the gyroscope
-     */
-    public abstract void calibrate();
-
-    /**
-     * Resets the gyroscope
-     */
-    public abstract void reset();
-
-    /**
-     * Sets the offset of the gyroscope
-     */
-    public abstract void setOffset(double offset);
-
-    /**
-     * @return the offset of the gyroscope
-     */
-    public abstract double getOffset();
-
-    /**
-     * @return the axis of the gyroscope
-     */
-    public abstract Axis getAxis();
-
-    /**
-     * The axis of the gyroscope
-     */
-    public enum Axis {
-        YAW, PITCH, ROLL
-    }
-}
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
new file mode 100644
index 0000000..b61770c
--- /dev/null
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
@@ -0,0 +1,82 @@
+package net.frc5183.librobot.hardware.gyro.imu;
+
+import edu.wpi.first.math.geometry.Rotation3d;
+import org.jetbrains.annotations.NotNull;
+
+/**
+ * Represents an IMU (Inertial Measurement Unit).
+ */
+public abstract class IMU {
+    /**
+     * The offset to be added to the gyroscope.
+     */
+    private Rotation3d offset = new Rotation3d(0, 0, 0);
+
+    /**
+     * Gets the rotation of the gyroscope (with offset) in degrees.
+     * @return {@link Rotation3d} of the rotation of the gyroscope (with offset) in degrees.
+     */
+    @NotNull
+    public Rotation3d getRotation3dDegrees() {
+        return getRawRotation3dDegrees().plus(getOffset());
+    }
+
+    /**
+     * Gets the rotation of the gyroscope (without offset) in degrees.
+     * @return {@link Rotation3d} of the rotation of the gyroscope (without offset) in degrees.
+     */
+    @NotNull
+    public abstract Rotation3d getRawRotation3dDegrees();
+
+    /**
+     * Gets the acceleration from the IMU in meters per second squared.
+     * @return {@link Rotation3d} of the acceleration from the IMU in meters per second squared.
+     */
+    @NotNull
+    public abstract Rotation3d getAccelerationMetersPerSecondSquared();
+
+    /**
+     * Sets the offset of the gyroscope.
+     * @param offset the offset of the gyroscope.
+     */
+    public void setOffset(@NotNull Rotation3d offset) {
+        this.offset = offset;
+    }
+
+    /**
+     * Gets the offset of the gyroscope.
+     * @return {@link Rotation3d} the offset of the gyroscope.
+     */
+    @NotNull
+    public Rotation3d getOffset() {
+        return offset;
+    }
+
+    /**
+     * Calibrates the gyroscope.
+     */
+    public abstract void calibrate();
+
+    /**
+     * Resets the gyroscope.
+     */
+    public abstract void reset();
+
+    /**
+     * Returns the raw gyroscope object.
+     * Should only be used when you know the subclass of the gyroscope.
+     * <br><br>
+     * Implementations should override the return type to the type of the gyroscope used in the vendor's API.
+     * (e.g the {@link ADIS16448IMU} would return {@link edu.wpi.first.wpilibj.ADIS16448_IMU})
+     * @return the raw gyroscope object.
+     */
+    @NotNull
+    public abstract Object getRawIMU();
+
+    /**
+     * Represents the three axes of a gyroscope.
+     */
+    public enum IMUAxis {
+        X, Y, Z
+    }
+}
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/single/SingleAxisGyroscope.java b/src/main/java/net/frc5183/librobot/hardware/gyro/single/SingleAxisGyroscope.java
new file mode 100644
index 0000000..763034b
--- /dev/null
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/single/SingleAxisGyroscope.java
@@ -0,0 +1,10 @@
+package net.frc5183.librobot.hardware.gyro.single;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public abstract class SingleAxisGyroscope {
+    // todo
+    public enum Axis {
+        YAW, PITCH, ROLL
+    }
+}

From a699ec701ff6db776f08877e7ec1d4d58885ec70 Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Wed, 2 Oct 2024 13:52:16 -0400
Subject: [PATCH 02/27] feat(hardware/gyro/imu): implement ADIS16448IMU and
 ADIS16470IMU

---
 .../hardware/gyro/imu/ADIS16448IMU.java       | 122 ++++++++++++++++++
 .../hardware/gyro/imu/ADIS16470IMU.java       | 111 ++++++++++++++++
 2 files changed, 233 insertions(+)
 create mode 100644 src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
 create mode 100644 src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
new file mode 100644
index 0000000..b3bffcd
--- /dev/null
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
@@ -0,0 +1,122 @@
+package net.frc5183.librobot.hardware.gyro.imu;
+
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.wpilibj.ADIS16448_IMU;
+import edu.wpi.first.wpilibj.SPI;
+import org.jetbrains.annotations.NotNull;
+
+import java.util.Optional;
+
+/**
+ * Represents an ADIS16448 {@link IMU}.
+ */
+public class ADIS16448IMU extends IMU {
+    /**
+     * The ADIS16448 IMU.
+     */
+    @NotNull
+    private final ADIS16448_IMU imu;
+
+    /**
+     * The YAW axis.
+     */
+    private final IMUAxis yaw;
+
+    /**
+     * The PITCH axis.
+     */
+    private final IMUAxis pitch;
+
+    /**
+     * The ROLL axis.
+     */
+    private final IMUAxis roll;
+
+    /**
+     * Creates a new {@link ADIS16448IMU} using the RIO's Onboard MXP port, 500ms calibration time, and Z axis as yaw.
+     * @see ADIS16448_IMU#ADIS16448_IMU()
+     */
+    public ADIS16448IMU() {
+        yaw = IMUAxis.Z;
+        imu = new ADIS16448_IMU();
+    }
+
+    /**
+     * Creates a new {@link ADIS16448IMU} from an existing ADIS16448 IMU instance.
+     * @param imu the ADIS16448 IMU to use.
+     */
+    public ADIS16448IMU(@NotNull ADIS16448_IMU imu) {
+        this.imu = imu;
+    }
+
+    /**
+     * Creates a new {@link ADIS16448IMU} with a specified yaw axis.
+     * Uses default RIO's Onboard MXP port and 512ms calibration time.
+     * @param yaw the YAW axis.
+     */
+    public ADIS16448IMU(IMUAxis yaw) {
+        this.yaw = yaw;
+        imu = new ADIS16448_IMU(fromIMUAxis(yaw), SPI.Port.kMXP, ADIS16448_IMU.CalibrationTime._512ms);
+    }
+
+    /**
+     * Creates a new {@link ADIS16448IMU} with a specified yaw axis and SPI port.
+     * Uses default 512ms calibration time.
+     * @param yaw the YAW axis.
+     * @param port the SPI port.
+     */
+    public ADIS16448IMU(IMUAxis yaw, SPI.Port port) {
+        this.yaw = yaw;
+        imu = new ADIS16448_IMU(fromIMUAxis(yaw), port, ADIS16448_IMU.CalibrationTime._512ms);
+    }
+
+    /**
+     * Creates a new {@link ADIS16448IMU} with a specified yaw axis, SPI port, and calibration time.
+     * @param yaw the YAW axis.
+     * @param port the SPI port.
+     * @param calibrationTime the calibration time.
+     */
+    public ADIS16448IMU(IMUAxis yaw, SPI.Port port, ADIS16448_IMU.CalibrationTime calibrationTime) {
+        this.yaw = yaw;
+        imu = new ADIS16448_IMU(fromIMUAxis(yaw), port, calibrationTime);
+    }
+
+    @Override
+    public @NotNull Rotation3d getRawRotation3dDegrees() {
+        return new Rotation3d(imu.getGyroAngleX(), imu.getGyroAngleY(), imu.getGyroAngleZ());
+    }
+
+    @Override
+    public @NotNull Rotation3d getAccelerationMetersPerSecondSquared() {
+        return new Rotation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ());
+    }
+
+    @Override
+    public void calibrate() {
+        imu.calibrate();
+    }
+
+    @Override
+    public void reset() {
+        imu.reset();
+    }
+
+    @Override
+    public @NotNull ADIS16448_IMU getRawIMU() {
+        return imu;
+    }
+
+    /**
+     * Returns a new {@link ADIS16448_IMU.IMUAxis} from an {@link IMUAxis}.
+     * @param axis the {@link IMUAxis} to convert
+     * @return the converted {@link ADIS16448_IMU.IMUAxis}
+     */
+    @NotNull
+    public static ADIS16448_IMU.IMUAxis fromIMUAxis(@NotNull IMUAxis axis) {
+        return switch (axis) {
+            case X -> ADIS16448_IMU.IMUAxis.kX;
+            case Y -> ADIS16448_IMU.IMUAxis.kY;
+            case Z -> ADIS16448_IMU.IMUAxis.kZ;
+        };
+    }
+}
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
new file mode 100644
index 0000000..d60cec9
--- /dev/null
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
@@ -0,0 +1,111 @@
+package net.frc5183.librobot.hardware.gyro.imu;
+
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.wpilibj.ADIS16448_IMU;
+import edu.wpi.first.wpilibj.ADIS16470_IMU;
+import edu.wpi.first.wpilibj.SPI;
+import org.jetbrains.annotations.NotNull;
+
+/**
+ * Represents an ADIS16470 {@link IMU}.
+ */
+public class ADIS16470IMU extends IMU {
+    /**
+     * The ADIS16470 IMU.
+     */
+    @NotNull
+    private final ADIS16470_IMU imu;
+
+    /**
+     * Creates a new {@link ADIS16470IMU} using the RIO's Onboard MXP port and 500ms calibration time, and yaw pitch roll as ZXY respectively.
+     * @see ADIS16470_IMU#ADIS16470_IMU()
+     */
+    public ADIS16470IMU() {
+        imu = new ADIS16470_IMU();
+    }
+
+    /**
+     * Creates a new {@link ADIS16470IMU} from an existing ADIS16470 IMU instance.
+     * @param imu the ADIS16470 IMU to use.
+     */
+    public ADIS16470IMU(@NotNull ADIS16470_IMU imu) {
+        this.imu = imu;
+    }
+
+    /**
+     * Creates a new {@link ADIS16470IMU} with a specified yaw, pitch, and roll axis.
+     * Uses default RIO's Onboard MXP port and 500ms calibration time.
+     * @param yaw the YAW axis.
+     * @param pitch the PITCH axis.
+     * @param roll the ROLL axis.
+     */
+    public ADIS16470IMU(IMUAxis yaw, IMUAxis pitch, IMUAxis roll) {
+        imu = new ADIS16470_IMU(fromIMUAxis(yaw), fromIMUAxis(pitch), fromIMUAxis(roll));
+    }
+
+    /**
+     * Creates a new {@link ADIS16470IMU} with a specified yaw, pitch, roll axis, and SPI port.
+     * Uses default 4s calibration time.
+     * @param yaw the YAW axis.
+     * @param pitch the PITCH axis.
+     * @param roll the ROLL axis.
+     * @param port the SPI port.
+     */
+    public ADIS16470IMU(IMUAxis yaw, IMUAxis pitch, IMUAxis roll, SPI.Port port) {
+        imu = new ADIS16470_IMU(fromIMUAxis(yaw), fromIMUAxis(pitch), fromIMUAxis(roll), port, ADIS16470_IMU.CalibrationTime._4s);
+    }
+
+    /**
+     * Creates a new {@link ADIS16470IMU} with a specified yaw, pitch, roll axis, SPI port, and calibration time.
+     * @param yaw the YAW axis.
+     * @param pitch the PITCH axis.
+     * @param roll the ROLL axis.
+     * @param port the SPI port.
+     * @param calibrationTime the calibration time.
+     */
+    public ADIS16470IMU(IMUAxis yaw, IMUAxis pitch, IMUAxis roll, SPI.Port port, ADIS16470_IMU.CalibrationTime calibrationTime) {
+        imu = new ADIS16470_IMU(fromIMUAxis(yaw), fromIMUAxis(pitch), fromIMUAxis(roll), port, calibrationTime);
+    }
+
+    @Override
+    public @NotNull Rotation3d getRawRotation3dDegrees() {
+        return new Rotation3d(
+                imu.getAngle(fromIMUAxis(IMUAxis.X)),
+                imu.getAngle(fromIMUAxis(IMUAxis.Y)),
+                imu.getAngle(fromIMUAxis(IMUAxis.Z))
+        );
+    }
+
+    @Override
+    public @NotNull Rotation3d getAccelerationMetersPerSecondSquared() {
+        return new Rotation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ());
+    }
+
+    @Override
+    public void calibrate() {
+        imu.calibrate();
+    }
+
+    @Override
+    public void reset() {
+        imu.reset();
+    }
+
+    @Override
+    public @NotNull ADIS16470_IMU getRawIMU() {
+        return imu;
+    }
+
+    /**
+     * Returns a new {@link ADIS16470_IMU.IMUAxis} from an {@link IMUAxis}.
+     * @param axis the {@link IMUAxis} to convert
+     * @return the converted {@link ADIS16470_IMU.IMUAxis}
+     */
+    public static ADIS16470_IMU.IMUAxis fromIMUAxis(IMUAxis axis) {
+        return switch (axis) {
+            case X -> ADIS16470_IMU.IMUAxis.kX;
+            case Y -> ADIS16470_IMU.IMUAxis.kY;
+            case Z -> ADIS16470_IMU.IMUAxis.kZ;
+        };
+    }
+}

From f2445dec9a85cf24d2cd69d3bffdeaf315ac862c Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Thu, 3 Oct 2024 08:12:28 -0400
Subject: [PATCH 03/27] refactor(hardware/gyro/imu): remove NotNull annotations
 from abstract methods

---
 .../java/net/frc5183/librobot/hardware/gyro/imu/IMU.java  | 8 +-------
 1 file changed, 1 insertion(+), 7 deletions(-)

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
index b61770c..9f394a6 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
@@ -1,7 +1,6 @@
 package net.frc5183.librobot.hardware.gyro.imu;
 
 import edu.wpi.first.math.geometry.Rotation3d;
-import org.jetbrains.annotations.NotNull;
 
 /**
  * Represents an IMU (Inertial Measurement Unit).
@@ -16,7 +15,6 @@ public abstract class IMU {
      * Gets the rotation of the gyroscope (with offset) in degrees.
      * @return {@link Rotation3d} of the rotation of the gyroscope (with offset) in degrees.
      */
-    @NotNull
     public Rotation3d getRotation3dDegrees() {
         return getRawRotation3dDegrees().plus(getOffset());
     }
@@ -25,21 +23,19 @@ public Rotation3d getRotation3dDegrees() {
      * Gets the rotation of the gyroscope (without offset) in degrees.
      * @return {@link Rotation3d} of the rotation of the gyroscope (without offset) in degrees.
      */
-    @NotNull
     public abstract Rotation3d getRawRotation3dDegrees();
 
     /**
      * Gets the acceleration from the IMU in meters per second squared.
      * @return {@link Rotation3d} of the acceleration from the IMU in meters per second squared.
      */
-    @NotNull
     public abstract Rotation3d getAccelerationMetersPerSecondSquared();
 
     /**
      * Sets the offset of the gyroscope.
      * @param offset the offset of the gyroscope.
      */
-    public void setOffset(@NotNull Rotation3d offset) {
+    public void setOffset(Rotation3d offset) {
         this.offset = offset;
     }
 
@@ -47,7 +43,6 @@ public void setOffset(@NotNull Rotation3d offset) {
      * Gets the offset of the gyroscope.
      * @return {@link Rotation3d} the offset of the gyroscope.
      */
-    @NotNull
     public Rotation3d getOffset() {
         return offset;
     }
@@ -70,7 +65,6 @@ public Rotation3d getOffset() {
      * (e.g the {@link ADIS16448IMU} would return {@link edu.wpi.first.wpilibj.ADIS16448_IMU})
      * @return the raw gyroscope object.
      */
-    @NotNull
     public abstract Object getRawIMU();
 
     /**

From 1b9402f0a08b40598f7b3b59ec6d8daed77075af Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Thu, 3 Oct 2024 08:14:06 -0400
Subject: [PATCH 04/27] refactor(hardware/gyro/imu): use Translation3d instead
 of Rotation3d for acceleration.

---
 .../hardware/gyro/imu/ADIS16448IMU.java       | 76 ++++++++++++++++++-
 .../hardware/gyro/imu/ADIS16470IMU.java       | 42 +++++++++-
 .../librobot/hardware/gyro/imu/IMU.java       |  5 +-
 3 files changed, 113 insertions(+), 10 deletions(-)

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
index b3bffcd..2e56fa4 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
@@ -1,6 +1,7 @@
 package net.frc5183.librobot.hardware.gyro.imu;
 
 import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.wpilibj.ADIS16448_IMU;
 import edu.wpi.first.wpilibj.SPI;
 import org.jetbrains.annotations.NotNull;
@@ -38,6 +39,8 @@ public class ADIS16448IMU extends IMU {
      */
     public ADIS16448IMU() {
         yaw = IMUAxis.Z;
+        pitch = IMUAxis.X;
+        roll = IMUAxis.Y;
         imu = new ADIS16448_IMU();
     }
 
@@ -46,6 +49,20 @@ public ADIS16448IMU() {
      * @param imu the ADIS16448 IMU to use.
      */
     public ADIS16448IMU(@NotNull ADIS16448_IMU imu) {
+        yaw = toIMUAxis(imu.getYawAxis());
+
+        //todo: confirm this is correct i had copilot do it
+        if (yaw == IMUAxis.X) {
+            pitch = IMUAxis.Y;
+            roll = IMUAxis.Z;
+        } else if (yaw == IMUAxis.Y) {
+            pitch = IMUAxis.X;
+            roll = IMUAxis.Z;
+        } else {
+            pitch = IMUAxis.X;
+            roll = IMUAxis.Y;
+        }
+
         this.imu = imu;
     }
 
@@ -56,6 +73,18 @@ public ADIS16448IMU(@NotNull ADIS16448_IMU imu) {
      */
     public ADIS16448IMU(IMUAxis yaw) {
         this.yaw = yaw;
+
+        if (yaw == IMUAxis.X) {
+            pitch = IMUAxis.Y;
+            roll = IMUAxis.Z;
+        } else if (yaw == IMUAxis.Y) {
+            pitch = IMUAxis.X;
+            roll = IMUAxis.Z;
+        } else {
+            pitch = IMUAxis.X;
+            roll = IMUAxis.Y;
+        }
+
         imu = new ADIS16448_IMU(fromIMUAxis(yaw), SPI.Port.kMXP, ADIS16448_IMU.CalibrationTime._512ms);
     }
 
@@ -67,6 +96,18 @@ public ADIS16448IMU(IMUAxis yaw) {
      */
     public ADIS16448IMU(IMUAxis yaw, SPI.Port port) {
         this.yaw = yaw;
+
+        if (yaw) {
+            pitch = IMUAxis.Y;
+            roll = IMUAxis.Z;
+        } else if (yaw == IMUAxis.Y) {
+            pitch = IMUAxis.X;
+            roll = IMUAxis.Z;
+        } else {
+            pitch = IMUAxis.X;
+            roll = IMUAxis.Y;
+        }
+
         imu = new ADIS16448_IMU(fromIMUAxis(yaw), port, ADIS16448_IMU.CalibrationTime._512ms);
     }
 
@@ -78,6 +119,18 @@ public ADIS16448IMU(IMUAxis yaw, SPI.Port port) {
      */
     public ADIS16448IMU(IMUAxis yaw, SPI.Port port, ADIS16448_IMU.CalibrationTime calibrationTime) {
         this.yaw = yaw;
+
+        if (yaw) {
+            pitch = IMUAxis.Y;
+            roll = IMUAxis.Z;
+        } else if (yaw == IMUAxis.Y) {
+            pitch = IMUAxis.X;
+            roll = IMUAxis.Z;
+        } else {
+            pitch = IMUAxis.X;
+            roll = IMUAxis.Y;
+        }
+
         imu = new ADIS16448_IMU(fromIMUAxis(yaw), port, calibrationTime);
     }
 
@@ -87,8 +140,8 @@ public ADIS16448IMU(IMUAxis yaw, SPI.Port port, ADIS16448_IMU.CalibrationTime ca
     }
 
     @Override
-    public @NotNull Rotation3d getAccelerationMetersPerSecondSquared() {
-        return new Rotation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ());
+    public @NotNull Translation3d getAccelerationMetersPerSecondSquared() {
+        return new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ());
     }
 
     @Override
@@ -108,8 +161,8 @@ public void reset() {
 
     /**
      * Returns a new {@link ADIS16448_IMU.IMUAxis} from an {@link IMUAxis}.
-     * @param axis the {@link IMUAxis} to convert
-     * @return the converted {@link ADIS16448_IMU.IMUAxis}
+     * @param axis the {@link IMUAxis} to convert.
+     * @return the converted {@link ADIS16448_IMU.IMUAxis}.
      */
     @NotNull
     public static ADIS16448_IMU.IMUAxis fromIMUAxis(@NotNull IMUAxis axis) {
@@ -119,4 +172,19 @@ public static ADIS16448_IMU.IMUAxis fromIMUAxis(@NotNull IMUAxis axis) {
             case Z -> ADIS16448_IMU.IMUAxis.kZ;
         };
     }
+
+
+    /**
+     * Returns a new {@link IMUAxis} from an {@link ADIS16448_IMU.IMUAxis}.
+     * @param axis the {@link ADIS16448_IMU.IMUAxis} to convert.
+     * @return the converted {@link IMUAxis}.
+     */
+    @NotNull
+    public static IMUAxis toIMUAxis(@NotNull ADIS16448_IMU.IMUAxis axis) {
+        return switch (axis) {
+            case kX -> IMUAxis.X;
+            case kY -> IMUAxis.Y;
+            case kZ -> IMUAxis.Z;
+        };
+    }
 }
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
index d60cec9..6a9e315 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
@@ -1,6 +1,7 @@
 package net.frc5183.librobot.hardware.gyro.imu;
 
 import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.wpilibj.ADIS16448_IMU;
 import edu.wpi.first.wpilibj.ADIS16470_IMU;
 import edu.wpi.first.wpilibj.SPI;
@@ -16,12 +17,31 @@ public class ADIS16470IMU extends IMU {
     @NotNull
     private final ADIS16470_IMU imu;
 
+    /**
+     * The YAW axis.
+     */
+    private final IMUAxis yaw;
+
+    /**
+     * The PITCH axis.
+     */
+    private final IMUAxis pitch;
+
+    /**
+     * The ROLL axis.
+     */
+    private final IMUAxis roll;
+
     /**
      * Creates a new {@link ADIS16470IMU} using the RIO's Onboard MXP port and 500ms calibration time, and yaw pitch roll as ZXY respectively.
      * @see ADIS16470_IMU#ADIS16470_IMU()
      */
     public ADIS16470IMU() {
         imu = new ADIS16470_IMU();
+
+        yaw = imu.getYawAxis();
+        pitch = imu.getPitchAxis();
+        roll = imu.getRollAxis();
     }
 
     /**
@@ -77,8 +97,8 @@ public ADIS16470IMU(IMUAxis yaw, IMUAxis pitch, IMUAxis roll, SPI.Port port, ADI
     }
 
     @Override
-    public @NotNull Rotation3d getAccelerationMetersPerSecondSquared() {
-        return new Rotation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ());
+    public @NotNull Translation3d getAccelerationMetersPerSecondSquared() {
+        return new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ());
     }
 
     @Override
@@ -98,8 +118,8 @@ public void reset() {
 
     /**
      * Returns a new {@link ADIS16470_IMU.IMUAxis} from an {@link IMUAxis}.
-     * @param axis the {@link IMUAxis} to convert
-     * @return the converted {@link ADIS16470_IMU.IMUAxis}
+     * @param axis the {@link IMUAxis} to convert.
+     * @return the converted {@link ADIS16470_IMU.IMUAxis}.
      */
     public static ADIS16470_IMU.IMUAxis fromIMUAxis(IMUAxis axis) {
         return switch (axis) {
@@ -108,4 +128,18 @@ public static ADIS16470_IMU.IMUAxis fromIMUAxis(IMUAxis axis) {
             case Z -> ADIS16470_IMU.IMUAxis.kZ;
         };
     }
+
+    /**
+     * Returns a new {@link IMUAxis} from an {@link ADIS16470_IMU.IMUAxis}.
+     * @param axis the {@link IMUAxis} to convert.
+     * @return the converted {@link ADIS16470_IMU.IMUAxis}.
+     */
+    public static IMUAxis toIMUAxis(ADIS16470_IMU.IMUAxis axis) {
+        return switch (axis) {
+            case ADIS16470_IMU.IMUAxis.kX -> IMUAxis.X;
+            case ADIS16470_IMU.IMUAxis.kY -> IMUAxis.Y;
+            case ADIS16470_IMU.IMUAxis.kZ -> IMUAxis.Z;
+            default -> throw new IllegalArgumentException("IMU axis must be one of  " + axis);
+        };
+    }
 }
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
index 9f394a6..637a1c9 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
@@ -1,6 +1,7 @@
 package net.frc5183.librobot.hardware.gyro.imu;
 
 import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
 
 /**
  * Represents an IMU (Inertial Measurement Unit).
@@ -27,9 +28,9 @@ public Rotation3d getRotation3dDegrees() {
 
     /**
      * Gets the acceleration from the IMU in meters per second squared.
-     * @return {@link Rotation3d} of the acceleration from the IMU in meters per second squared.
+     * @return {@link Translation3d} of the acceleration from the IMU in meters per second squared.
      */
-    public abstract Rotation3d getAccelerationMetersPerSecondSquared();
+    public abstract Translation3d getAccelerationMetersPerSecondSquared();
 
     /**
      * Sets the offset of the gyroscope.

From 045bb2cdebfed36b39509bab69cc427896388c27 Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Thu, 3 Oct 2024 08:27:39 -0400
Subject: [PATCH 05/27] refactor(hardware/gyro/imu): use radians instead of
 degrees

---
 .../librobot/hardware/gyro/imu/IMU.java       | 22 +++++++++----------
 1 file changed, 11 insertions(+), 11 deletions(-)

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
index 637a1c9..34b9fd1 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
@@ -13,18 +13,18 @@ public abstract class IMU {
     private Rotation3d offset = new Rotation3d(0, 0, 0);
 
     /**
-     * Gets the rotation of the gyroscope (with offset) in degrees.
-     * @return {@link Rotation3d} of the rotation of the gyroscope (with offset) in degrees.
+     * Gets the rotation of the gyroscope (with offset) in radians.
+     * @return {@link Rotation3d} of the rotation of the gyroscope (with offset) in radians.
      */
-    public Rotation3d getRotation3dDegrees() {
-        return getRawRotation3dDegrees().plus(getOffset());
+    public Rotation3d getRotation3d() {
+        return getRawRotation3d().plus(getOffset());
     }
 
     /**
-     * Gets the rotation of the gyroscope (without offset) in degrees.
-     * @return {@link Rotation3d} of the rotation of the gyroscope (without offset) in degrees.
+     * Gets the rotation of the gyroscope (without offset) in radians.
+     * @return {@link Rotation3d} of the rotation of the gyroscope (without offset) in radians.
      */
-    public abstract Rotation3d getRawRotation3dDegrees();
+    public abstract Rotation3d getRawRotation3d();
 
     /**
      * Gets the acceleration from the IMU in meters per second squared.
@@ -33,16 +33,16 @@ public Rotation3d getRotation3dDegrees() {
     public abstract Translation3d getAccelerationMetersPerSecondSquared();
 
     /**
-     * Sets the offset of the gyroscope.
-     * @param offset the offset of the gyroscope.
+     * Sets the offset of the gyroscope in radians.
+     * @param offset the {@link Rotation3d} of the offset of the gyroscope in radians.
      */
     public void setOffset(Rotation3d offset) {
         this.offset = offset;
     }
 
     /**
-     * Gets the offset of the gyroscope.
-     * @return {@link Rotation3d} the offset of the gyroscope.
+     * Gets the offset of the gyroscope in radians.
+     * @return {@link Rotation3d} the offset of the gyroscope in radians.
      */
     public Rotation3d getOffset() {
         return offset;

From d180bee6caa19b8a9eac4deb0c949430b78409ff Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Thu, 3 Oct 2024 08:31:08 -0400
Subject: [PATCH 06/27] feat/refactor(hardware/gyro/imu/adis16*): finish
 implementations

---
 .../hardware/gyro/imu/ADIS16448IMU.java       | 62 ++++++-------------
 .../hardware/gyro/imu/ADIS16470IMU.java       | 26 ++++----
 2 files changed, 35 insertions(+), 53 deletions(-)

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
index 2e56fa4..cfc2494 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
@@ -19,17 +19,19 @@ public class ADIS16448IMU extends IMU {
     private final ADIS16448_IMU imu;
 
     /**
-     * The YAW axis.
+     * The yaw axis.
      */
+    @NotNull
     private final IMUAxis yaw;
 
     /**
-     * The PITCH axis.
+     * The pitch axis.
      */
+    @NotNull
     private final IMUAxis pitch;
 
     /**
-     * The ROLL axis.
+     * The roll axis.
      */
     private final IMUAxis roll;
 
@@ -38,10 +40,7 @@ public class ADIS16448IMU extends IMU {
      * @see ADIS16448_IMU#ADIS16448_IMU()
      */
     public ADIS16448IMU() {
-        yaw = IMUAxis.Z;
-        pitch = IMUAxis.X;
-        roll = IMUAxis.Y;
-        imu = new ADIS16448_IMU();
+        this(new ADIS16448_IMU());
     }
 
     /**
@@ -71,21 +70,8 @@ public ADIS16448IMU(@NotNull ADIS16448_IMU imu) {
      * Uses default RIO's Onboard MXP port and 512ms calibration time.
      * @param yaw the YAW axis.
      */
-    public ADIS16448IMU(IMUAxis yaw) {
-        this.yaw = yaw;
-
-        if (yaw == IMUAxis.X) {
-            pitch = IMUAxis.Y;
-            roll = IMUAxis.Z;
-        } else if (yaw == IMUAxis.Y) {
-            pitch = IMUAxis.X;
-            roll = IMUAxis.Z;
-        } else {
-            pitch = IMUAxis.X;
-            roll = IMUAxis.Y;
-        }
-
-        imu = new ADIS16448_IMU(fromIMUAxis(yaw), SPI.Port.kMXP, ADIS16448_IMU.CalibrationTime._512ms);
+    public ADIS16448IMU(@NotNull IMUAxis yaw) {
+        this(yaw, SPI.Port.kMXP);
     }
 
     /**
@@ -94,21 +80,8 @@ public ADIS16448IMU(IMUAxis yaw) {
      * @param yaw the YAW axis.
      * @param port the SPI port.
      */
-    public ADIS16448IMU(IMUAxis yaw, SPI.Port port) {
-        this.yaw = yaw;
-
-        if (yaw) {
-            pitch = IMUAxis.Y;
-            roll = IMUAxis.Z;
-        } else if (yaw == IMUAxis.Y) {
-            pitch = IMUAxis.X;
-            roll = IMUAxis.Z;
-        } else {
-            pitch = IMUAxis.X;
-            roll = IMUAxis.Y;
-        }
-
-        imu = new ADIS16448_IMU(fromIMUAxis(yaw), port, ADIS16448_IMU.CalibrationTime._512ms);
+    public ADIS16448IMU(@NotNull IMUAxis yaw, @NotNull SPI.Port port) {
+        this(yaw, port, ADIS16448_IMU.CalibrationTime._512ms);
     }
 
     /**
@@ -117,10 +90,11 @@ public ADIS16448IMU(IMUAxis yaw, SPI.Port port) {
      * @param port the SPI port.
      * @param calibrationTime the calibration time.
      */
-    public ADIS16448IMU(IMUAxis yaw, SPI.Port port, ADIS16448_IMU.CalibrationTime calibrationTime) {
+    public ADIS16448IMU(@NotNull IMUAxis yaw, @NotNull SPI.Port port, @NotNull ADIS16448_IMU.CalibrationTime calibrationTime) {
         this.yaw = yaw;
 
-        if (yaw) {
+        //todo: confirm this is correct i had copilot do it
+        if (yaw == IMUAxis.X) {
             pitch = IMUAxis.Y;
             roll = IMUAxis.Z;
         } else if (yaw == IMUAxis.Y) {
@@ -135,8 +109,12 @@ public ADIS16448IMU(IMUAxis yaw, SPI.Port port, ADIS16448_IMU.CalibrationTime ca
     }
 
     @Override
-    public @NotNull Rotation3d getRawRotation3dDegrees() {
-        return new Rotation3d(imu.getGyroAngleX(), imu.getGyroAngleY(), imu.getGyroAngleZ());
+    public @NotNull Rotation3d getRawRotation3d() {
+        return new Rotation3d(
+                Math.toRadians(imu.getGyroAngleX()),
+                Math.toRadians(imu.getGyroAngleY()),
+                Math.toRadians(imu.getGyroAngleZ())
+        );
     }
 
     @Override
@@ -187,4 +165,4 @@ public static IMUAxis toIMUAxis(@NotNull ADIS16448_IMU.IMUAxis axis) {
             case kZ -> IMUAxis.Z;
         };
     }
-}
+}
\ No newline at end of file
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
index 6a9e315..85509e1 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
@@ -37,11 +37,7 @@ public class ADIS16470IMU extends IMU {
      * @see ADIS16470_IMU#ADIS16470_IMU()
      */
     public ADIS16470IMU() {
-        imu = new ADIS16470_IMU();
-
-        yaw = imu.getYawAxis();
-        pitch = imu.getPitchAxis();
-        roll = imu.getRollAxis();
+        this(new ADIS16470_IMU());
     }
 
     /**
@@ -50,6 +46,10 @@ public ADIS16470IMU() {
      */
     public ADIS16470IMU(@NotNull ADIS16470_IMU imu) {
         this.imu = imu;
+
+        yaw = toIMUAxis(imu.getYawAxis());
+        pitch = toIMUAxis(imu.getPitchAxis());
+        roll = toIMUAxis(imu.getRollAxis());
     }
 
     /**
@@ -60,7 +60,7 @@ public ADIS16470IMU(@NotNull ADIS16470_IMU imu) {
      * @param roll the ROLL axis.
      */
     public ADIS16470IMU(IMUAxis yaw, IMUAxis pitch, IMUAxis roll) {
-        imu = new ADIS16470_IMU(fromIMUAxis(yaw), fromIMUAxis(pitch), fromIMUAxis(roll));
+        this(yaw, pitch, roll, SPI.Port.kMXP);
     }
 
     /**
@@ -72,7 +72,7 @@ public ADIS16470IMU(IMUAxis yaw, IMUAxis pitch, IMUAxis roll) {
      * @param port the SPI port.
      */
     public ADIS16470IMU(IMUAxis yaw, IMUAxis pitch, IMUAxis roll, SPI.Port port) {
-        imu = new ADIS16470_IMU(fromIMUAxis(yaw), fromIMUAxis(pitch), fromIMUAxis(roll), port, ADIS16470_IMU.CalibrationTime._4s);
+        this(yaw, pitch, roll, port, ADIS16470_IMU.CalibrationTime._4s);
     }
 
     /**
@@ -85,14 +85,18 @@ public ADIS16470IMU(IMUAxis yaw, IMUAxis pitch, IMUAxis roll, SPI.Port port) {
      */
     public ADIS16470IMU(IMUAxis yaw, IMUAxis pitch, IMUAxis roll, SPI.Port port, ADIS16470_IMU.CalibrationTime calibrationTime) {
         imu = new ADIS16470_IMU(fromIMUAxis(yaw), fromIMUAxis(pitch), fromIMUAxis(roll), port, calibrationTime);
+
+        this.yaw = yaw;
+        this.pitch = pitch;
+        this.roll = roll;
     }
 
     @Override
-    public @NotNull Rotation3d getRawRotation3dDegrees() {
+    public @NotNull Rotation3d getRawRotation3d() {
         return new Rotation3d(
-                imu.getAngle(fromIMUAxis(IMUAxis.X)),
-                imu.getAngle(fromIMUAxis(IMUAxis.Y)),
-                imu.getAngle(fromIMUAxis(IMUAxis.Z))
+                Math.toRadians(imu.getAngle(fromIMUAxis(IMUAxis.X))),
+                Math.toRadians(imu.getAngle(fromIMUAxis(IMUAxis.Y))),
+                Math.toRadians(imu.getAngle(fromIMUAxis(IMUAxis.Z)))
         );
     }
 

From 4f91bc7278ac0b79000101e94af5a9d6a995ecd0 Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Thu, 3 Oct 2024 08:32:32 -0400
Subject: [PATCH 07/27] style(hardware/gyro/imu/adis16470): remove unused
 import

---
 .../net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java     | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
index 85509e1..1fe0d8a 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
@@ -2,7 +2,6 @@
 
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.wpilibj.ADIS16448_IMU;
 import edu.wpi.first.wpilibj.ADIS16470_IMU;
 import edu.wpi.first.wpilibj.SPI;
 import org.jetbrains.annotations.NotNull;

From a49c1713816644a4b5aba335d81e83d563a4ed6c Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Thu, 3 Oct 2024 08:36:53 -0400
Subject: [PATCH 08/27] chore(build.gradle): add wpiunits dependency

---
 build.gradle | 1 +
 1 file changed, 1 insertion(+)

diff --git a/build.gradle b/build.gradle
index a8f0ffe..2cf0967 100644
--- a/build.gradle
+++ b/build.gradle
@@ -48,6 +48,7 @@ dependencies {
     compileOnly "edu.wpi.first.wpiutil:wpiutil-java:2024.3.2"
     compileOnly "edu.wpi.first.wpilibj:commands:2024.3.2"
     compileOnly "edu.wpi.first.wpimath:wpimath-java:2024.3.2"
+    compileOnly "edu.wpi.first.wpiunits:wpiunits-java:2024.3.2"
     compileOnly "edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2024.3.2"
 
     // PathPlannerLib

From 2e5b57d26e5dc3b6e52a9896b33442a103646deb Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Thu, 3 Oct 2024 08:39:36 -0400
Subject: [PATCH 09/27] refactor(hardware/gyro/imu/adis16470): use unqualified
 name instead of FQDN in switch statement

---
 .../frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java    | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
index 1fe0d8a..c69c861 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
@@ -139,9 +139,9 @@ public static ADIS16470_IMU.IMUAxis fromIMUAxis(IMUAxis axis) {
      */
     public static IMUAxis toIMUAxis(ADIS16470_IMU.IMUAxis axis) {
         return switch (axis) {
-            case ADIS16470_IMU.IMUAxis.kX -> IMUAxis.X;
-            case ADIS16470_IMU.IMUAxis.kY -> IMUAxis.Y;
-            case ADIS16470_IMU.IMUAxis.kZ -> IMUAxis.Z;
+            case kX -> IMUAxis.X;
+            case kY -> IMUAxis.Y;
+            case kZ -> IMUAxis.Z;
             default -> throw new IllegalArgumentException("IMU axis must be one of  " + axis);
         };
     }

From 38811041f3ef9b0ef2860a7e8dd0e5ec25e8a27d Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Thu, 3 Oct 2024 11:13:38 -0400
Subject: [PATCH 10/27] refactor(hardware/gyro/imu): put unit in method name
 for IMU.getRotation3d() and IMU.getRawRotation3d()

---
 .../frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java    | 2 +-
 .../frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java    | 2 +-
 .../java/net/frc5183/librobot/hardware/gyro/imu/IMU.java    | 6 +++---
 3 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
index cfc2494..0e5153a 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
@@ -109,7 +109,7 @@ public ADIS16448IMU(@NotNull IMUAxis yaw, @NotNull SPI.Port port, @NotNull ADIS1
     }
 
     @Override
-    public @NotNull Rotation3d getRawRotation3d() {
+    public @NotNull Rotation3d getRawRotation3dRadians() {
         return new Rotation3d(
                 Math.toRadians(imu.getGyroAngleX()),
                 Math.toRadians(imu.getGyroAngleY()),
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
index c69c861..5a4d831 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
@@ -91,7 +91,7 @@ public ADIS16470IMU(IMUAxis yaw, IMUAxis pitch, IMUAxis roll, SPI.Port port, ADI
     }
 
     @Override
-    public @NotNull Rotation3d getRawRotation3d() {
+    public @NotNull Rotation3d getRawRotation3dRadians() {
         return new Rotation3d(
                 Math.toRadians(imu.getAngle(fromIMUAxis(IMUAxis.X))),
                 Math.toRadians(imu.getAngle(fromIMUAxis(IMUAxis.Y))),
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
index 34b9fd1..378a89a 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
@@ -16,15 +16,15 @@ public abstract class IMU {
      * Gets the rotation of the gyroscope (with offset) in radians.
      * @return {@link Rotation3d} of the rotation of the gyroscope (with offset) in radians.
      */
-    public Rotation3d getRotation3d() {
-        return getRawRotation3d().plus(getOffset());
+    public Rotation3d getRotation3dRadians() {
+        return getRawRotation3dRadians().plus(getOffset());
     }
 
     /**
      * Gets the rotation of the gyroscope (without offset) in radians.
      * @return {@link Rotation3d} of the rotation of the gyroscope (without offset) in radians.
      */
-    public abstract Rotation3d getRawRotation3d();
+    public abstract Rotation3d getRawRotation3dRadians();
 
     /**
      * Gets the acceleration from the IMU in meters per second squared.

From e26558e485ef59cce2dab0962466aa0c90a7af54 Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Thu, 3 Oct 2024 11:56:40 -0400
Subject: [PATCH 11/27] feat(hardware/gyro/imu): add
 getAngleRadians(SingleAxisGyroscope.Axis) method to IMU

---
 .../hardware/gyro/imu/ADIS16448IMU.java       | 27 +++++++++++++++++--
 .../hardware/gyro/imu/ADIS16470IMU.java       | 19 +++++++++++++
 .../librobot/hardware/gyro/imu/IMU.java       |  7 +++++
 3 files changed, 51 insertions(+), 2 deletions(-)

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
index 0e5153a..1de0731 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
@@ -4,10 +4,9 @@
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.wpilibj.ADIS16448_IMU;
 import edu.wpi.first.wpilibj.SPI;
+import net.frc5183.librobot.hardware.gyro.single.SingleAxisGyroscope;
 import org.jetbrains.annotations.NotNull;
 
-import java.util.Optional;
-
 /**
  * Represents an ADIS16448 {@link IMU}.
  */
@@ -108,6 +107,30 @@ public ADIS16448IMU(@NotNull IMUAxis yaw, @NotNull SPI.Port port, @NotNull ADIS1
         imu = new ADIS16448_IMU(fromIMUAxis(yaw), port, calibrationTime);
     }
 
+    @Override
+    public double getAngleRadians(@NotNull SingleAxisGyroscope.Axis axis) {
+        return switch (axis) {
+            case YAW ->
+                    switch (this.yaw) {
+                        case IMUAxis.X -> imu.getGyroAngleX();
+                        case IMUAxis.Y -> imu.getGyroAngleY();
+                        case IMUAxis.Z -> imu.getGyroAngleZ();
+                    };
+            case PITCH ->
+                    switch (this.pitch) {
+                        case IMUAxis.X -> imu.getGyroAngleX();
+                        case IMUAxis.Y -> imu.getGyroAngleY();
+                        case IMUAxis.Z -> imu.getGyroAngleZ();
+                    };
+            case ROLL ->
+                    switch (this.roll) {
+                        case IMUAxis.X -> imu.getGyroAngleX();
+                        case IMUAxis.Y -> imu.getGyroAngleY();
+                        case IMUAxis.Z -> imu.getGyroAngleZ();
+                    };
+        };
+    }
+
     @Override
     public @NotNull Rotation3d getRawRotation3dRadians() {
         return new Rotation3d(
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
index 5a4d831..3f2748e 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
@@ -4,6 +4,7 @@
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.wpilibj.ADIS16470_IMU;
 import edu.wpi.first.wpilibj.SPI;
+import net.frc5183.librobot.hardware.gyro.single.SingleAxisGyroscope;
 import org.jetbrains.annotations.NotNull;
 
 /**
@@ -90,6 +91,11 @@ public ADIS16470IMU(IMUAxis yaw, IMUAxis pitch, IMUAxis roll, SPI.Port port, ADI
         this.roll = roll;
     }
 
+    @Override
+    public double getAngleRadians(@NotNull SingleAxisGyroscope.Axis axis) {
+        return imu.getAngle(fromSingleAxis(axis));
+    }
+
     @Override
     public @NotNull Rotation3d getRawRotation3dRadians() {
         return new Rotation3d(
@@ -132,6 +138,19 @@ public static ADIS16470_IMU.IMUAxis fromIMUAxis(IMUAxis axis) {
         };
     }
 
+    /**
+     * Returns a new {@link ADIS16470_IMU.IMUAxis} from an {@link SingleAxisGyroscope.Axis}
+     * @param axis the {@link SingleAxisGyroscope.Axis} to convert.
+     * @return the converted {@link ADIS16470_IMU.IMUAxis}
+     */
+    public static ADIS16470_IMU.IMUAxis fromSingleAxis(SingleAxisGyroscope.Axis axis) {
+        return switch (axis) {
+            case YAW -> ADIS16470_IMU.IMUAxis.kYaw;
+            case PITCH -> ADIS16470_IMU.IMUAxis.kPitch;
+            case ROLL -> ADIS16470_IMU.IMUAxis.kRoll;
+        };
+    }
+
     /**
      * Returns a new {@link IMUAxis} from an {@link ADIS16470_IMU.IMUAxis}.
      * @param axis the {@link IMUAxis} to convert.
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
index 378a89a..0c473bc 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
@@ -2,6 +2,7 @@
 
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
+import net.frc5183.librobot.hardware.gyro.single.SingleAxisGyroscope;
 
 /**
  * Represents an IMU (Inertial Measurement Unit).
@@ -12,6 +13,12 @@ public abstract class IMU {
      */
     private Rotation3d offset = new Rotation3d(0, 0, 0);
 
+    /**
+     * Gets the specified angle's rotation in radians.
+     * @return the angle's rotation in radians.
+     */
+    public abstract double getAngleRadians(SingleAxisGyroscope.Axis axis);
+
     /**
      * Gets the rotation of the gyroscope (with offset) in radians.
      * @return {@link Rotation3d} of the rotation of the gyroscope (with offset) in radians.

From a95c63928b15bb0d0a57c5fa02a4af9c4fb452bc Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Thu, 3 Oct 2024 11:59:55 -0400
Subject: [PATCH 12/27] refactor(hardware/imu/adis16448): use unqualified name
 instead of FQDN in switch statement

---
 .../hardware/gyro/imu/ADIS16448IMU.java        | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
index 1de0731..90bded6 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
@@ -112,21 +112,21 @@ public double getAngleRadians(@NotNull SingleAxisGyroscope.Axis axis) {
         return switch (axis) {
             case YAW ->
                     switch (this.yaw) {
-                        case IMUAxis.X -> imu.getGyroAngleX();
-                        case IMUAxis.Y -> imu.getGyroAngleY();
-                        case IMUAxis.Z -> imu.getGyroAngleZ();
+                        case X -> imu.getGyroAngleX();
+                        case Y -> imu.getGyroAngleY();
+                        case Z -> imu.getGyroAngleZ();
                     };
             case PITCH ->
                     switch (this.pitch) {
-                        case IMUAxis.X -> imu.getGyroAngleX();
-                        case IMUAxis.Y -> imu.getGyroAngleY();
-                        case IMUAxis.Z -> imu.getGyroAngleZ();
+                        case X -> imu.getGyroAngleX();
+                        case Y -> imu.getGyroAngleY();
+                        case Z -> imu.getGyroAngleZ();
                     };
             case ROLL ->
                     switch (this.roll) {
-                        case IMUAxis.X -> imu.getGyroAngleX();
-                        case IMUAxis.Y -> imu.getGyroAngleY();
-                        case IMUAxis.Z -> imu.getGyroAngleZ();
+                        case X -> imu.getGyroAngleX();
+                        case Y -> imu.getGyroAngleY();
+                        case Z -> imu.getGyroAngleZ();
                     };
         };
     }

From 648adab8931965bdd28732f5a6412ab880dab18d Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Fri, 4 Oct 2024 08:27:38 -0400
Subject: [PATCH 13/27] feat/fix(hardware/gyro/imu/adis16*): implement
 getAngleRadians(SingleAxisGyroscope.Axis) and fix getRawRotation3dRadians()
 by using yaw pitch roll instead of XYZ

---
 .../hardware/gyro/imu/ADIS16448IMU.java       | 33 ++++++++++++++++---
 .../hardware/gyro/imu/ADIS16470IMU.java       |  6 ++--
 2 files changed, 31 insertions(+), 8 deletions(-)

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
index 90bded6..12ff901 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
@@ -109,6 +109,7 @@ public ADIS16448IMU(@NotNull IMUAxis yaw, @NotNull SPI.Port port, @NotNull ADIS1
 
     @Override
     public double getAngleRadians(@NotNull SingleAxisGyroscope.Axis axis) {
+        // todo: this has a lot of branching but im not sure if there's really a better way to do it
         return switch (axis) {
             case YAW ->
                     switch (this.yaw) {
@@ -133,11 +134,33 @@ public double getAngleRadians(@NotNull SingleAxisGyroscope.Axis axis) {
 
     @Override
     public @NotNull Rotation3d getRawRotation3dRadians() {
-        return new Rotation3d(
-                Math.toRadians(imu.getGyroAngleX()),
-                Math.toRadians(imu.getGyroAngleY()),
-                Math.toRadians(imu.getGyroAngleZ())
-        );
+        double rollRadians;
+        double pitchRadians;
+        double yawRadians;
+
+        // todo: again, not too sure if there's a better way
+        if (this.roll == IMUAxis.X)
+            rollRadians = imu.getGyroAngleX();
+        else if (this.roll == IMUAxis.Y)
+            rollRadians = imu.getGyroAngleY();
+        else
+            rollRadians = imu.getGyroAngleZ();
+
+        if (this.pitch == IMUAxis.X)
+            pitchRadians = imu.getGyroAngleX();
+        else if (this.pitch == IMUAxis.Y)
+            pitchRadians = imu.getGyroAngleY();
+        else
+            pitchRadians = imu.getGyroAngleZ();
+
+        if (this.yaw == IMUAxis.X)
+            yawRadians = imu.getGyroAngleX();
+        else if (this.yaw == IMUAxis.Y)
+            yawRadians = imu.getGyroAngleY();
+        else
+            yawRadians = imu.getGyroAngleZ();
+
+        return new Rotation3d(rollRadians, pitchRadians, yawRadians);
     }
 
     @Override
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
index 3f2748e..c966c20 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
@@ -99,9 +99,9 @@ public double getAngleRadians(@NotNull SingleAxisGyroscope.Axis axis) {
     @Override
     public @NotNull Rotation3d getRawRotation3dRadians() {
         return new Rotation3d(
-                Math.toRadians(imu.getAngle(fromIMUAxis(IMUAxis.X))),
-                Math.toRadians(imu.getAngle(fromIMUAxis(IMUAxis.Y))),
-                Math.toRadians(imu.getAngle(fromIMUAxis(IMUAxis.Z)))
+                Math.toRadians(imu.getAngle(fromIMUAxis(this.roll))),
+                Math.toRadians(imu.getAngle(fromIMUAxis(this.pitch))),
+                Math.toRadians(imu.getAngle(fromIMUAxis(this.yaw)))
         );
     }
 

From 28dfa6cc5c031522dc691642e3434a877f0a7903 Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Mon, 7 Oct 2024 08:51:21 -0400
Subject: [PATCH 14/27] feat(hardware/gyro/imu): finish implementation of IMU
 and it's subclasses

---
 .../hardware/gyro/imu/ADIS16448IMU.java       |  48 +++++-
 .../hardware/gyro/imu/ADIS16470IMU.java       | 106 +++++++++++--
 .../librobot/hardware/gyro/imu/IMU.java       | 143 ++++++++++++++++--
 3 files changed, 271 insertions(+), 26 deletions(-)

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
index 12ff901..6bd119a 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
@@ -108,7 +108,7 @@ public ADIS16448IMU(@NotNull IMUAxis yaw, @NotNull SPI.Port port, @NotNull ADIS1
     }
 
     @Override
-    public double getAngleRadians(@NotNull SingleAxisGyroscope.Axis axis) {
+    public double getRawAngleRadians(@NotNull SingleAxisGyroscope.Axis axis) {
         // todo: this has a lot of branching but im not sure if there's really a better way to do it
         return switch (axis) {
             case YAW ->
@@ -132,6 +132,15 @@ public double getAngleRadians(@NotNull SingleAxisGyroscope.Axis axis) {
         };
     }
 
+    @Override
+    public double getRawAngleRadians(IMUAxis axis) {
+        return switch (axis) {
+            case X -> imu.getGyroAngleX();
+            case Y -> imu.getGyroAngleY();
+            case Z -> imu.getGyroAngleZ();
+        };
+    }
+
     @Override
     public @NotNull Rotation3d getRawRotation3dRadians() {
         double rollRadians;
@@ -168,6 +177,15 @@ else if (this.yaw == IMUAxis.Y)
         return new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ());
     }
 
+    @Override
+    public double getAccelerationMetersPerSecondSquared(IMUAxis axis) {
+        return switch (axis) {
+            case X -> imu.getAccelX();
+            case Y -> imu.getAccelY();
+            case Z -> imu.getAccelZ();
+        };
+    }
+
     @Override
     public void calibrate() {
         imu.calibrate();
@@ -179,7 +197,33 @@ public void reset() {
     }
 
     @Override
-    public @NotNull ADIS16448_IMU getRawIMU() {
+    public void factoryDefault() {
+        imu.calibrate();
+        super.setOffset(new Rotation3d());
+    }
+
+    @Override
+    public void clearStickyFaults() {
+        // The ADIS16448 IMU does not have a method for clearing sticky faults.
+    }
+
+    @Override
+    public @NotNull IMUAxis getYawAxis() {
+        return yaw;
+    }
+
+    @Override
+    public @NotNull IMUAxis getPitchAxis() {
+        return pitch;
+    }
+
+    @Override
+    public IMUAxis getRollAxis() {
+        return roll;
+    }
+
+    @Override
+    public @NotNull ADIS16448_IMU getIMU() {
         return imu;
     }
 
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
index c966c20..f395bc7 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
@@ -33,7 +33,7 @@ public class ADIS16470IMU extends IMU {
     private final IMUAxis roll;
 
     /**
-     * Creates a new {@link ADIS16470IMU} using the RIO's Onboard MXP port and 500ms calibration time, and yaw pitch roll as ZXY respectively.
+     * Creates a new {@link ADIS16470IMU} using the RIO's Onboard MXP port and 4s calibration time, and yaw pitch roll as ZXY respectively.
      * @see ADIS16470_IMU#ADIS16470_IMU()
      */
     public ADIS16470IMU() {
@@ -54,12 +54,15 @@ public ADIS16470IMU(@NotNull ADIS16470_IMU imu) {
 
     /**
      * Creates a new {@link ADIS16470IMU} with a specified yaw, pitch, and roll axis.
-     * Uses default RIO's Onboard MXP port and 500ms calibration time.
+     * Uses default RIO's Onboard MXP port and 4s calibration time.
      * @param yaw the YAW axis.
      * @param pitch the PITCH axis.
      * @param roll the ROLL axis.
      */
-    public ADIS16470IMU(IMUAxis yaw, IMUAxis pitch, IMUAxis roll) {
+    public ADIS16470IMU(
+            @NotNull IMUAxis yaw,
+            @NotNull IMUAxis pitch,
+            @NotNull IMUAxis roll) {
         this(yaw, pitch, roll, SPI.Port.kMXP);
     }
 
@@ -71,7 +74,12 @@ public ADIS16470IMU(IMUAxis yaw, IMUAxis pitch, IMUAxis roll) {
      * @param roll the ROLL axis.
      * @param port the SPI port.
      */
-    public ADIS16470IMU(IMUAxis yaw, IMUAxis pitch, IMUAxis roll, SPI.Port port) {
+    public ADIS16470IMU(
+            @NotNull IMUAxis yaw,
+            @NotNull IMUAxis pitch,
+            @NotNull IMUAxis roll,
+            @NotNull SPI.Port port
+    ) {
         this(yaw, pitch, roll, port, ADIS16470_IMU.CalibrationTime._4s);
     }
 
@@ -83,8 +91,20 @@ public ADIS16470IMU(IMUAxis yaw, IMUAxis pitch, IMUAxis roll, SPI.Port port) {
      * @param port the SPI port.
      * @param calibrationTime the calibration time.
      */
-    public ADIS16470IMU(IMUAxis yaw, IMUAxis pitch, IMUAxis roll, SPI.Port port, ADIS16470_IMU.CalibrationTime calibrationTime) {
-        imu = new ADIS16470_IMU(fromIMUAxis(yaw), fromIMUAxis(pitch), fromIMUAxis(roll), port, calibrationTime);
+    public ADIS16470IMU(
+            @NotNull IMUAxis yaw,
+            @NotNull IMUAxis pitch,
+            @NotNull IMUAxis roll,
+            @NotNull SPI.Port port,
+            @NotNull ADIS16470_IMU.CalibrationTime calibrationTime
+    ) {
+        imu = new ADIS16470_IMU(
+                fromIMUAxis(yaw),
+                fromIMUAxis(pitch),
+                fromIMUAxis(roll),
+                port,
+                calibrationTime
+        );
 
         this.yaw = yaw;
         this.pitch = pitch;
@@ -92,10 +112,15 @@ public ADIS16470IMU(IMUAxis yaw, IMUAxis pitch, IMUAxis roll, SPI.Port port, ADI
     }
 
     @Override
-    public double getAngleRadians(@NotNull SingleAxisGyroscope.Axis axis) {
+    public double getRawAngleRadians(@NotNull SingleAxisGyroscope.Axis axis) {
         return imu.getAngle(fromSingleAxis(axis));
     }
 
+    @Override
+    public double getRawAngleRadians(@NotNull IMUAxis axis) {
+        return imu.getAngle(fromIMUAxis(axis));
+    }
+
     @Override
     public @NotNull Rotation3d getRawRotation3dRadians() {
         return new Rotation3d(
@@ -110,6 +135,15 @@ public double getAngleRadians(@NotNull SingleAxisGyroscope.Axis axis) {
         return new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ());
     }
 
+    @Override
+    public double getAccelerationMetersPerSecondSquared(IMUAxis axis) {
+        return switch (axis) {
+            case X -> imu.getAccelX();
+            case Y -> imu.getAccelY();
+            case Z -> imu.getAccelZ();
+        };
+    }
+
     @Override
     public void calibrate() {
         imu.calibrate();
@@ -121,7 +155,33 @@ public void reset() {
     }
 
     @Override
-    public @NotNull ADIS16470_IMU getRawIMU() {
+    public void factoryDefault() {
+        imu.calibrate();
+        super.setOffset(new Rotation3d());
+    }
+
+    @Override
+    public void clearStickyFaults() {
+        // The ADIS16470 IMU does not have a method for clearing sticky faults.
+    }
+
+    @Override
+    public IMUAxis getYawAxis() {
+        return yaw;
+    }
+
+    @Override
+    public IMUAxis getPitchAxis() {
+        return pitch;
+    }
+
+    @Override
+    public IMUAxis getRollAxis() {
+        return roll;
+    }
+
+    @Override
+    public @NotNull ADIS16470_IMU getIMU() {
         return imu;
     }
 
@@ -130,7 +190,8 @@ public void reset() {
      * @param axis the {@link IMUAxis} to convert.
      * @return the converted {@link ADIS16470_IMU.IMUAxis}.
      */
-    public static ADIS16470_IMU.IMUAxis fromIMUAxis(IMUAxis axis) {
+    @NotNull
+    public static ADIS16470_IMU.IMUAxis fromIMUAxis(@NotNull IMUAxis axis) {
         return switch (axis) {
             case X -> ADIS16470_IMU.IMUAxis.kX;
             case Y -> ADIS16470_IMU.IMUAxis.kY;
@@ -143,7 +204,8 @@ public static ADIS16470_IMU.IMUAxis fromIMUAxis(IMUAxis axis) {
      * @param axis the {@link SingleAxisGyroscope.Axis} to convert.
      * @return the converted {@link ADIS16470_IMU.IMUAxis}
      */
-    public static ADIS16470_IMU.IMUAxis fromSingleAxis(SingleAxisGyroscope.Axis axis) {
+    @NotNull
+    public static ADIS16470_IMU.IMUAxis fromSingleAxis(@NotNull SingleAxisGyroscope.Axis axis) {
         return switch (axis) {
             case YAW -> ADIS16470_IMU.IMUAxis.kYaw;
             case PITCH -> ADIS16470_IMU.IMUAxis.kPitch;
@@ -153,15 +215,31 @@ public static ADIS16470_IMU.IMUAxis fromSingleAxis(SingleAxisGyroscope.Axis axis
 
     /**
      * Returns a new {@link IMUAxis} from an {@link ADIS16470_IMU.IMUAxis}.
-     * @param axis the {@link IMUAxis} to convert.
-     * @return the converted {@link ADIS16470_IMU.IMUAxis}.
+     * @param axis the {@link ADIS16470_IMU.IMUAxis} to convert.
+     * @return the converted {@link IMUAxis}.
+     * @throws IllegalArgumentException if {@code axis} is not one of kX, kY, or kZ.
      */
-    public static IMUAxis toIMUAxis(ADIS16470_IMU.IMUAxis axis) {
+    public static IMUAxis toIMUAxis(@NotNull ADIS16470_IMU.IMUAxis axis) {
         return switch (axis) {
             case kX -> IMUAxis.X;
             case kY -> IMUAxis.Y;
             case kZ -> IMUAxis.Z;
-            default -> throw new IllegalArgumentException("IMU axis must be one of  " + axis);
+            default -> throw new IllegalArgumentException("ADIS16470_IMU Axis must be one of kX, kY, or kZ. Was: " + axis);
+        };
+    }
+
+    /**
+     * Returns a new {@link SingleAxisGyroscope.Axis} from an {@link ADIS16470_IMU.IMUAxis}.
+     * @param axis the {@link ADIS16470_IMU.IMUAxis} to convert.
+     * @return the converted {@link SingleAxisGyroscope.Axis}.
+     * @throws IllegalArgumentException if {@code axis} is not one of kYaw, kPitch, or kRoll.
+     */
+    public static SingleAxisGyroscope.Axis toSingleAxis(@NotNull ADIS16470_IMU.IMUAxis axis) {
+        return switch (axis) {
+            case kYaw -> SingleAxisGyroscope.Axis.YAW;
+            case kPitch -> SingleAxisGyroscope.Axis.PITCH;
+            case kRoll -> SingleAxisGyroscope.Axis.ROLL;
+            default -> throw new IllegalArgumentException("ADIS16470_IMU Axis must be one of kYaw, kPitch, or kRoll. Was" + axis);
         };
     }
 }
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
index 0c473bc..1ea3ebb 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
@@ -3,42 +3,121 @@
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import net.frc5183.librobot.hardware.gyro.single.SingleAxisGyroscope;
+import swervelib.imu.SwerveIMU;
+
+import java.util.Optional;
 
 /**
  * Represents an IMU (Inertial Measurement Unit).
  */
-public abstract class IMU {
+public abstract class IMU extends SwerveIMU {
+    /**
+     * Whether the IMU's values should be inverted.
+     */
+    private boolean inverted = false;
+
     /**
      * The offset to be added to the gyroscope.
      */
     private Rotation3d offset = new Rotation3d(0, 0, 0);
 
     /**
-     * Gets the specified angle's rotation in radians.
-     * @return the angle's rotation in radians.
+     * Gets the specified angle's rotation (with offset) in radians.
+     * @param axis the {@link IMUAxis} to get the angle's rotation of.
+     * @return the angle's rotation (with offset) in radians.
+     */
+    public double getAngleRadians(IMUAxis axis) {
+        return switch (axis) {
+            case Z -> getRawAngleRadians(axis) + offset.getZ();
+            case Y -> getRawAngleRadians(axis) + offset.getY();
+            case X -> getRawAngleRadians(axis) + offset.getX();
+        };
+    }
+
+    /**
+     * Gets the specified angle's rotation (with offset) in radians.
+     * @param axis the {@link SingleAxisGyroscope.Axis} to get the angle's rotation of.
+     * @return the angle's rotation (with offset) in radians.
      */
-    public abstract double getAngleRadians(SingleAxisGyroscope.Axis axis);
+    public double getAngleRadians(SingleAxisGyroscope.Axis axis) {
+        return switch (axis) {
+            case YAW -> getRawAngleRadians(axis) + offset.getZ();
+            case PITCH -> getRawAngleRadians(axis) + offset.getY();
+            case ROLL -> getRawAngleRadians(axis) + offset.getX();
+        };
+    }
 
     /**
-     * Gets the rotation of the gyroscope (with offset) in radians.
-     * @return {@link Rotation3d} of the rotation of the gyroscope (with offset) in radians.
+     * Gets the specified angle's rotation (without offset) in radians.
+     * @param axis the {@link SingleAxisGyroscope.Axis} to get the angle's rotation of.
+     * @return the angle's rotation (without offset) in radians.
+     */
+    public abstract double getRawAngleRadians(SingleAxisGyroscope.Axis axis);
+
+    /**
+     * Gets the specified angle's rotation (without offset) in radians.
+     * @param axis the {@link IMUAxis} to get the angle's rotation of.
+     * @return the angle's rotation (without offset) in radians.
+     */
+    public abstract double getRawAngleRadians(IMUAxis axis);
+
+    /**
+     * Gets the rotation of the gyroscope (with offset and inverted state) in radians.
+     * @return {@link Rotation3d} of the rotation of the gyroscope (with offset and inverted state) in radians.
      */
     public Rotation3d getRotation3dRadians() {
-        return getRawRotation3dRadians().plus(getOffset());
+        return !inverted ? getRawRotation3dRadians().minus(getOffset()) : getRawRotation3dRadians().minus(getOffset()).unaryMinus();
     }
 
     /**
-     * Gets the rotation of the gyroscope (without offset) in radians.
-     * @return {@link Rotation3d} of the rotation of the gyroscope (without offset) in radians.
+     * Gets the rotation of the gyroscope (with offset and inverted state) in radians.
+     * @return {@link Rotation3d} of the rotation of the gyroscope (with offset and inverted state) in radians.
+     * @deprecated this only exists for YAGSL, use {@link #getRotation3dRadians()} instead (since that's what this method calls internally).
+     * @see #getRotation3dRadians()
+     */
+    public Rotation3d getRawRotation3d() {
+        return getRawRotation3dRadians();
+    }
+
+    /**
+     * Gets the rotation of the gyroscope (without offset or inverted state) in radians.
+     * @return {@link Rotation3d} of the rotation of the gyroscope (without offset or inverted state) in radians.
      */
     public abstract Rotation3d getRawRotation3dRadians();
 
+    /**
+     * Gets the rotation of the gyroscope (without offset or inverted state) in radians.
+     * @return {@link Rotation3d} of the rotation of the gyroscope (without offset or inverted state) in radians.
+     * @deprecated this only exists for YAGSL, use {@link #getRotation3dRadians()} instead (since that's what this method calls internally).
+     * @see #getRotation3dRadians()
+     */
+    public Rotation3d getRotation3d() {
+        return getRotation3dRadians();
+    }
+
     /**
      * Gets the acceleration from the IMU in meters per second squared.
      * @return {@link Translation3d} of the acceleration from the IMU in meters per second squared.
      */
     public abstract Translation3d getAccelerationMetersPerSecondSquared();
 
+    /**
+     * Returns the acceleration of the IMU.
+     * @return {@link Optional <Translation3d>} of the acceleration of the IMU.
+     * @see #getAccelerationMetersPerSecondSquared()
+     * @deprecated this only exists for use by YAGSL, use {@link #getAccelerationMetersPerSecondSquared()} instead (since that's what this method calls internally).
+     */
+    public Optional<Translation3d> getAccel() {
+        return Optional.of(getAccelerationMetersPerSecondSquared());
+    }
+
+    /**
+     * Gets the acceleration from the IMU in meters per second squared on a specific axis.
+     * @param axis the {@link IMUAxis} to get the acceleration of.
+     * @return the acceleration from the IMU in meters per second squared on a specific axis.
+     */
+    public abstract double getAccelerationMetersPerSecondSquared(IMUAxis axis);
+
     /**
      * Sets the offset of the gyroscope in radians.
      * @param offset the {@link Rotation3d} of the offset of the gyroscope in radians.
@@ -55,6 +134,22 @@ public Rotation3d getOffset() {
         return offset;
     }
 
+    /**
+     * Sets the IMU's values to be inverted.
+     * @param inverted whether the IMU's values should be inverted.
+     */
+    public void setInverted(boolean inverted) {
+        this.inverted = inverted;
+    }
+
+    /**
+     * Gets whether the IMU's values are inverted.
+     * @return whether the IMU's values are inverted.
+     */
+    public boolean getInverted() {
+        return inverted;
+    }
+
     /**
      * Calibrates the gyroscope.
      */
@@ -65,6 +160,34 @@ public Rotation3d getOffset() {
      */
     public abstract void reset();
 
+    /**
+     * Resets the IMU's settings to factory defaults and clears offsets.
+     */
+    public abstract void factoryDefault();
+
+    /**
+     * Clears all sticky faults on the IMU.
+     */
+    public abstract void clearStickyFaults();
+
+    /**
+     * Returns the {@link IMUAxis} of the yaw.
+     * @return the {@link IMUAxis} of the yaw.
+     */
+    public abstract IMUAxis getYawAxis();
+
+    /**
+     * Returns the {@link IMUAxis} of the pitch.
+     * @return the {@link IMUAxis} of the pitch.
+     */
+    public abstract IMUAxis getPitchAxis();
+
+    /**
+     * Returns the {@link IMUAxis} of the roll.
+     * @return the {@link IMUAxis} of the roll.
+     */
+    public abstract IMUAxis getRollAxis();
+
     /**
      * Returns the raw gyroscope object.
      * Should only be used when you know the subclass of the gyroscope.
@@ -73,7 +196,7 @@ public Rotation3d getOffset() {
      * (e.g the {@link ADIS16448IMU} would return {@link edu.wpi.first.wpilibj.ADIS16448_IMU})
      * @return the raw gyroscope object.
      */
-    public abstract Object getRawIMU();
+    public abstract Object getIMU();
 
     /**
      * Represents the three axes of a gyroscope.

From 5722c6b74858520e1872243cd9a7568c4d219ea7 Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Mon, 7 Oct 2024 09:54:18 -0400
Subject: [PATCH 15/27] refactor(hardware/gyro/imu): move yaw pitch roll axis
 assignment to a utility method

---
 .../hardware/gyro/imu/ADIS16448IMU.java       | 30 ++++---------------
 .../librobot/hardware/gyro/imu/IMU.java       | 25 +++++++++++++++-
 2 files changed, 30 insertions(+), 25 deletions(-)

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
index 6bd119a..12e9ebc 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
@@ -48,18 +48,9 @@ public ADIS16448IMU() {
      */
     public ADIS16448IMU(@NotNull ADIS16448_IMU imu) {
         yaw = toIMUAxis(imu.getYawAxis());
-
-        //todo: confirm this is correct i had copilot do it
-        if (yaw == IMUAxis.X) {
-            pitch = IMUAxis.Y;
-            roll = IMUAxis.Z;
-        } else if (yaw == IMUAxis.Y) {
-            pitch = IMUAxis.X;
-            roll = IMUAxis.Z;
-        } else {
-            pitch = IMUAxis.X;
-            roll = IMUAxis.Y;
-        }
+        IMUAxis[] axes = IMUAxis.assignAxes(yaw);
+        pitch = axes[0];
+        roll = axes[1];
 
         this.imu = imu;
     }
@@ -91,18 +82,9 @@ public ADIS16448IMU(@NotNull IMUAxis yaw, @NotNull SPI.Port port) {
      */
     public ADIS16448IMU(@NotNull IMUAxis yaw, @NotNull SPI.Port port, @NotNull ADIS16448_IMU.CalibrationTime calibrationTime) {
         this.yaw = yaw;
-
-        //todo: confirm this is correct i had copilot do it
-        if (yaw == IMUAxis.X) {
-            pitch = IMUAxis.Y;
-            roll = IMUAxis.Z;
-        } else if (yaw == IMUAxis.Y) {
-            pitch = IMUAxis.X;
-            roll = IMUAxis.Z;
-        } else {
-            pitch = IMUAxis.X;
-            roll = IMUAxis.Y;
-        }
+        IMUAxis[] axes = IMUAxis.assignAxes(yaw);
+        pitch = axes[0];
+        roll = axes[1];
 
         imu = new ADIS16448_IMU(fromIMUAxis(yaw), port, calibrationTime);
     }
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
index 1ea3ebb..db8e86b 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
@@ -202,6 +202,29 @@ public boolean getInverted() {
      * Represents the three axes of a gyroscope.
      */
     public enum IMUAxis {
-        X, Y, Z
+        X, Y, Z;
+
+        /**
+         * A utility method used to assign the pitch and roll axes based on the yaw axis.
+         * @param yaw the yaw axis.
+         * @return an array of the pitch and roll axes, index 0 and 1 respectively.
+         */
+        public static IMUAxis[] assignAxes(IMUAxis yaw) {
+            IMUAxis pitch;
+            IMUAxis roll;
+
+            if (yaw == IMUAxis.X) {
+                pitch = IMUAxis.Y;
+                roll = IMUAxis.Z;
+            } else if (yaw == IMUAxis.Y) {
+                pitch = IMUAxis.X;
+                roll = IMUAxis.Z;
+            } else {
+                pitch = IMUAxis.X;
+                roll = IMUAxis.Y;
+            }
+
+            return new IMUAxis[]{pitch, roll};
+        }
     }
 }

From 213fecf873d7e6fde14651cac9a8ce01ad03ee14 Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Mon, 7 Oct 2024 12:41:23 -0400
Subject: [PATCH 16/27] refactor(hardware/gyro): remove SingleAxisGyroscope,
 just use IMU.

all the functionality from SingleAxisGyroscope exists in IMU, and it just complicates things
---
 .../hardware/gyro/{imu => }/ADIS16448IMU.java | 61 +++++++--------
 .../hardware/gyro/{imu => }/ADIS16470IMU.java | 75 +++++++++---------
 .../librobot/hardware/gyro/{imu => }/IMU.java | 78 ++++++++++---------
 .../gyro/single/SingleAxisGyroscope.java      | 10 ---
 4 files changed, 109 insertions(+), 115 deletions(-)
 rename src/main/java/net/frc5183/librobot/hardware/gyro/{imu => }/ADIS16448IMU.java (76%)
 rename src/main/java/net/frc5183/librobot/hardware/gyro/{imu => }/ADIS16470IMU.java (75%)
 rename src/main/java/net/frc5183/librobot/hardware/gyro/{imu => }/IMU.java (76%)
 delete mode 100644 src/main/java/net/frc5183/librobot/hardware/gyro/single/SingleAxisGyroscope.java

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java
similarity index 76%
rename from src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
rename to src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java
index 12e9ebc..7297b43 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16448IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java
@@ -1,10 +1,9 @@
-package net.frc5183.librobot.hardware.gyro.imu;
+package net.frc5183.librobot.hardware.gyro;
 
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.wpilibj.ADIS16448_IMU;
 import edu.wpi.first.wpilibj.SPI;
-import net.frc5183.librobot.hardware.gyro.single.SingleAxisGyroscope;
 import org.jetbrains.annotations.NotNull;
 
 /**
@@ -21,18 +20,18 @@ public class ADIS16448IMU extends IMU {
      * The yaw axis.
      */
     @NotNull
-    private final IMUAxis yaw;
+    private final CartesianAxis yaw;
 
     /**
      * The pitch axis.
      */
     @NotNull
-    private final IMUAxis pitch;
+    private final CartesianAxis pitch;
 
     /**
      * The roll axis.
      */
-    private final IMUAxis roll;
+    private final CartesianAxis roll;
 
     /**
      * Creates a new {@link ADIS16448IMU} using the RIO's Onboard MXP port, 500ms calibration time, and Z axis as yaw.
@@ -48,7 +47,7 @@ public ADIS16448IMU() {
      */
     public ADIS16448IMU(@NotNull ADIS16448_IMU imu) {
         yaw = toIMUAxis(imu.getYawAxis());
-        IMUAxis[] axes = IMUAxis.assignAxes(yaw);
+        CartesianAxis[] axes = CartesianAxis.assignAxes(yaw);
         pitch = axes[0];
         roll = axes[1];
 
@@ -60,7 +59,7 @@ public ADIS16448IMU(@NotNull ADIS16448_IMU imu) {
      * Uses default RIO's Onboard MXP port and 512ms calibration time.
      * @param yaw the YAW axis.
      */
-    public ADIS16448IMU(@NotNull IMUAxis yaw) {
+    public ADIS16448IMU(@NotNull CartesianAxis yaw) {
         this(yaw, SPI.Port.kMXP);
     }
 
@@ -70,7 +69,7 @@ public ADIS16448IMU(@NotNull IMUAxis yaw) {
      * @param yaw the YAW axis.
      * @param port the SPI port.
      */
-    public ADIS16448IMU(@NotNull IMUAxis yaw, @NotNull SPI.Port port) {
+    public ADIS16448IMU(@NotNull CartesianAxis yaw, @NotNull SPI.Port port) {
         this(yaw, port, ADIS16448_IMU.CalibrationTime._512ms);
     }
 
@@ -80,9 +79,9 @@ public ADIS16448IMU(@NotNull IMUAxis yaw, @NotNull SPI.Port port) {
      * @param port the SPI port.
      * @param calibrationTime the calibration time.
      */
-    public ADIS16448IMU(@NotNull IMUAxis yaw, @NotNull SPI.Port port, @NotNull ADIS16448_IMU.CalibrationTime calibrationTime) {
+    public ADIS16448IMU(@NotNull CartesianAxis yaw, @NotNull SPI.Port port, @NotNull ADIS16448_IMU.CalibrationTime calibrationTime) {
         this.yaw = yaw;
-        IMUAxis[] axes = IMUAxis.assignAxes(yaw);
+        CartesianAxis[] axes = CartesianAxis.assignAxes(yaw);
         pitch = axes[0];
         roll = axes[1];
 
@@ -90,7 +89,7 @@ public ADIS16448IMU(@NotNull IMUAxis yaw, @NotNull SPI.Port port, @NotNull ADIS1
     }
 
     @Override
-    public double getRawAngleRadians(@NotNull SingleAxisGyroscope.Axis axis) {
+    public double getRawAngleRadians(@NotNull Attitude axis) {
         // todo: this has a lot of branching but im not sure if there's really a better way to do it
         return switch (axis) {
             case YAW ->
@@ -115,7 +114,7 @@ public double getRawAngleRadians(@NotNull SingleAxisGyroscope.Axis axis) {
     }
 
     @Override
-    public double getRawAngleRadians(IMUAxis axis) {
+    public double getRawAngleRadians(CartesianAxis axis) {
         return switch (axis) {
             case X -> imu.getGyroAngleX();
             case Y -> imu.getGyroAngleY();
@@ -130,23 +129,23 @@ public double getRawAngleRadians(IMUAxis axis) {
         double yawRadians;
 
         // todo: again, not too sure if there's a better way
-        if (this.roll == IMUAxis.X)
+        if (this.roll == CartesianAxis.X)
             rollRadians = imu.getGyroAngleX();
-        else if (this.roll == IMUAxis.Y)
+        else if (this.roll == CartesianAxis.Y)
             rollRadians = imu.getGyroAngleY();
         else
             rollRadians = imu.getGyroAngleZ();
 
-        if (this.pitch == IMUAxis.X)
+        if (this.pitch == CartesianAxis.X)
             pitchRadians = imu.getGyroAngleX();
-        else if (this.pitch == IMUAxis.Y)
+        else if (this.pitch == CartesianAxis.Y)
             pitchRadians = imu.getGyroAngleY();
         else
             pitchRadians = imu.getGyroAngleZ();
 
-        if (this.yaw == IMUAxis.X)
+        if (this.yaw == CartesianAxis.X)
             yawRadians = imu.getGyroAngleX();
-        else if (this.yaw == IMUAxis.Y)
+        else if (this.yaw == CartesianAxis.Y)
             yawRadians = imu.getGyroAngleY();
         else
             yawRadians = imu.getGyroAngleZ();
@@ -160,7 +159,7 @@ else if (this.yaw == IMUAxis.Y)
     }
 
     @Override
-    public double getAccelerationMetersPerSecondSquared(IMUAxis axis) {
+    public double getAccelerationMetersPerSecondSquared(CartesianAxis axis) {
         return switch (axis) {
             case X -> imu.getAccelX();
             case Y -> imu.getAccelY();
@@ -190,17 +189,17 @@ public void clearStickyFaults() {
     }
 
     @Override
-    public @NotNull IMUAxis getYawAxis() {
+    public @NotNull CartesianAxis getYawAxis() {
         return yaw;
     }
 
     @Override
-    public @NotNull IMUAxis getPitchAxis() {
+    public @NotNull CartesianAxis getPitchAxis() {
         return pitch;
     }
 
     @Override
-    public IMUAxis getRollAxis() {
+    public CartesianAxis getRollAxis() {
         return roll;
     }
 
@@ -210,12 +209,12 @@ public IMUAxis getRollAxis() {
     }
 
     /**
-     * Returns a new {@link ADIS16448_IMU.IMUAxis} from an {@link IMUAxis}.
-     * @param axis the {@link IMUAxis} to convert.
+     * Returns a new {@link ADIS16448_IMU.IMUAxis} from an {@link CartesianAxis}.
+     * @param axis the {@link CartesianAxis} to convert.
      * @return the converted {@link ADIS16448_IMU.IMUAxis}.
      */
     @NotNull
-    public static ADIS16448_IMU.IMUAxis fromIMUAxis(@NotNull IMUAxis axis) {
+    public static ADIS16448_IMU.IMUAxis fromIMUAxis(@NotNull CartesianAxis axis) {
         return switch (axis) {
             case X -> ADIS16448_IMU.IMUAxis.kX;
             case Y -> ADIS16448_IMU.IMUAxis.kY;
@@ -225,16 +224,16 @@ public static ADIS16448_IMU.IMUAxis fromIMUAxis(@NotNull IMUAxis axis) {
 
 
     /**
-     * Returns a new {@link IMUAxis} from an {@link ADIS16448_IMU.IMUAxis}.
+     * Returns a new {@link CartesianAxis} from an {@link ADIS16448_IMU.IMUAxis}.
      * @param axis the {@link ADIS16448_IMU.IMUAxis} to convert.
-     * @return the converted {@link IMUAxis}.
+     * @return the converted {@link CartesianAxis}.
      */
     @NotNull
-    public static IMUAxis toIMUAxis(@NotNull ADIS16448_IMU.IMUAxis axis) {
+    public static CartesianAxis toIMUAxis(@NotNull ADIS16448_IMU.IMUAxis axis) {
         return switch (axis) {
-            case kX -> IMUAxis.X;
-            case kY -> IMUAxis.Y;
-            case kZ -> IMUAxis.Z;
+            case kX -> CartesianAxis.X;
+            case kY -> CartesianAxis.Y;
+            case kZ -> CartesianAxis.Z;
         };
     }
 }
\ No newline at end of file
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java
similarity index 75%
rename from src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
rename to src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java
index f395bc7..a0eecdc 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/ADIS16470IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java
@@ -1,10 +1,9 @@
-package net.frc5183.librobot.hardware.gyro.imu;
+package net.frc5183.librobot.hardware.gyro;
 
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.wpilibj.ADIS16470_IMU;
 import edu.wpi.first.wpilibj.SPI;
-import net.frc5183.librobot.hardware.gyro.single.SingleAxisGyroscope;
 import org.jetbrains.annotations.NotNull;
 
 /**
@@ -20,17 +19,17 @@ public class ADIS16470IMU extends IMU {
     /**
      * The YAW axis.
      */
-    private final IMUAxis yaw;
+    private final CartesianAxis yaw;
 
     /**
      * The PITCH axis.
      */
-    private final IMUAxis pitch;
+    private final CartesianAxis pitch;
 
     /**
      * The ROLL axis.
      */
-    private final IMUAxis roll;
+    private final CartesianAxis roll;
 
     /**
      * Creates a new {@link ADIS16470IMU} using the RIO's Onboard MXP port and 4s calibration time, and yaw pitch roll as ZXY respectively.
@@ -60,9 +59,9 @@ public ADIS16470IMU(@NotNull ADIS16470_IMU imu) {
      * @param roll the ROLL axis.
      */
     public ADIS16470IMU(
-            @NotNull IMUAxis yaw,
-            @NotNull IMUAxis pitch,
-            @NotNull IMUAxis roll) {
+            @NotNull CartesianAxis yaw,
+            @NotNull CartesianAxis pitch,
+            @NotNull CartesianAxis roll) {
         this(yaw, pitch, roll, SPI.Port.kMXP);
     }
 
@@ -75,9 +74,9 @@ public ADIS16470IMU(
      * @param port the SPI port.
      */
     public ADIS16470IMU(
-            @NotNull IMUAxis yaw,
-            @NotNull IMUAxis pitch,
-            @NotNull IMUAxis roll,
+            @NotNull CartesianAxis yaw,
+            @NotNull CartesianAxis pitch,
+            @NotNull CartesianAxis roll,
             @NotNull SPI.Port port
     ) {
         this(yaw, pitch, roll, port, ADIS16470_IMU.CalibrationTime._4s);
@@ -92,9 +91,9 @@ public ADIS16470IMU(
      * @param calibrationTime the calibration time.
      */
     public ADIS16470IMU(
-            @NotNull IMUAxis yaw,
-            @NotNull IMUAxis pitch,
-            @NotNull IMUAxis roll,
+            @NotNull CartesianAxis yaw,
+            @NotNull CartesianAxis pitch,
+            @NotNull CartesianAxis roll,
             @NotNull SPI.Port port,
             @NotNull ADIS16470_IMU.CalibrationTime calibrationTime
     ) {
@@ -112,12 +111,12 @@ public ADIS16470IMU(
     }
 
     @Override
-    public double getRawAngleRadians(@NotNull SingleAxisGyroscope.Axis axis) {
+    public double getRawAngleRadians(@NotNull Attitude axis) {
         return imu.getAngle(fromSingleAxis(axis));
     }
 
     @Override
-    public double getRawAngleRadians(@NotNull IMUAxis axis) {
+    public double getRawAngleRadians(@NotNull CartesianAxis axis) {
         return imu.getAngle(fromIMUAxis(axis));
     }
 
@@ -136,7 +135,7 @@ public double getRawAngleRadians(@NotNull IMUAxis axis) {
     }
 
     @Override
-    public double getAccelerationMetersPerSecondSquared(IMUAxis axis) {
+    public double getAccelerationMetersPerSecondSquared(CartesianAxis axis) {
         return switch (axis) {
             case X -> imu.getAccelX();
             case Y -> imu.getAccelY();
@@ -166,17 +165,17 @@ public void clearStickyFaults() {
     }
 
     @Override
-    public IMUAxis getYawAxis() {
+    public CartesianAxis getYawAxis() {
         return yaw;
     }
 
     @Override
-    public IMUAxis getPitchAxis() {
+    public CartesianAxis getPitchAxis() {
         return pitch;
     }
 
     @Override
-    public IMUAxis getRollAxis() {
+    public CartesianAxis getRollAxis() {
         return roll;
     }
 
@@ -186,12 +185,12 @@ public IMUAxis getRollAxis() {
     }
 
     /**
-     * Returns a new {@link ADIS16470_IMU.IMUAxis} from an {@link IMUAxis}.
-     * @param axis the {@link IMUAxis} to convert.
+     * Returns a new {@link ADIS16470_IMU.IMUAxis} from an {@link CartesianAxis}.
+     * @param axis the {@link CartesianAxis} to convert.
      * @return the converted {@link ADIS16470_IMU.IMUAxis}.
      */
     @NotNull
-    public static ADIS16470_IMU.IMUAxis fromIMUAxis(@NotNull IMUAxis axis) {
+    public static ADIS16470_IMU.IMUAxis fromIMUAxis(@NotNull CartesianAxis axis) {
         return switch (axis) {
             case X -> ADIS16470_IMU.IMUAxis.kX;
             case Y -> ADIS16470_IMU.IMUAxis.kY;
@@ -200,12 +199,12 @@ public static ADIS16470_IMU.IMUAxis fromIMUAxis(@NotNull IMUAxis axis) {
     }
 
     /**
-     * Returns a new {@link ADIS16470_IMU.IMUAxis} from an {@link SingleAxisGyroscope.Axis}
-     * @param axis the {@link SingleAxisGyroscope.Axis} to convert.
+     * Returns a new {@link ADIS16470_IMU.IMUAxis} from an {@link Attitude}
+     * @param axis the {@link Attitude} to convert.
      * @return the converted {@link ADIS16470_IMU.IMUAxis}
      */
     @NotNull
-    public static ADIS16470_IMU.IMUAxis fromSingleAxis(@NotNull SingleAxisGyroscope.Axis axis) {
+    public static ADIS16470_IMU.IMUAxis fromSingleAxis(@NotNull Attitude axis) {
         return switch (axis) {
             case YAW -> ADIS16470_IMU.IMUAxis.kYaw;
             case PITCH -> ADIS16470_IMU.IMUAxis.kPitch;
@@ -214,31 +213,31 @@ public static ADIS16470_IMU.IMUAxis fromSingleAxis(@NotNull SingleAxisGyroscope.
     }
 
     /**
-     * Returns a new {@link IMUAxis} from an {@link ADIS16470_IMU.IMUAxis}.
+     * Returns a new {@link CartesianAxis} from an {@link ADIS16470_IMU.IMUAxis}.
      * @param axis the {@link ADIS16470_IMU.IMUAxis} to convert.
-     * @return the converted {@link IMUAxis}.
+     * @return the converted {@link CartesianAxis}.
      * @throws IllegalArgumentException if {@code axis} is not one of kX, kY, or kZ.
      */
-    public static IMUAxis toIMUAxis(@NotNull ADIS16470_IMU.IMUAxis axis) {
+    public static CartesianAxis toIMUAxis(@NotNull ADIS16470_IMU.IMUAxis axis) {
         return switch (axis) {
-            case kX -> IMUAxis.X;
-            case kY -> IMUAxis.Y;
-            case kZ -> IMUAxis.Z;
+            case kX -> CartesianAxis.X;
+            case kY -> CartesianAxis.Y;
+            case kZ -> CartesianAxis.Z;
             default -> throw new IllegalArgumentException("ADIS16470_IMU Axis must be one of kX, kY, or kZ. Was: " + axis);
         };
     }
 
     /**
-     * Returns a new {@link SingleAxisGyroscope.Axis} from an {@link ADIS16470_IMU.IMUAxis}.
+     * Returns a new {@link Attitude} from an {@link ADIS16470_IMU.IMUAxis}.
      * @param axis the {@link ADIS16470_IMU.IMUAxis} to convert.
-     * @return the converted {@link SingleAxisGyroscope.Axis}.
+     * @return the converted {@link Attitude}.
      * @throws IllegalArgumentException if {@code axis} is not one of kYaw, kPitch, or kRoll.
      */
-    public static SingleAxisGyroscope.Axis toSingleAxis(@NotNull ADIS16470_IMU.IMUAxis axis) {
+    public static Attitude toSingleAxis(@NotNull ADIS16470_IMU.IMUAxis axis) {
         return switch (axis) {
-            case kYaw -> SingleAxisGyroscope.Axis.YAW;
-            case kPitch -> SingleAxisGyroscope.Axis.PITCH;
-            case kRoll -> SingleAxisGyroscope.Axis.ROLL;
+            case kYaw -> Attitude.YAW;
+            case kPitch -> Attitude.PITCH;
+            case kRoll -> Attitude.ROLL;
             default -> throw new IllegalArgumentException("ADIS16470_IMU Axis must be one of kYaw, kPitch, or kRoll. Was" + axis);
         };
     }
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java
similarity index 76%
rename from src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
rename to src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java
index db8e86b..c1153ca 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/imu/IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java
@@ -1,8 +1,7 @@
-package net.frc5183.librobot.hardware.gyro.imu;
+package net.frc5183.librobot.hardware.gyro;
 
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
-import net.frc5183.librobot.hardware.gyro.single.SingleAxisGyroscope;
 import swervelib.imu.SwerveIMU;
 
 import java.util.Optional;
@@ -23,10 +22,10 @@ public abstract class IMU extends SwerveIMU {
 
     /**
      * Gets the specified angle's rotation (with offset) in radians.
-     * @param axis the {@link IMUAxis} to get the angle's rotation of.
+     * @param axis the {@link CartesianAxis} to get the angle's rotation of.
      * @return the angle's rotation (with offset) in radians.
      */
-    public double getAngleRadians(IMUAxis axis) {
+    public double getAngleRadians(CartesianAxis axis) {
         return switch (axis) {
             case Z -> getRawAngleRadians(axis) + offset.getZ();
             case Y -> getRawAngleRadians(axis) + offset.getY();
@@ -36,10 +35,10 @@ public double getAngleRadians(IMUAxis axis) {
 
     /**
      * Gets the specified angle's rotation (with offset) in radians.
-     * @param axis the {@link SingleAxisGyroscope.Axis} to get the angle's rotation of.
+     * @param axis the {@link Attitude} to get the angle's rotation of.
      * @return the angle's rotation (with offset) in radians.
      */
-    public double getAngleRadians(SingleAxisGyroscope.Axis axis) {
+    public double getAngleRadians(Attitude axis) {
         return switch (axis) {
             case YAW -> getRawAngleRadians(axis) + offset.getZ();
             case PITCH -> getRawAngleRadians(axis) + offset.getY();
@@ -49,17 +48,17 @@ public double getAngleRadians(SingleAxisGyroscope.Axis axis) {
 
     /**
      * Gets the specified angle's rotation (without offset) in radians.
-     * @param axis the {@link SingleAxisGyroscope.Axis} to get the angle's rotation of.
+     * @param axis the {@link Attitude} to get the angle's rotation of.
      * @return the angle's rotation (without offset) in radians.
      */
-    public abstract double getRawAngleRadians(SingleAxisGyroscope.Axis axis);
+    public abstract double getRawAngleRadians(Attitude axis);
 
     /**
      * Gets the specified angle's rotation (without offset) in radians.
-     * @param axis the {@link IMUAxis} to get the angle's rotation of.
+     * @param axis the {@link CartesianAxis} to get the angle's rotation of.
      * @return the angle's rotation (without offset) in radians.
      */
-    public abstract double getRawAngleRadians(IMUAxis axis);
+    public abstract double getRawAngleRadians(CartesianAxis axis);
 
     /**
      * Gets the rotation of the gyroscope (with offset and inverted state) in radians.
@@ -113,10 +112,10 @@ public Optional<Translation3d> getAccel() {
 
     /**
      * Gets the acceleration from the IMU in meters per second squared on a specific axis.
-     * @param axis the {@link IMUAxis} to get the acceleration of.
+     * @param axis the {@link CartesianAxis} to get the acceleration of.
      * @return the acceleration from the IMU in meters per second squared on a specific axis.
      */
-    public abstract double getAccelerationMetersPerSecondSquared(IMUAxis axis);
+    public abstract double getAccelerationMetersPerSecondSquared(CartesianAxis axis);
 
     /**
      * Sets the offset of the gyroscope in radians.
@@ -171,22 +170,22 @@ public boolean getInverted() {
     public abstract void clearStickyFaults();
 
     /**
-     * Returns the {@link IMUAxis} of the yaw.
-     * @return the {@link IMUAxis} of the yaw.
+     * Returns the {@link CartesianAxis} of the yaw.
+     * @return the {@link CartesianAxis} of the yaw.
      */
-    public abstract IMUAxis getYawAxis();
+    public abstract CartesianAxis getYawAxis();
 
     /**
-     * Returns the {@link IMUAxis} of the pitch.
-     * @return the {@link IMUAxis} of the pitch.
+     * Returns the {@link CartesianAxis} of the pitch.
+     * @return the {@link CartesianAxis} of the pitch.
      */
-    public abstract IMUAxis getPitchAxis();
+    public abstract CartesianAxis getPitchAxis();
 
     /**
-     * Returns the {@link IMUAxis} of the roll.
-     * @return the {@link IMUAxis} of the roll.
+     * Returns the {@link CartesianAxis} of the roll.
+     * @return the {@link CartesianAxis} of the roll.
      */
-    public abstract IMUAxis getRollAxis();
+    public abstract CartesianAxis getRollAxis();
 
     /**
      * Returns the raw gyroscope object.
@@ -199,9 +198,9 @@ public boolean getInverted() {
     public abstract Object getIMU();
 
     /**
-     * Represents the three axes of a gyroscope.
+     * Represents the cartesian axes of a gyroscope (xyz).
      */
-    public enum IMUAxis {
+    public enum CartesianAxis {
         X, Y, Z;
 
         /**
@@ -209,22 +208,29 @@ public enum IMUAxis {
          * @param yaw the yaw axis.
          * @return an array of the pitch and roll axes, index 0 and 1 respectively.
          */
-        public static IMUAxis[] assignAxes(IMUAxis yaw) {
-            IMUAxis pitch;
-            IMUAxis roll;
-
-            if (yaw == IMUAxis.X) {
-                pitch = IMUAxis.Y;
-                roll = IMUAxis.Z;
-            } else if (yaw == IMUAxis.Y) {
-                pitch = IMUAxis.X;
-                roll = IMUAxis.Z;
+        public static CartesianAxis[] assignAxes(CartesianAxis yaw) {
+            CartesianAxis pitch;
+            CartesianAxis roll;
+
+            if (yaw == CartesianAxis.X) {
+                pitch = CartesianAxis.Y;
+                roll = CartesianAxis.Z;
+            } else if (yaw == CartesianAxis.Y) {
+                pitch = CartesianAxis.X;
+                roll = CartesianAxis.Z;
             } else {
-                pitch = IMUAxis.X;
-                roll = IMUAxis.Y;
+                pitch = CartesianAxis.X;
+                roll = CartesianAxis.Y;
             }
 
-            return new IMUAxis[]{pitch, roll};
+            return new CartesianAxis[]{pitch, roll};
         }
     }
+
+    /**
+     * Represents the attitude of a gyroscope (yaw, pitch, roll).
+     */
+    public enum Attitude {
+        YAW, PITCH, ROLL
+    }
 }
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/single/SingleAxisGyroscope.java b/src/main/java/net/frc5183/librobot/hardware/gyro/single/SingleAxisGyroscope.java
deleted file mode 100644
index 763034b..0000000
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/single/SingleAxisGyroscope.java
+++ /dev/null
@@ -1,10 +0,0 @@
-package net.frc5183.librobot.hardware.gyro.single;
-
-import edu.wpi.first.math.geometry.Rotation2d;
-
-public abstract class SingleAxisGyroscope {
-    // todo
-    public enum Axis {
-        YAW, PITCH, ROLL
-    }
-}

From 898776ef628306ee2ba1615c1d335c99e4dd2101 Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Mon, 7 Oct 2024 12:58:07 -0400
Subject: [PATCH 17/27] feat(hardware/gyro): implement KauaiLabs NavX

---
 .../librobot/hardware/gyro/NavXIMU.java       | 204 ++++++++++++++++++
 1 file changed, 204 insertions(+)
 create mode 100644 src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java
new file mode 100644
index 0000000..31f40ef
--- /dev/null
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java
@@ -0,0 +1,204 @@
+package net.frc5183.librobot.hardware.gyro;
+
+import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.SPI;
+import edu.wpi.first.wpilibj.SerialPort;
+import org.jetbrains.annotations.NotNull;
+
+/**
+ * Represents a NavX {@link IMU}.
+ */
+public class NavXIMU extends IMU {
+    /**
+     * The NavX IMU.
+     */
+    @NotNull
+    private final AHRS imu;
+
+    /**
+     * The yaw axis.
+     */
+    @NotNull
+    private final CartesianAxis yaw;
+
+    /**
+     * The pitch axis.
+     */
+    @NotNull
+    private final CartesianAxis pitch;
+
+    /**
+     * The roll axis.
+     */
+    @NotNull
+    private final CartesianAxis roll;
+
+    /**
+     * Creates a new {@link NavXIMU} using the RIO's Onboard MXP port and default update rate.
+     * @see AHRS#AHRS()
+     */
+    public NavXIMU() {
+        this(new AHRS());
+    }
+
+    /**
+     * Creates a new {@link NavXIMU} from an existing NavX IMU instance.
+     * @param imu the NavX IMU to use.
+     */
+    public NavXIMU(@NotNull AHRS imu) {
+        this.imu = imu;
+
+        yaw = toCartesian(imu.getBoardYawAxis().board_axis);
+
+        CartesianAxis[] axes = CartesianAxis.assignAxes(yaw);
+        pitch = axes[0];
+        roll = axes[1];
+    }
+
+    /**
+     * Creates a new {@link NavXIMU} with a specified SPI port and default update rate.
+     * @param spiPort the SPI port to use.
+     * @see AHRS#AHRS(SPI.Port)
+     */
+    public NavXIMU(@NotNull SPI.Port spiPort) {
+        this(new AHRS(spiPort));
+    }
+
+    /**
+     * Creates a new {@link NavXIMU} with a specified I2C port and default update rate.
+     * @param i2cPort the I2C port to use.
+     * @see AHRS#AHRS(I2C.Port)
+     */
+    public NavXIMU(@NotNull I2C.Port i2cPort) {
+        this(new AHRS(i2cPort));
+    }
+
+    /**
+     * Creates a new {@link NavXIMU} with a specified Serial port and default update rate.
+     * @param serialPort the Serial port to use.
+     * @see AHRS#AHRS(SerialPort.Port)
+     */
+    public NavXIMU(@NotNull SerialPort.Port serialPort) {
+        this(new AHRS(serialPort));
+    }
+
+
+    @Override
+    public double getRawAngleRadians(@NotNull Attitude axis) {
+        return switch (axis) {
+            case YAW -> imu.getYaw();
+            case PITCH -> imu.getPitch();
+            case ROLL -> imu.getRoll();
+        };
+    }
+
+    @Override
+    public double getRawAngleRadians(@NotNull CartesianAxis axis) {
+        return switch (axis) {
+            case Z -> imu.getYaw();
+            case Y -> imu.getPitch();
+            case X -> imu.getRoll();
+        };
+    }
+
+    @Override
+    public @NotNull Rotation3d getRawRotation3dRadians() {
+        return new Rotation3d(
+                Math.toRadians(imu.getRoll()),
+                Math.toRadians(imu.getPitch()),
+                Math.toRadians(imu.getYaw())
+        );
+    }
+
+    @Override
+    public @NotNull Translation3d getAccelerationMetersPerSecondSquared() {
+        return new Translation3d(
+                imu.getWorldLinearAccelX(),
+                imu.getWorldLinearAccelY(),
+                imu.getWorldLinearAccelZ()
+        );
+    }
+
+    @Override
+    public double getAccelerationMetersPerSecondSquared(@NotNull CartesianAxis axis) {
+        return switch (axis) {
+            case X -> imu.getWorldLinearAccelX();
+            case Y -> imu.getWorldLinearAccelY();
+            case Z -> imu.getWorldLinearAccelZ();
+        };
+    }
+
+    @Override
+    public void calibrate() {
+        imu.reset();
+    }
+
+    @Override
+    public void reset() {
+        imu.reset();
+    }
+
+    @Override
+    public void factoryDefault() {
+        imu.reset();
+        super.setOffset(new Rotation3d());
+    }
+
+    @Override
+    public void clearStickyFaults() {
+        // The NavX IMU does not have a method for clearing sticky faults.
+    }
+
+    @Override
+    public @NotNull CartesianAxis getYawAxis() {
+        return yaw;
+    }
+
+    @Override
+    public @NotNull CartesianAxis getPitchAxis() {
+        return pitch;
+    }
+
+    @Override
+    public @NotNull CartesianAxis getRollAxis() {
+        return roll;
+    }
+
+    @Override
+    public @NotNull AHRS getIMU() {
+        return imu;
+    }
+
+
+    /**
+     * Returns a new {@link CartesianAxis} from an {@link AHRS.BoardYawAxis}.
+     * @param axis the {@link AHRS.BoardAxis} to convert.
+     * @return the converted {@link CartesianAxis}.
+     */
+    @NotNull
+    public static CartesianAxis toCartesian(@NotNull AHRS.BoardAxis axis) {
+        return switch (axis) {
+            case kBoardAxisX -> CartesianAxis.X;
+            case kBoardAxisY -> CartesianAxis.Y;
+            case kBoardAxisZ -> CartesianAxis.Z;
+        };
+    }
+
+    /**
+     * Returns a new {@link AHRS.BoardAxis} from an {@link CartesianAxis}.
+     * @param axis the {@link CartesianAxis} to convert.
+     * @return the converted {@link AHRS.BoardAxis}.
+     */
+    @NotNull
+    public static AHRS.BoardAxis fromCartesian(@NotNull CartesianAxis axis) {
+        return switch (axis) {
+            case X -> AHRS.BoardAxis.kBoardAxisX;
+            case Y -> AHRS.BoardAxis.kBoardAxisY;
+            case Z -> AHRS.BoardAxis.kBoardAxisZ;
+        };
+    }
+}
+

From 64eb036cd67a926e938b81e1ceaf39fd29d105b0 Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Mon, 7 Oct 2024 12:59:01 -0400
Subject: [PATCH 18/27] docs(hardware/gyro/adis16*): update documentation to
 reflect changes

Changes reflected:
IMUAxis -> CartesianAxis
SingleAxisGyroscope -> Attitude
---
 .../librobot/hardware/gyro/ADIS16448IMU.java  | 10 +++----
 .../librobot/hardware/gyro/ADIS16470IMU.java  | 30 +++++++++----------
 2 files changed, 20 insertions(+), 20 deletions(-)

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java
index 7297b43..7168184 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java
@@ -31,6 +31,7 @@ public class ADIS16448IMU extends IMU {
     /**
      * The roll axis.
      */
+    @NotNull
     private final CartesianAxis roll;
 
     /**
@@ -46,7 +47,7 @@ public ADIS16448IMU() {
      * @param imu the ADIS16448 IMU to use.
      */
     public ADIS16448IMU(@NotNull ADIS16448_IMU imu) {
-        yaw = toIMUAxis(imu.getYawAxis());
+        yaw = toCartesian(imu.getYawAxis());
         CartesianAxis[] axes = CartesianAxis.assignAxes(yaw);
         pitch = axes[0];
         roll = axes[1];
@@ -85,7 +86,7 @@ public ADIS16448IMU(@NotNull CartesianAxis yaw, @NotNull SPI.Port port, @NotNull
         pitch = axes[0];
         roll = axes[1];
 
-        imu = new ADIS16448_IMU(fromIMUAxis(yaw), port, calibrationTime);
+        imu = new ADIS16448_IMU(fromCartesian(yaw), port, calibrationTime);
     }
 
     @Override
@@ -214,7 +215,7 @@ public CartesianAxis getRollAxis() {
      * @return the converted {@link ADIS16448_IMU.IMUAxis}.
      */
     @NotNull
-    public static ADIS16448_IMU.IMUAxis fromIMUAxis(@NotNull CartesianAxis axis) {
+    public static ADIS16448_IMU.IMUAxis fromCartesian(@NotNull CartesianAxis axis) {
         return switch (axis) {
             case X -> ADIS16448_IMU.IMUAxis.kX;
             case Y -> ADIS16448_IMU.IMUAxis.kY;
@@ -222,14 +223,13 @@ public static ADIS16448_IMU.IMUAxis fromIMUAxis(@NotNull CartesianAxis axis) {
         };
     }
 
-
     /**
      * Returns a new {@link CartesianAxis} from an {@link ADIS16448_IMU.IMUAxis}.
      * @param axis the {@link ADIS16448_IMU.IMUAxis} to convert.
      * @return the converted {@link CartesianAxis}.
      */
     @NotNull
-    public static CartesianAxis toIMUAxis(@NotNull ADIS16448_IMU.IMUAxis axis) {
+    public static CartesianAxis toCartesian(@NotNull ADIS16448_IMU.IMUAxis axis) {
         return switch (axis) {
             case kX -> CartesianAxis.X;
             case kY -> CartesianAxis.Y;
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java
index a0eecdc..f5b3a3f 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java
@@ -46,9 +46,9 @@ public ADIS16470IMU() {
     public ADIS16470IMU(@NotNull ADIS16470_IMU imu) {
         this.imu = imu;
 
-        yaw = toIMUAxis(imu.getYawAxis());
-        pitch = toIMUAxis(imu.getPitchAxis());
-        roll = toIMUAxis(imu.getRollAxis());
+        yaw = toCartesian(imu.getYawAxis());
+        pitch = toCartesian(imu.getPitchAxis());
+        roll = toCartesian(imu.getRollAxis());
     }
 
     /**
@@ -98,9 +98,9 @@ public ADIS16470IMU(
             @NotNull ADIS16470_IMU.CalibrationTime calibrationTime
     ) {
         imu = new ADIS16470_IMU(
-                fromIMUAxis(yaw),
-                fromIMUAxis(pitch),
-                fromIMUAxis(roll),
+                fromCartesian(yaw),
+                fromCartesian(pitch),
+                fromCartesian(roll),
                 port,
                 calibrationTime
         );
@@ -112,20 +112,20 @@ public ADIS16470IMU(
 
     @Override
     public double getRawAngleRadians(@NotNull Attitude axis) {
-        return imu.getAngle(fromSingleAxis(axis));
+        return imu.getAngle(fromAttitude(axis));
     }
 
     @Override
     public double getRawAngleRadians(@NotNull CartesianAxis axis) {
-        return imu.getAngle(fromIMUAxis(axis));
+        return imu.getAngle(fromCartesian(axis));
     }
 
     @Override
     public @NotNull Rotation3d getRawRotation3dRadians() {
         return new Rotation3d(
-                Math.toRadians(imu.getAngle(fromIMUAxis(this.roll))),
-                Math.toRadians(imu.getAngle(fromIMUAxis(this.pitch))),
-                Math.toRadians(imu.getAngle(fromIMUAxis(this.yaw)))
+                Math.toRadians(imu.getAngle(fromCartesian(this.roll))),
+                Math.toRadians(imu.getAngle(fromCartesian(this.pitch))),
+                Math.toRadians(imu.getAngle(fromCartesian(this.yaw)))
         );
     }
 
@@ -190,7 +190,7 @@ public CartesianAxis getRollAxis() {
      * @return the converted {@link ADIS16470_IMU.IMUAxis}.
      */
     @NotNull
-    public static ADIS16470_IMU.IMUAxis fromIMUAxis(@NotNull CartesianAxis axis) {
+    public static ADIS16470_IMU.IMUAxis fromCartesian(@NotNull CartesianAxis axis) {
         return switch (axis) {
             case X -> ADIS16470_IMU.IMUAxis.kX;
             case Y -> ADIS16470_IMU.IMUAxis.kY;
@@ -204,7 +204,7 @@ public static ADIS16470_IMU.IMUAxis fromIMUAxis(@NotNull CartesianAxis axis) {
      * @return the converted {@link ADIS16470_IMU.IMUAxis}
      */
     @NotNull
-    public static ADIS16470_IMU.IMUAxis fromSingleAxis(@NotNull Attitude axis) {
+    public static ADIS16470_IMU.IMUAxis fromAttitude(@NotNull Attitude axis) {
         return switch (axis) {
             case YAW -> ADIS16470_IMU.IMUAxis.kYaw;
             case PITCH -> ADIS16470_IMU.IMUAxis.kPitch;
@@ -218,7 +218,7 @@ public static ADIS16470_IMU.IMUAxis fromSingleAxis(@NotNull Attitude axis) {
      * @return the converted {@link CartesianAxis}.
      * @throws IllegalArgumentException if {@code axis} is not one of kX, kY, or kZ.
      */
-    public static CartesianAxis toIMUAxis(@NotNull ADIS16470_IMU.IMUAxis axis) {
+    public static CartesianAxis toCartesian(@NotNull ADIS16470_IMU.IMUAxis axis) {
         return switch (axis) {
             case kX -> CartesianAxis.X;
             case kY -> CartesianAxis.Y;
@@ -233,7 +233,7 @@ public static CartesianAxis toIMUAxis(@NotNull ADIS16470_IMU.IMUAxis axis) {
      * @return the converted {@link Attitude}.
      * @throws IllegalArgumentException if {@code axis} is not one of kYaw, kPitch, or kRoll.
      */
-    public static Attitude toSingleAxis(@NotNull ADIS16470_IMU.IMUAxis axis) {
+    public static Attitude toAttitude(@NotNull ADIS16470_IMU.IMUAxis axis) {
         return switch (axis) {
             case kYaw -> Attitude.YAW;
             case kPitch -> Attitude.PITCH;

From 9ba483434da221dfec73f9865afd9221697f3f8c Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Mon, 7 Oct 2024 13:08:44 -0400
Subject: [PATCH 19/27] style(hardware/gyro/imu): remove unnecessary qualifiers
 in IMU.CartesianAxis.assignAxes(CartesianAxis yaw)

---
 .../net/frc5183/librobot/hardware/gyro/IMU.java  | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java
index c1153ca..04829a2 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java
@@ -212,15 +212,15 @@ public static CartesianAxis[] assignAxes(CartesianAxis yaw) {
             CartesianAxis pitch;
             CartesianAxis roll;
 
-            if (yaw == CartesianAxis.X) {
-                pitch = CartesianAxis.Y;
-                roll = CartesianAxis.Z;
-            } else if (yaw == CartesianAxis.Y) {
-                pitch = CartesianAxis.X;
-                roll = CartesianAxis.Z;
+            if (yaw == X) {
+                pitch = Y;
+                roll = Z;
+            } else if (yaw == Y) {
+                pitch = X;
+                roll = Z;
             } else {
-                pitch = CartesianAxis.X;
-                roll = CartesianAxis.Y;
+                pitch = X;
+                roll = Y;
             }
 
             return new CartesianAxis[]{pitch, roll};

From 7108bbcf18699611e7f7774d133e4d298170c7ac Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Mon, 7 Oct 2024 13:18:15 -0400
Subject: [PATCH 20/27] refactor(hardware/gyro): clean up some of the long
 methods

---
 .../librobot/hardware/gyro/ADIS16448IMU.java  | 58 ++++---------------
 .../librobot/hardware/gyro/ADIS16470IMU.java  |  6 +-
 .../librobot/hardware/gyro/NavXIMU.java       | 22 ++++---
 3 files changed, 28 insertions(+), 58 deletions(-)

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java
index 7168184..64afdf5 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java
@@ -91,27 +91,13 @@ public ADIS16448IMU(@NotNull CartesianAxis yaw, @NotNull SPI.Port port, @NotNull
 
     @Override
     public double getRawAngleRadians(@NotNull Attitude axis) {
-        // todo: this has a lot of branching but im not sure if there's really a better way to do it
-        return switch (axis) {
-            case YAW ->
-                    switch (this.yaw) {
-                        case X -> imu.getGyroAngleX();
-                        case Y -> imu.getGyroAngleY();
-                        case Z -> imu.getGyroAngleZ();
-                    };
-            case PITCH ->
-                    switch (this.pitch) {
-                        case X -> imu.getGyroAngleX();
-                        case Y -> imu.getGyroAngleY();
-                        case Z -> imu.getGyroAngleZ();
-                    };
-            case ROLL ->
-                    switch (this.roll) {
-                        case X -> imu.getGyroAngleX();
-                        case Y -> imu.getGyroAngleY();
-                        case Z -> imu.getGyroAngleZ();
-                    };
+        CartesianAxis cartesianAxis = switch (axis) {
+            case YAW -> yaw;
+            case PITCH -> pitch;
+            case ROLL -> roll;
         };
+
+        return getRawAngleRadians(cartesianAxis);
     }
 
     @Override
@@ -125,33 +111,11 @@ public double getRawAngleRadians(CartesianAxis axis) {
 
     @Override
     public @NotNull Rotation3d getRawRotation3dRadians() {
-        double rollRadians;
-        double pitchRadians;
-        double yawRadians;
-
-        // todo: again, not too sure if there's a better way
-        if (this.roll == CartesianAxis.X)
-            rollRadians = imu.getGyroAngleX();
-        else if (this.roll == CartesianAxis.Y)
-            rollRadians = imu.getGyroAngleY();
-        else
-            rollRadians = imu.getGyroAngleZ();
-
-        if (this.pitch == CartesianAxis.X)
-            pitchRadians = imu.getGyroAngleX();
-        else if (this.pitch == CartesianAxis.Y)
-            pitchRadians = imu.getGyroAngleY();
-        else
-            pitchRadians = imu.getGyroAngleZ();
-
-        if (this.yaw == CartesianAxis.X)
-            yawRadians = imu.getGyroAngleX();
-        else if (this.yaw == CartesianAxis.Y)
-            yawRadians = imu.getGyroAngleY();
-        else
-            yawRadians = imu.getGyroAngleZ();
-
-        return new Rotation3d(rollRadians, pitchRadians, yawRadians);
+        return new Rotation3d(
+                getRawAngleRadians(this.roll),
+                getRawAngleRadians(this.pitch),
+                getRawAngleRadians(this.yaw)
+        );
     }
 
     @Override
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java
index f5b3a3f..dff2477 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java
@@ -123,9 +123,9 @@ public double getRawAngleRadians(@NotNull CartesianAxis axis) {
     @Override
     public @NotNull Rotation3d getRawRotation3dRadians() {
         return new Rotation3d(
-                Math.toRadians(imu.getAngle(fromCartesian(this.roll))),
-                Math.toRadians(imu.getAngle(fromCartesian(this.pitch))),
-                Math.toRadians(imu.getAngle(fromCartesian(this.yaw)))
+                getRawAngleRadians(this.roll),
+                getRawAngleRadians(this.pitch),
+                getRawAngleRadians(this.yaw)
         );
     }
 
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java
index 31f40ef..390f411 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java
@@ -97,19 +97,25 @@ public double getRawAngleRadians(@NotNull Attitude axis) {
 
     @Override
     public double getRawAngleRadians(@NotNull CartesianAxis axis) {
-        return switch (axis) {
-            case Z -> imu.getYaw();
-            case Y -> imu.getPitch();
-            case X -> imu.getRoll();
-        };
+        Attitude attitude;
+
+        if (axis == yaw) {
+            attitude = Attitude.YAW;
+        } else if (axis == pitch) {
+            attitude = Attitude.PITCH;
+        } else {
+            attitude = Attitude.ROLL;
+        }
+
+        return getRawAngleRadians(attitude);
     }
 
     @Override
     public @NotNull Rotation3d getRawRotation3dRadians() {
         return new Rotation3d(
-                Math.toRadians(imu.getRoll()),
-                Math.toRadians(imu.getPitch()),
-                Math.toRadians(imu.getYaw())
+                getRawAngleRadians(Attitude.ROLL),
+                getRawAngleRadians(Attitude.PITCH),
+                getRawAngleRadians(Attitude.YAW)
         );
     }
 

From fa12c525deba33bfa90633a3ad7fde463fa69d75 Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Thu, 10 Oct 2024 13:29:03 -0400
Subject: [PATCH 21/27] refactor(hardware/gyro/imu): only use Rotation3d where
 absolutely necessary

keep my family's name out of your mouth, quaternions
---
 .../librobot/hardware/gyro/ADIS16448IMU.java  |  19 ++-
 .../librobot/hardware/gyro/ADIS16470IMU.java  |   6 +-
 .../frc5183/librobot/hardware/gyro/IMU.java   | 116 +++++++++++++++---
 3 files changed, 113 insertions(+), 28 deletions(-)

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java
index 64afdf5..761d30d 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java
@@ -53,6 +53,7 @@ public ADIS16448IMU(@NotNull ADIS16448_IMU imu) {
         roll = axes[1];
 
         this.imu = imu;
+        factoryDefault();
     }
 
     /**
@@ -81,12 +82,7 @@ public ADIS16448IMU(@NotNull CartesianAxis yaw, @NotNull SPI.Port port) {
      * @param calibrationTime the calibration time.
      */
     public ADIS16448IMU(@NotNull CartesianAxis yaw, @NotNull SPI.Port port, @NotNull ADIS16448_IMU.CalibrationTime calibrationTime) {
-        this.yaw = yaw;
-        CartesianAxis[] axes = CartesianAxis.assignAxes(yaw);
-        pitch = axes[0];
-        roll = axes[1];
-
-        imu = new ADIS16448_IMU(fromCartesian(yaw), port, calibrationTime);
+        this(new ADIS16448_IMU(fromCartesian(yaw), port, calibrationTime));
     }
 
     @Override
@@ -103,9 +99,9 @@ public double getRawAngleRadians(@NotNull Attitude axis) {
     @Override
     public double getRawAngleRadians(CartesianAxis axis) {
         return switch (axis) {
-            case X -> imu.getGyroAngleX();
-            case Y -> imu.getGyroAngleY();
-            case Z -> imu.getGyroAngleZ();
+            case X -> Math.toRadians(imu.getGyroAngleX());
+            case Y -> Math.toRadians(imu.getGyroAngleY());
+            case Z -> Math.toRadians(imu.getGyroAngleZ());
         };
     }
 
@@ -145,7 +141,10 @@ public void reset() {
     @Override
     public void factoryDefault() {
         imu.calibrate();
-        super.setOffset(new Rotation3d());
+        super.setOffset(null);
+        super.setOffsetX(0);
+        super.setOffsetY(0);
+        super.setOffsetZ(0);
     }
 
     @Override
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java
index dff2477..b593789 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java
@@ -32,7 +32,7 @@ public class ADIS16470IMU extends IMU {
     private final CartesianAxis roll;
 
     /**
-     * Creates a new {@link ADIS16470IMU} using the RIO's Onboard MXP port and 4s calibration time, and yaw pitch roll as ZXY respectively.
+     * Creates a new {@link ADIS16470IMU} using the RIO's Onboard SPI port and 4s calibration time, and yaw pitch roll as ZXY respectively.
      * @see ADIS16470_IMU#ADIS16470_IMU()
      */
     public ADIS16470IMU() {
@@ -112,12 +112,12 @@ public ADIS16470IMU(
 
     @Override
     public double getRawAngleRadians(@NotNull Attitude axis) {
-        return imu.getAngle(fromAttitude(axis));
+        return Math.toRadians(imu.getAngle(fromAttitude(axis)));
     }
 
     @Override
     public double getRawAngleRadians(@NotNull CartesianAxis axis) {
-        return imu.getAngle(fromCartesian(axis));
+        return Math.toRadians(imu.getAngle(fromCartesian(axis)));
     }
 
     @Override
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java
index 04829a2..1c46f74 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java
@@ -2,8 +2,10 @@
 
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
+import org.jetbrains.annotations.Nullable;
 import swervelib.imu.SwerveIMU;
 
+import java.util.Map;
 import java.util.Optional;
 
 /**
@@ -16,9 +18,27 @@ public abstract class IMU extends SwerveIMU {
     private boolean inverted = false;
 
     /**
-     * The offset to be added to the gyroscope.
+     * The {@link Rotation3d} offset of the gyroscope.
+     * @deprecated use {@link #offsetX}, {@link #offsetY}, and {@link #offsetZ} instead, this only exists for YAGSL.<br>Changes are not reflected on {@link #offsetX}, {@link #offsetY}, and {@link #offsetZ}.
      */
-    private Rotation3d offset = new Rotation3d(0, 0, 0);
+    @Deprecated
+    @Nullable
+    private Rotation3d offset;
+
+    /**
+     * The offset to be subtracted from the gyroscope's X axis (in radians).
+     */
+    private double offsetX;
+
+    /**
+     * The offset to be subtracted from the gyroscope's Y axis (in radians).
+     */
+    private double offsetY;
+
+    /**
+     * The offset to be subtracted from the gyroscope's Z axis (in radians).
+     */
+    private double offsetZ;
 
     /**
      * Gets the specified angle's rotation (with offset) in radians.
@@ -27,9 +47,9 @@ public abstract class IMU extends SwerveIMU {
      */
     public double getAngleRadians(CartesianAxis axis) {
         return switch (axis) {
-            case Z -> getRawAngleRadians(axis) + offset.getZ();
-            case Y -> getRawAngleRadians(axis) + offset.getY();
-            case X -> getRawAngleRadians(axis) + offset.getX();
+            case Z -> getRawAngleRadians(axis) - offsetZ;
+            case Y -> getRawAngleRadians(axis) - offsetY;
+            case X -> getRawAngleRadians(axis) - offsetX;
         };
     }
 
@@ -40,9 +60,9 @@ public double getAngleRadians(CartesianAxis axis) {
      */
     public double getAngleRadians(Attitude axis) {
         return switch (axis) {
-            case YAW -> getRawAngleRadians(axis) + offset.getZ();
-            case PITCH -> getRawAngleRadians(axis) + offset.getY();
-            case ROLL -> getRawAngleRadians(axis) + offset.getX();
+            case YAW -> getAngleRadians(getYawAxis());
+            case PITCH -> getAngleRadians(getPitchAxis());
+            case ROLL -> getAngleRadians(getRollAxis());
         };
     }
 
@@ -118,19 +138,85 @@ public Optional<Translation3d> getAccel() {
     public abstract double getAccelerationMetersPerSecondSquared(CartesianAxis axis);
 
     /**
-     * Sets the offset of the gyroscope in radians.
-     * @param offset the {@link Rotation3d} of the offset of the gyroscope in radians.
+     * Returns the offset as a {@link Rotation3d}.
+     * <br><br>
+     * If {@link #offset} is null, it will be calculated from {@link #offsetX}, {@link #offsetY}, and {@link #offsetZ}.
+     * @return the offset as a {@link Rotation3d}.
+     * @deprecated use {@link #getOffsetX()}, {@link #getOffsetY()}, and {@link #getOffsetZ()} instead, this only exists for YAGSL.
      */
-    public void setOffset(Rotation3d offset) {
+    @Deprecated
+    public Rotation3d getOffset() {
+        if (offset != null) return offset;
+
+        Map<CartesianAxis, Double> offset = Map.of(
+                CartesianAxis.X, offsetX,
+                CartesianAxis.Y, offsetY,
+                CartesianAxis.Z, offsetZ
+        );
+
+        return new Rotation3d(
+                offset.get(getYawAxis()),
+                offset.get(getPitchAxis()),
+                offset.get(getRollAxis())
+        );
+    }
+
+    /**
+     * Sets the offset as a {@link Rotation3d}.
+     * @param offset the offset as a {@link Rotation3d}.
+     * @deprecated use {@link #setOffsetX(double)}, {@link #setOffsetY(double)}, and {@link #setOffsetZ(double)} instead, this only exists for YAGSL.<br>Changes are not reflected on {@link #offsetX}, {@link #offsetY}, and {@link #offsetZ}.
+     */
+    @Deprecated
+    public void setOffset(@Nullable Rotation3d offset) {
         this.offset = offset;
     }
 
     /**
-     * Gets the offset of the gyroscope in radians.
-     * @return {@link Rotation3d} the offset of the gyroscope in radians.
+     * Returns the offset to be subtracted from the gyroscope's rotation.
+     * @return the offset to be subtracted from the gyroscope's rotation.
      */
-    public Rotation3d getOffset() {
-        return offset;
+    public double getOffsetX() {
+        return offsetX;
+    }
+
+    /**
+     * Sets the offset to be subtracted from the gyroscope's rotation.
+     * @param offsetX the offset to be subtracted from the gyroscope's rotation.
+     */
+    public void setOffsetX(double offsetX) {
+        this.offsetX = offsetX;
+    }
+
+    /**
+     * Returns the offset to be subtracted from the gyroscope's rotation.
+     * @return the offset to be subtracted from the gyroscope's rotation.
+     */
+    public double getOffsetY() {
+        return offsetY;
+    }
+
+    /**
+     * Sets the offset to be subtracted from the gyroscope's rotation.
+     * @param offsetY the offset to be subtracted from the gyroscope's rotation.
+     */
+    public void setOffsetY(double offsetY) {
+        this.offsetY = offsetY;
+    }
+
+    /**
+     * Returns the offset to be subtracted from the gyroscope's rotation.
+     * @return the offset to be subtracted from the gyroscope's rotation.
+     */
+    public double getOffsetZ() {
+        return offsetZ;
+    }
+
+    /**
+     * Sets the offset to be subtracted from the gyroscope's rotation.
+     * @param offsetZ the offset to be subtracted from the gyroscope's rotation.
+     */
+    public void setOffsetZ(double offsetZ) {
+        this.offsetZ = offsetZ;
     }
 
     /**

From 9669e4876b7c611d50c9a689b48f7e84541cf758 Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Mon, 14 Oct 2024 09:19:51 -0400
Subject: [PATCH 22/27] test(hardware/gyro): implement tests for ADIS16448,
 ADIS16470, and default IMU methods.

ADIS16470 acceleration tests are broken due to a [bug in wpilibj](https://github.com/wpilibsuite/allwpilib/issues/7180).
---
 .../hardware/gyro/ADIS16448Tests.java         | 528 ++++++++++++++++++
 .../hardware/gyro/ADIS16470Tests.java         | 515 +++++++++++++++++
 .../librobot/hardware/gyro/IMUTests.java      |  22 +
 3 files changed, 1065 insertions(+)
 create mode 100644 src/test/java/net/frc5183/librobot/hardware/gyro/ADIS16448Tests.java
 create mode 100644 src/test/java/net/frc5183/librobot/hardware/gyro/ADIS16470Tests.java
 create mode 100644 src/test/java/net/frc5183/librobot/hardware/gyro/IMUTests.java

diff --git a/src/test/java/net/frc5183/librobot/hardware/gyro/ADIS16448Tests.java b/src/test/java/net/frc5183/librobot/hardware/gyro/ADIS16448Tests.java
new file mode 100644
index 0000000..2eda694
--- /dev/null
+++ b/src/test/java/net/frc5183/librobot/hardware/gyro/ADIS16448Tests.java
@@ -0,0 +1,528 @@
+package net.frc5183.librobot.hardware.gyro;
+
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.wpilibj.ADIS16448_IMU;
+import edu.wpi.first.wpilibj.SPI;
+import edu.wpi.first.wpilibj.simulation.ADIS16448_IMUSim;
+import org.junit.jupiter.api.*;
+
+import java.util.function.BiConsumer;
+import java.util.function.Consumer;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class ADIS16448Tests {
+    static final double DELTA = 1E-12;
+
+    ADIS16448IMU imu;
+    ADIS16448_IMU wpiIMU;
+    ADIS16448_IMUSim sim;
+
+    @BeforeEach
+    void setup(TestInfo info) {
+        if (info.getTags().contains("noBefore")) return;
+
+        wpiIMU = new ADIS16448_IMU();
+        sim = new ADIS16448_IMUSim(wpiIMU);
+        imu = new ADIS16448IMU(wpiIMU);
+    }
+
+    @AfterEach
+    void shutdown() {
+        wpiIMU.close();
+    }
+
+    //<editor-fold desc="Constructor Tests">
+    @Test()
+    @Tag("noBefore")
+    void testConstructor_givenNothing() {
+        imu = new ADIS16448IMU();
+        wpiIMU = imu.getIMU();
+        sim = new ADIS16448_IMUSim(wpiIMU);
+
+        assertEquals(SPI.Port.kMXP.value, wpiIMU.getPort(), "Default port is not MXP.");
+        assertEquals(IMU.CartesianAxis.Z, imu.getYawAxis(), "Default yaw axis is not Z.");
+    }
+
+    @Test
+    @Tag("noBefore")
+    void testConstructor_givenYaw() {
+        imu = new ADIS16448IMU(IMU.CartesianAxis.X);
+        wpiIMU = imu.getIMU();
+        sim = new ADIS16448_IMUSim(wpiIMU);
+
+        assertEquals(SPI.Port.kMXP.value, wpiIMU.getPort(), "Default port is not MXP.");
+        assertEquals(IMU.CartesianAxis.X, imu.getYawAxis(), "Given yaw axis is not the same.");
+    }
+
+    @Test
+    @Tag("noBefore")
+    void testConstructor_givenYawPort() {
+        imu = new ADIS16448IMU(IMU.CartesianAxis.X, SPI.Port.kOnboardCS0);
+        wpiIMU = imu.getIMU();
+        sim = new ADIS16448_IMUSim(wpiIMU);
+
+        assertEquals(SPI.Port.kOnboardCS0.value, wpiIMU.getPort(), "Given port is not the same.");
+        assertEquals(IMU.CartesianAxis.X, imu.getYawAxis(), "Given yaw axis is not the same.");
+    }
+
+    @Test
+    @Tag("noBefore")
+    void testConstructor_givenYawPortCalibrationTime() {
+        imu = new ADIS16448IMU(IMU.CartesianAxis.X, SPI.Port.kOnboardCS0, ADIS16448_IMU.CalibrationTime._2s);
+        wpiIMU = imu.getIMU();
+        sim = new ADIS16448_IMUSim(wpiIMU);
+
+        assertEquals(SPI.Port.kOnboardCS0.value, wpiIMU.getPort(), "Given port is not the same.");
+        assertEquals(IMU.CartesianAxis.X, imu.getYawAxis(), "Given yaw axis is not the same.");
+    }
+    //</editor-fold>
+
+    //<editor-fold desc="Angle Tests">
+    static final double TEST_ANGLE_1 = 0;
+    static final double TEST_ANGLE_2 = 90;
+    static final double TEST_ANGLE_3 = 180;
+    static final double TEST_ANGLE_4 = 270;
+    static final double TEST_ANGLE_5 = Math.random() * 360;
+
+    @Test
+    void testAngleYaw() {
+        sim.setGyroAngleZ(TEST_ANGLE_1);
+        assertEquals(TEST_ANGLE_1, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.YAW)), DELTA);
+
+        sim.setGyroAngleZ(TEST_ANGLE_2);
+        assertEquals(TEST_ANGLE_2, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.YAW)), DELTA);
+
+        sim.setGyroAngleZ(TEST_ANGLE_3);
+        assertEquals(TEST_ANGLE_3, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.YAW)), DELTA);
+
+        sim.setGyroAngleZ(TEST_ANGLE_4);
+        assertEquals(TEST_ANGLE_4, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.YAW)), DELTA);
+
+        sim.setGyroAngleZ(TEST_ANGLE_5);
+        assertEquals(TEST_ANGLE_5, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.YAW)), DELTA);
+    }
+
+    @Test
+    void testAnglePitch() {
+        sim.setGyroAngleX(TEST_ANGLE_1);
+        assertEquals(TEST_ANGLE_1, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.PITCH)), DELTA);
+
+        sim.setGyroAngleX(TEST_ANGLE_2);
+        assertEquals(TEST_ANGLE_2, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.PITCH)), DELTA);
+
+        sim.setGyroAngleX(TEST_ANGLE_3);
+        assertEquals(TEST_ANGLE_3, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.PITCH)), DELTA);
+
+        sim.setGyroAngleX(TEST_ANGLE_4);
+        assertEquals(TEST_ANGLE_4, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.PITCH)), DELTA);
+
+        sim.setGyroAngleX(TEST_ANGLE_5);
+        assertEquals(TEST_ANGLE_5, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.PITCH)), DELTA);
+    }
+
+    @Test
+    void testAngleRoll() {
+        sim.setGyroAngleY(TEST_ANGLE_1);
+        assertEquals(TEST_ANGLE_1, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.ROLL)), DELTA);
+
+        sim.setGyroAngleY(TEST_ANGLE_2);
+        assertEquals(TEST_ANGLE_2, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.ROLL)), DELTA);
+
+        sim.setGyroAngleY(TEST_ANGLE_3);
+        assertEquals(TEST_ANGLE_3, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.ROLL)), DELTA);
+
+        sim.setGyroAngleY(TEST_ANGLE_4);
+        assertEquals(TEST_ANGLE_4, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.ROLL)), DELTA);
+
+        sim.setGyroAngleY(TEST_ANGLE_5);
+        assertEquals(TEST_ANGLE_5, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.ROLL)), DELTA);
+    }
+
+    @Test
+    void testAngleCartesianX() {
+        sim.setGyroAngleX(TEST_ANGLE_1);
+        assertEquals(TEST_ANGLE_1, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.X)), DELTA);
+
+        sim.setGyroAngleX(TEST_ANGLE_2);
+        assertEquals(TEST_ANGLE_2, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.X)), DELTA);
+
+        sim.setGyroAngleX(TEST_ANGLE_3);
+        assertEquals(TEST_ANGLE_3, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.X)), DELTA);
+
+        sim.setGyroAngleX(TEST_ANGLE_4);
+        assertEquals(TEST_ANGLE_4, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.X)), DELTA);
+
+        sim.setGyroAngleX(TEST_ANGLE_5);
+        assertEquals(TEST_ANGLE_5, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.X)), DELTA);
+    }
+
+    @Test
+    void testAngleCartesianY() {
+        sim.setGyroAngleY(TEST_ANGLE_1);
+        assertEquals(TEST_ANGLE_1, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Y)), DELTA);
+
+        sim.setGyroAngleY(TEST_ANGLE_2);
+        assertEquals(TEST_ANGLE_2, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Y)), DELTA);
+
+        sim.setGyroAngleY(TEST_ANGLE_3);
+        assertEquals(TEST_ANGLE_3, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Y)), DELTA);
+
+        sim.setGyroAngleY(TEST_ANGLE_4);
+        assertEquals(TEST_ANGLE_4, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Y)), DELTA);
+
+        sim.setGyroAngleY(TEST_ANGLE_5);
+        assertEquals(TEST_ANGLE_5, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Y)), DELTA);
+    }
+
+    @Test
+    void testAngleCartesianZ() {
+        sim.setGyroAngleZ(TEST_ANGLE_1);
+        assertEquals(TEST_ANGLE_1, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Z)), DELTA);
+
+        sim.setGyroAngleZ(TEST_ANGLE_2);
+        assertEquals(TEST_ANGLE_2, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Z)), DELTA);
+
+        sim.setGyroAngleZ(TEST_ANGLE_3);
+        assertEquals(TEST_ANGLE_3, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Z)), DELTA);
+
+        sim.setGyroAngleZ(TEST_ANGLE_4);
+        assertEquals(TEST_ANGLE_4, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Z)), DELTA);
+
+        sim.setGyroAngleZ(TEST_ANGLE_5);
+        assertEquals(TEST_ANGLE_5, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Z)), DELTA);
+    }
+
+    @Test
+    void testAngleRotation3d() {
+        // Degrees
+        Consumer<Double> setAngle = (Double angle) -> {
+            sim.setGyroAngleX(angle);
+            sim.setGyroAngleY(angle);
+            sim.setGyroAngleZ(angle);
+        };
+
+        // Radians
+        Consumer<Double> verifyRot = (Double angle) -> {
+            assertEquals(
+                    new Rotation3d(
+                            Math.toRadians(angle),
+                            Math.toRadians(angle),
+                            Math.toRadians(angle)
+                    ),
+                    imu.getRotation3dRadians(),
+                    "Rotation3d failed."
+            );
+        };
+
+        setAngle.accept(TEST_ANGLE_1);
+        verifyRot.accept(TEST_ANGLE_1);
+
+        setAngle.accept(TEST_ANGLE_2);
+        verifyRot.accept(TEST_ANGLE_2);
+
+        setAngle.accept(TEST_ANGLE_3);
+        verifyRot.accept(TEST_ANGLE_3);
+
+        setAngle.accept(TEST_ANGLE_4);
+        verifyRot.accept(TEST_ANGLE_4);
+
+        setAngle.accept(TEST_ANGLE_5);
+        verifyRot.accept(TEST_ANGLE_5);
+    }
+
+    @Test
+    void testAngleOffset() {
+        // Radians.
+        Consumer<Double> setOffset = (Double offset) -> {
+            imu.setOffsetX((double) offset);
+            imu.setOffsetY((double) offset);
+            imu.setOffsetZ((double) offset);
+        };
+
+        // Degrees.
+        Consumer<Double> setAngle = (Double angle) -> {
+            sim.setGyroAngleX(angle);
+            sim.setGyroAngleY(angle);
+            sim.setGyroAngleZ(angle);
+        };
+
+        // Radians.
+        BiConsumer<Double, Double> testAngles = (Double angle, Double offset) -> {
+            double expected = angle - offset;
+
+            assertEquals(
+                    expected,
+                    imu.getAngleRadians(IMU.CartesianAxis.X),
+                    DELTA,
+                    "X offset failed."
+            );
+
+            assertEquals(
+                    expected,
+                    imu.getAngleRadians(IMU.CartesianAxis.Y),
+                    DELTA,
+                    "Y offset failed."
+            );
+
+            assertEquals(
+                    expected,
+                    imu.getAngleRadians(IMU.CartesianAxis.Z),
+                    DELTA,
+                    "Z offset failed."
+            );
+
+            assertEquals(
+                    new Rotation3d(
+                            angle, angle, angle
+                    ).minus(new Rotation3d(offset, offset, offset)),
+                    imu.getRotation3dRadians(),
+                    "Rotation3d offset failed."
+            );
+        };
+
+
+        setOffset.accept(Math.toRadians(TEST_ANGLE_1));
+        //<editor-fold desc="TEST_ANGLE_1 as offset.">
+        setAngle.accept(TEST_ANGLE_1);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_1), Math.toRadians(TEST_ANGLE_1));
+
+        setAngle.accept(TEST_ANGLE_2);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_2), Math.toRadians(TEST_ANGLE_1));
+
+        setAngle.accept(TEST_ANGLE_3);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_3), Math.toRadians(TEST_ANGLE_1));
+
+        setAngle.accept(TEST_ANGLE_4);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_4), Math.toRadians(TEST_ANGLE_1));
+
+        setAngle.accept(TEST_ANGLE_5);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_5), Math.toRadians(TEST_ANGLE_1));
+        //</editor-fold>
+
+        setOffset.accept(Math.toRadians(TEST_ANGLE_2));
+        //<editor-fold desc="TEST_ANGLE_2 as offset.">
+        setAngle.accept(TEST_ANGLE_1);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_1), Math.toRadians(TEST_ANGLE_2));
+
+        setAngle.accept(TEST_ANGLE_2);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_2), Math.toRadians(TEST_ANGLE_2));
+
+        setAngle.accept(TEST_ANGLE_3);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_3), Math.toRadians(TEST_ANGLE_2));
+
+        setAngle.accept(TEST_ANGLE_4);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_4), Math.toRadians(TEST_ANGLE_2));
+
+        setAngle.accept(TEST_ANGLE_5);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_5), Math.toRadians(TEST_ANGLE_2));
+        //</editor-fold>
+
+        setOffset.accept(Math.toRadians(TEST_ANGLE_3));
+        //<editor-fold desc="TEST_ANGLE_3 as offset.">
+        setAngle.accept(TEST_ANGLE_1);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_1), Math.toRadians(TEST_ANGLE_3));
+
+        setAngle.accept(TEST_ANGLE_2);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_2), Math.toRadians(TEST_ANGLE_3));
+
+        setAngle.accept(TEST_ANGLE_3);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_3), Math.toRadians(TEST_ANGLE_3));
+
+        setAngle.accept(TEST_ANGLE_4);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_4), Math.toRadians(TEST_ANGLE_3));
+
+        setAngle.accept(TEST_ANGLE_5);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_5), Math.toRadians(TEST_ANGLE_3));
+        //</editor-fold>
+
+        setOffset.accept(Math.toRadians(TEST_ANGLE_4));
+        //<editor-fold desc="TEST_ANGLE_4 as offset.">
+        setAngle.accept(TEST_ANGLE_1);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_1), Math.toRadians(TEST_ANGLE_4));
+
+        setAngle.accept(TEST_ANGLE_2);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_2), Math.toRadians(TEST_ANGLE_4));
+
+        setAngle.accept(TEST_ANGLE_3);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_3), Math.toRadians(TEST_ANGLE_4));
+
+        setAngle.accept(TEST_ANGLE_4);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_4), Math.toRadians(TEST_ANGLE_4));
+
+        setAngle.accept(TEST_ANGLE_5);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_5), Math.toRadians(TEST_ANGLE_4));
+        //</editor-fold>
+
+        setOffset.accept(Math.toRadians(TEST_ANGLE_5));
+        //<editor-fold desc="TEST_ANGLE_5 as offset.">
+        setAngle.accept(TEST_ANGLE_1);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_1), Math.toRadians(TEST_ANGLE_5));
+
+        setAngle.accept(TEST_ANGLE_2);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_2), Math.toRadians(TEST_ANGLE_5));
+
+        setAngle.accept(TEST_ANGLE_3);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_3), Math.toRadians(TEST_ANGLE_5));
+
+        setAngle.accept(TEST_ANGLE_4);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_4), Math.toRadians(TEST_ANGLE_5));
+
+        setAngle.accept(TEST_ANGLE_5);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_5), Math.toRadians(TEST_ANGLE_5));
+        //</editor-fold>
+    }
+    //</editor-fold>
+
+    //<editor-fold desc="Acceleration Tests">
+    static final double TEST_ACCEL_1 = 0;
+    static final double TEST_ACCEL_2 = 1;
+    static final double TEST_ACCEL_3 = 2;
+    static final double TEST_ACCEL_4 = 3;
+    static final double TEST_ACCEL_5 = Math.random() * 3;
+
+    @Test
+    void testAccelerationCartesianX() {
+        sim.setAccelX(TEST_ACCEL_1);
+        assertEquals(TEST_ACCEL_1, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X));
+
+        sim.setAccelX(TEST_ACCEL_2);
+        assertEquals(TEST_ACCEL_2, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X));
+
+        sim.setAccelX(TEST_ACCEL_3);
+        assertEquals(TEST_ACCEL_3, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X));
+
+        sim.setAccelX(TEST_ACCEL_4);
+        assertEquals(TEST_ACCEL_4, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X));
+
+        sim.setAccelX(TEST_ACCEL_5);
+        assertEquals(TEST_ACCEL_5, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X));
+    }
+
+    @Test
+    void testAccelerationCartesianY() {
+        sim.setAccelY(TEST_ACCEL_1);
+        assertEquals(TEST_ACCEL_1, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y));
+
+        sim.setAccelY(TEST_ACCEL_2);
+        assertEquals(TEST_ACCEL_2, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y));
+
+        sim.setAccelY(TEST_ACCEL_3);
+        assertEquals(TEST_ACCEL_3, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y));
+
+        sim.setAccelY(TEST_ACCEL_4);
+        assertEquals(TEST_ACCEL_4, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y));
+
+        sim.setAccelY(TEST_ACCEL_5);
+        assertEquals(TEST_ACCEL_5, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y));
+    }
+
+    @Test
+    void testAccelerationCartesianZ() {
+        sim.setAccelZ(TEST_ACCEL_1);
+        assertEquals(TEST_ACCEL_1, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z));
+
+        sim.setAccelZ(TEST_ACCEL_2);
+        assertEquals(TEST_ACCEL_2, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z));
+
+        sim.setAccelZ(TEST_ACCEL_3);
+        assertEquals(TEST_ACCEL_3, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z));
+
+        sim.setAccelZ(TEST_ACCEL_4);
+        assertEquals(TEST_ACCEL_4, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z));
+
+        sim.setAccelZ(TEST_ACCEL_5);
+        assertEquals(TEST_ACCEL_5, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z));
+    }
+
+    @Test
+    void testAccelerationTranslation3d() {
+        // Meters per second squared.
+        Consumer<Double> setAccel = (Double accel) -> {
+            sim.setAccelX(accel);
+            sim.setAccelY(accel);
+            sim.setAccelZ(accel);
+        };
+
+        // Meters per second squared.
+        BiConsumer<Double, Double> testAccel = (Double accel, Double expected) -> {
+            assertEquals(
+                    new Translation3d(expected, expected, expected),
+                    imu.getAccelerationMetersPerSecondSquared(),
+                    "Translation3d failed."
+            );
+        };
+
+        setAccel.accept(TEST_ACCEL_1);
+        testAccel.accept(TEST_ACCEL_1, TEST_ACCEL_1);
+
+        setAccel.accept(TEST_ACCEL_2);
+        testAccel.accept(TEST_ACCEL_2, TEST_ACCEL_2);
+
+        setAccel.accept(TEST_ACCEL_3);
+        testAccel.accept(TEST_ACCEL_3, TEST_ACCEL_3);
+
+        setAccel.accept(TEST_ACCEL_4);
+        testAccel.accept(TEST_ACCEL_4, TEST_ACCEL_4);
+
+        setAccel.accept(TEST_ACCEL_5);
+        testAccel.accept(TEST_ACCEL_5, TEST_ACCEL_5);
+    }
+    //</editor-fold>
+
+    //<editor-fold desc="Misc Tests">
+    @Test
+    void testCalibrate() {
+        assertDoesNotThrow(() -> imu.calibrate());
+        assertEquals(0, wpiIMU.getAccelX(), "X calibrate failed.");
+        assertEquals(0, wpiIMU.getAccelY(), "Y calibrate failed.");
+        assertEquals(0, wpiIMU.getAccelZ(), "Z calibrate failed.");
+    }
+
+    @Test
+    void testReset() {
+        assertDoesNotThrow(() -> imu.reset());
+        assertEquals(0, wpiIMU.getAccelX(), "X reset failed.");
+        assertEquals(0, wpiIMU.getAccelY(), "Y reset failed.");
+        assertEquals(0, wpiIMU.getAccelZ(), "Z reset failed.");
+    }
+
+    @Test
+    void testFactoryDefault() {
+        assertDoesNotThrow(() -> imu.factoryDefault());
+        assertEquals(0, wpiIMU.getAccelX(), "X factory default failed.");
+        assertEquals(0, wpiIMU.getAccelY(), "Y factory default failed.");
+        assertEquals(0, wpiIMU.getAccelZ(), "Z factory default failed.");
+
+        assertEquals(0, imu.getOffsetX(), "X factory default failed.");
+        assertEquals(0, imu.getOffsetY(), "Y factory default failed.");
+        assertEquals(0, imu.getOffsetZ(), "Z factory default failed.");
+
+        assertEquals(0, wpiIMU.getGyroAngleX(), "X factory default failed.");
+        assertEquals(0, wpiIMU.getGyroAngleY(), "Y factory default failed.");
+        assertEquals(0, wpiIMU.getGyroAngleZ(), "Z factory default failed.");
+    }
+
+    @Test
+    void testIMU() {
+        assertEquals(wpiIMU, imu.getIMU());
+    }
+
+    @Test
+    void testToCartesian() {
+        assertEquals(IMU.CartesianAxis.X, ADIS16448IMU.toCartesian(ADIS16448_IMU.IMUAxis.kX));
+        assertEquals(IMU.CartesianAxis.Y, ADIS16448IMU.toCartesian(ADIS16448_IMU.IMUAxis.kY));
+        assertEquals(IMU.CartesianAxis.Z, ADIS16448IMU.toCartesian(ADIS16448_IMU.IMUAxis.kZ));
+    }
+
+    @Test
+    void testFromCartesian() {
+        assertEquals(ADIS16448_IMU.IMUAxis.kX, ADIS16448IMU.fromCartesian(IMU.CartesianAxis.X));
+        assertEquals(ADIS16448_IMU.IMUAxis.kY, ADIS16448IMU.fromCartesian(IMU.CartesianAxis.Y));
+        assertEquals(ADIS16448_IMU.IMUAxis.kZ, ADIS16448IMU.fromCartesian(IMU.CartesianAxis.Z));
+    }
+}
+
+
diff --git a/src/test/java/net/frc5183/librobot/hardware/gyro/ADIS16470Tests.java b/src/test/java/net/frc5183/librobot/hardware/gyro/ADIS16470Tests.java
new file mode 100644
index 0000000..e3de762
--- /dev/null
+++ b/src/test/java/net/frc5183/librobot/hardware/gyro/ADIS16470Tests.java
@@ -0,0 +1,515 @@
+package net.frc5183.librobot.hardware.gyro;
+
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.wpilibj.ADIS16470_IMU;
+import edu.wpi.first.wpilibj.SPI;
+import edu.wpi.first.wpilibj.simulation.ADIS16470_IMUSim;
+import org.junit.jupiter.api.*;
+
+import java.util.function.BiConsumer;
+import java.util.function.Consumer;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class ADIS16470Tests {
+    static final double DELTA = 1E-12;
+
+    ADIS16470IMU imu;
+    ADIS16470_IMU wpiIMU;
+    ADIS16470_IMUSim sim;
+
+    @BeforeEach
+    void setup(TestInfo info) {
+        if (info.getTags().contains("noBefore")) return;
+
+        wpiIMU = new ADIS16470_IMU();
+        sim = new ADIS16470_IMUSim(wpiIMU);
+        imu = new ADIS16470IMU(wpiIMU);
+    }
+
+    @AfterEach
+    void shutdown() {
+        wpiIMU.close();
+    }
+
+    //<editor-fold desc="Constructor Tests">
+    @Test()
+    @Tag("noBefore")
+    void testConstructor_givenNothing() {
+        imu = new ADIS16470IMU();
+        wpiIMU = imu.getIMU();
+        sim = new ADIS16470_IMUSim(wpiIMU);
+
+        assertEquals(IMU.CartesianAxis.Z, imu.getYawAxis(), "Default yaw axis is not Z.");
+        assertEquals(IMU.CartesianAxis.X, imu.getPitchAxis(), "Default pitch axis is not X.");
+        assertEquals(IMU.CartesianAxis.Y, imu.getRollAxis(), "Default roll axis is not Y.");
+    }
+
+    @Test
+    @Tag("noBefore")
+    void testConstructor_givenYawPitchRoll() {
+        imu = new ADIS16470IMU(IMU.CartesianAxis.X, IMU.CartesianAxis.Z, IMU.CartesianAxis.Y);
+        wpiIMU = imu.getIMU();
+        sim = new ADIS16470_IMUSim(wpiIMU);
+
+        assertEquals(SPI.Port.kMXP.value, wpiIMU.getPort(), "Default port is not MXP.");
+        assertEquals(IMU.CartesianAxis.X, imu.getYawAxis(), "Given yaw axis is not the same.");
+        assertEquals(IMU.CartesianAxis.Z, imu.getPitchAxis(), "Given pitch axis is not the same.");
+        assertEquals(IMU.CartesianAxis.Y, imu.getRollAxis(), "Given roll axis is not the same.");
+    }
+
+    @Test
+    @Tag("noBefore")
+    void testConstructor_givenYawPitchRollPort() {
+        imu = new ADIS16470IMU(IMU.CartesianAxis.X, IMU.CartesianAxis.Z, IMU.CartesianAxis.Y, SPI.Port.kOnboardCS0);
+        wpiIMU = imu.getIMU();
+        sim = new ADIS16470_IMUSim(wpiIMU);
+
+        assertEquals(SPI.Port.kOnboardCS0.value, wpiIMU.getPort(), "Given port is not the same.");
+        assertEquals(IMU.CartesianAxis.X, imu.getYawAxis(), "Given yaw axis is not the same.");
+        assertEquals(IMU.CartesianAxis.Z, imu.getPitchAxis(), "Given pitch axis is not the same.");
+        assertEquals(IMU.CartesianAxis.Y, imu.getRollAxis(), "Given roll axis is not the same.");
+    }
+
+    @Test
+    @Tag("noBefore")
+    void testConstructor_givenYawPortCalibrationTime() {
+        imu = new ADIS16470IMU(IMU.CartesianAxis.X, IMU.CartesianAxis.Z, IMU.CartesianAxis.Y, SPI.Port.kOnboardCS0, ADIS16470_IMU.CalibrationTime._2s);
+        wpiIMU = imu.getIMU();
+        sim = new ADIS16470_IMUSim(wpiIMU);
+
+        assertEquals(SPI.Port.kOnboardCS0.value, wpiIMU.getPort(), "Given port is not the same.");
+        assertEquals(IMU.CartesianAxis.X, imu.getYawAxis(), "Given yaw axis is not the same.");
+        assertEquals(IMU.CartesianAxis.Z, imu.getPitchAxis(), "Given pitch axis is not the same.");
+        assertEquals(IMU.CartesianAxis.Y, imu.getRollAxis(), "Given roll axis is not the same.");
+    }
+    //</editor-fold>
+
+    //<editor-fold desc="Angle Tests">
+    static final double TEST_ANGLE_1 = 0;
+    static final double TEST_ANGLE_2 = 90;
+    static final double TEST_ANGLE_3 = 180;
+    static final double TEST_ANGLE_4 = 270;
+    static final double TEST_ANGLE_5 = Math.random() * 360;
+
+    @Test
+    void testAngleYaw() {
+        sim.setGyroAngleZ(TEST_ANGLE_1);
+        assertEquals(TEST_ANGLE_1, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.YAW)), DELTA);
+
+        sim.setGyroAngleZ(TEST_ANGLE_2);
+        assertEquals(TEST_ANGLE_2, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.YAW)), DELTA);
+
+        sim.setGyroAngleZ(TEST_ANGLE_3);
+        assertEquals(TEST_ANGLE_3, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.YAW)), DELTA);
+
+        sim.setGyroAngleZ(TEST_ANGLE_4);
+        assertEquals(TEST_ANGLE_4, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.YAW)), DELTA);
+
+        sim.setGyroAngleZ(TEST_ANGLE_5);
+        assertEquals(TEST_ANGLE_5, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.YAW)), DELTA);
+    }
+
+    @Test
+    void testAnglePitch() {
+        sim.setGyroAngleX(TEST_ANGLE_1);
+        assertEquals(TEST_ANGLE_1, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.PITCH)), DELTA);
+
+        sim.setGyroAngleX(TEST_ANGLE_2);
+        assertEquals(TEST_ANGLE_2, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.PITCH)), DELTA);
+
+        sim.setGyroAngleX(TEST_ANGLE_3);
+        assertEquals(TEST_ANGLE_3, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.PITCH)), DELTA);
+
+        sim.setGyroAngleX(TEST_ANGLE_4);
+        assertEquals(TEST_ANGLE_4, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.PITCH)), DELTA);
+
+        sim.setGyroAngleX(TEST_ANGLE_5);
+        assertEquals(TEST_ANGLE_5, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.PITCH)), DELTA);
+    }
+
+    @Test
+    void testAngleRoll() {
+        sim.setGyroAngleY(TEST_ANGLE_1);
+        assertEquals(TEST_ANGLE_1, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.ROLL)), DELTA);
+
+        sim.setGyroAngleY(TEST_ANGLE_2);
+        assertEquals(TEST_ANGLE_2, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.ROLL)), DELTA);
+
+        sim.setGyroAngleY(TEST_ANGLE_3);
+        assertEquals(TEST_ANGLE_3, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.ROLL)), DELTA);
+
+        sim.setGyroAngleY(TEST_ANGLE_4);
+        assertEquals(TEST_ANGLE_4, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.ROLL)), DELTA);
+
+        sim.setGyroAngleY(TEST_ANGLE_5);
+        assertEquals(TEST_ANGLE_5, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.ROLL)), DELTA);
+    }
+
+    @Test
+    void testAngleCartesianX() {
+        sim.setGyroAngleX(TEST_ANGLE_1);
+        assertEquals(TEST_ANGLE_1, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.X)), DELTA);
+
+        sim.setGyroAngleX(TEST_ANGLE_2);
+        assertEquals(TEST_ANGLE_2, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.X)), DELTA);
+
+        sim.setGyroAngleX(TEST_ANGLE_3);
+        assertEquals(TEST_ANGLE_3, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.X)), DELTA);
+
+        sim.setGyroAngleX(TEST_ANGLE_4);
+        assertEquals(TEST_ANGLE_4, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.X)), DELTA);
+
+        sim.setGyroAngleX(TEST_ANGLE_5);
+        assertEquals(TEST_ANGLE_5, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.X)), DELTA);
+    }
+
+    @Test
+    void testAngleCartesianY() {
+        sim.setGyroAngleY(TEST_ANGLE_1);
+        assertEquals(TEST_ANGLE_1, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Y)), DELTA);
+
+        sim.setGyroAngleY(TEST_ANGLE_2);
+        assertEquals(TEST_ANGLE_2, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Y)), DELTA);
+
+        sim.setGyroAngleY(TEST_ANGLE_3);
+        assertEquals(TEST_ANGLE_3, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Y)), DELTA);
+
+        sim.setGyroAngleY(TEST_ANGLE_4);
+        assertEquals(TEST_ANGLE_4, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Y)), DELTA);
+
+        sim.setGyroAngleY(TEST_ANGLE_5);
+        assertEquals(TEST_ANGLE_5, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Y)), DELTA);
+    }
+
+    @Test
+    void testAngleCartesianZ() {
+        sim.setGyroAngleZ(TEST_ANGLE_1);
+        assertEquals(TEST_ANGLE_1, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Z)), DELTA);
+
+        sim.setGyroAngleZ(TEST_ANGLE_2);
+        assertEquals(TEST_ANGLE_2, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Z)), DELTA);
+
+        sim.setGyroAngleZ(TEST_ANGLE_3);
+        assertEquals(TEST_ANGLE_3, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Z)), DELTA);
+
+        sim.setGyroAngleZ(TEST_ANGLE_4);
+        assertEquals(TEST_ANGLE_4, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Z)), DELTA);
+
+        sim.setGyroAngleZ(TEST_ANGLE_5);
+        assertEquals(TEST_ANGLE_5, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Z)), DELTA);
+    }
+
+    @Test
+    void testAngleRotation3d() {
+        // Degrees
+        Consumer<Double> setAngle = (Double angle) -> {
+            sim.setGyroAngleX(angle);
+            sim.setGyroAngleY(angle);
+            sim.setGyroAngleZ(angle);
+        };
+
+        // Radians
+        Consumer<Double> verifyRot = (Double angle) -> {
+            assertEquals(
+                    new Rotation3d(
+                            Math.toRadians(angle),
+                            Math.toRadians(angle),
+                            Math.toRadians(angle)
+                    ),
+                    imu.getRotation3dRadians(),
+                    "Rotation3d failed."
+            );
+        };
+
+        setAngle.accept(TEST_ANGLE_1);
+        verifyRot.accept(TEST_ANGLE_1);
+
+        setAngle.accept(TEST_ANGLE_2);
+        verifyRot.accept(TEST_ANGLE_2);
+
+        setAngle.accept(TEST_ANGLE_3);
+        verifyRot.accept(TEST_ANGLE_3);
+
+        setAngle.accept(TEST_ANGLE_4);
+        verifyRot.accept(TEST_ANGLE_4);
+
+        setAngle.accept(TEST_ANGLE_5);
+        verifyRot.accept(TEST_ANGLE_5);
+    }
+
+    @Test
+    void testAngleOffset() {
+        // Radians.
+        Consumer<Double> setOffset = (Double offset) -> {
+            imu.setOffsetX((double) offset);
+            imu.setOffsetY((double) offset);
+            imu.setOffsetZ((double) offset);
+        };
+
+        // Degrees.
+        Consumer<Double> setAngle = (Double angle) -> {
+            sim.setGyroAngleX(angle);
+            sim.setGyroAngleY(angle);
+            sim.setGyroAngleZ(angle);
+        };
+
+        // Radians.
+        BiConsumer<Double, Double> testAngles = (Double angle, Double offset) -> {
+            double expected = angle - offset;
+
+            assertEquals(
+                    expected,
+                    imu.getAngleRadians(IMU.CartesianAxis.X),
+                    DELTA,
+                    "X offset failed."
+            );
+
+            assertEquals(
+                    expected,
+                    imu.getAngleRadians(IMU.CartesianAxis.Y),
+                    DELTA,
+                    "Y offset failed."
+            );
+
+            assertEquals(
+                    expected,
+                    imu.getAngleRadians(IMU.CartesianAxis.Z),
+                    DELTA,
+                    "Z offset failed."
+            );
+
+            assertEquals(
+                    new Rotation3d(
+                            angle, angle, angle
+                    ).minus(new Rotation3d(offset, offset, offset)),
+                    imu.getRotation3dRadians(),
+                    "Rotation3d offset failed."
+            );
+        };
+
+
+        setOffset.accept(Math.toRadians(TEST_ANGLE_1));
+        //<editor-fold desc="TEST_ANGLE_1 as offset.">
+        setAngle.accept(TEST_ANGLE_1);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_1), Math.toRadians(TEST_ANGLE_1));
+
+        setAngle.accept(TEST_ANGLE_2);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_2), Math.toRadians(TEST_ANGLE_1));
+
+        setAngle.accept(TEST_ANGLE_3);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_3), Math.toRadians(TEST_ANGLE_1));
+
+        setAngle.accept(TEST_ANGLE_4);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_4), Math.toRadians(TEST_ANGLE_1));
+
+        setAngle.accept(TEST_ANGLE_5);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_5), Math.toRadians(TEST_ANGLE_1));
+        //</editor-fold>
+
+        setOffset.accept(Math.toRadians(TEST_ANGLE_2));
+        //<editor-fold desc="TEST_ANGLE_2 as offset.">
+        setAngle.accept(TEST_ANGLE_1);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_1), Math.toRadians(TEST_ANGLE_2));
+
+        setAngle.accept(TEST_ANGLE_2);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_2), Math.toRadians(TEST_ANGLE_2));
+
+        setAngle.accept(TEST_ANGLE_3);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_3), Math.toRadians(TEST_ANGLE_2));
+
+        setAngle.accept(TEST_ANGLE_4);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_4), Math.toRadians(TEST_ANGLE_2));
+
+        setAngle.accept(TEST_ANGLE_5);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_5), Math.toRadians(TEST_ANGLE_2));
+        //</editor-fold>
+
+        setOffset.accept(Math.toRadians(TEST_ANGLE_3));
+        //<editor-fold desc="TEST_ANGLE_3 as offset.">
+        setAngle.accept(TEST_ANGLE_1);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_1), Math.toRadians(TEST_ANGLE_3));
+
+        setAngle.accept(TEST_ANGLE_2);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_2), Math.toRadians(TEST_ANGLE_3));
+
+        setAngle.accept(TEST_ANGLE_3);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_3), Math.toRadians(TEST_ANGLE_3));
+
+        setAngle.accept(TEST_ANGLE_4);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_4), Math.toRadians(TEST_ANGLE_3));
+
+        setAngle.accept(TEST_ANGLE_5);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_5), Math.toRadians(TEST_ANGLE_3));
+        //</editor-fold>
+
+        setOffset.accept(Math.toRadians(TEST_ANGLE_4));
+        //<editor-fold desc="TEST_ANGLE_4 as offset.">
+        setAngle.accept(TEST_ANGLE_1);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_1), Math.toRadians(TEST_ANGLE_4));
+
+        setAngle.accept(TEST_ANGLE_2);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_2), Math.toRadians(TEST_ANGLE_4));
+
+        setAngle.accept(TEST_ANGLE_3);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_3), Math.toRadians(TEST_ANGLE_4));
+
+        setAngle.accept(TEST_ANGLE_4);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_4), Math.toRadians(TEST_ANGLE_4));
+
+        setAngle.accept(TEST_ANGLE_5);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_5), Math.toRadians(TEST_ANGLE_4));
+        //</editor-fold>
+
+        setOffset.accept(Math.toRadians(TEST_ANGLE_5));
+        //<editor-fold desc="TEST_ANGLE_5 as offset.">
+        setAngle.accept(TEST_ANGLE_1);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_1), Math.toRadians(TEST_ANGLE_5));
+
+        setAngle.accept(TEST_ANGLE_2);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_2), Math.toRadians(TEST_ANGLE_5));
+
+        setAngle.accept(TEST_ANGLE_3);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_3), Math.toRadians(TEST_ANGLE_5));
+
+        setAngle.accept(TEST_ANGLE_4);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_4), Math.toRadians(TEST_ANGLE_5));
+
+        setAngle.accept(TEST_ANGLE_5);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_5), Math.toRadians(TEST_ANGLE_5));
+        //</editor-fold>
+    }
+    //</editor-fold>
+
+    // TODO: Acceleration tests are currently disabled due to a bug in wpilibj.
+    // https://github.com/wpilibsuite/allwpilib/issues/7180
+    //<editor-fold desc="Acceleration Tests">
+    /*
+    static final double TEST_ACCEL_1 = 0;
+    static final double TEST_ACCEL_2 = 1;
+    static final double TEST_ACCEL_3 = 2;
+    static final double TEST_ACCEL_4 = 3;
+    static final double TEST_ACCEL_5 = Math.random() * 3;
+
+    @Test
+    void testAccelerationCartesianX() {
+        sim.setAccelX(TEST_ACCEL_1);
+        assertEquals(TEST_ACCEL_1, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X));
+
+        sim.setAccelX(TEST_ACCEL_2);
+        assertEquals(TEST_ACCEL_2, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X));
+
+        sim.setAccelX(TEST_ACCEL_3);
+        assertEquals(TEST_ACCEL_3, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X));
+
+        sim.setAccelX(TEST_ACCEL_4);
+        assertEquals(TEST_ACCEL_4, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X));
+
+        sim.setAccelX(TEST_ACCEL_5);
+        assertEquals(TEST_ACCEL_5, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X));
+    }
+
+    @Test
+    void testAccelerationCartesianY() {
+        sim.setAccelY(TEST_ACCEL_1);
+        assertEquals(TEST_ACCEL_1, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y));
+
+        sim.setAccelY(TEST_ACCEL_2);
+        assertEquals(TEST_ACCEL_2, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y));
+
+        sim.setAccelY(TEST_ACCEL_3);
+        assertEquals(TEST_ACCEL_3, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y));
+
+        sim.setAccelY(TEST_ACCEL_4);
+        assertEquals(TEST_ACCEL_4, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y));
+
+        sim.setAccelY(TEST_ACCEL_5);
+        assertEquals(TEST_ACCEL_5, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y));
+    }
+
+    @Test
+    void testAccelerationCartesianZ() {
+        sim.setAccelZ(TEST_ACCEL_1);
+        assertEquals(TEST_ACCEL_1, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z));
+
+        sim.setAccelZ(TEST_ACCEL_2);
+        assertEquals(TEST_ACCEL_2, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z));
+
+        sim.setAccelZ(TEST_ACCEL_3);
+        assertEquals(TEST_ACCEL_3, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z));
+
+        sim.setAccelZ(TEST_ACCEL_4);
+        assertEquals(TEST_ACCEL_4, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z));
+
+        sim.setAccelZ(TEST_ACCEL_5);
+        assertEquals(TEST_ACCEL_5, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z));
+    }
+
+    @Test
+    void testAccelerationTranslation3d() {
+        // Meters per second squared.
+        Consumer<Double> setAccel = (Double accel) -> {
+            sim.setAccelX(accel);
+            sim.setAccelY(accel);
+            sim.setAccelZ(accel);
+        };
+
+        // Meters per second squared.
+        BiConsumer<Double, Double> testAccel = (Double accel, Double expected) -> {
+            assertEquals(
+                    new Translation3d(expected, expected, expected),
+                    imu.getAccelerationMetersPerSecondSquared(),
+                    "Translation3d failed."
+            );
+        };
+
+        setAccel.accept(TEST_ACCEL_1);
+        testAccel.accept(TEST_ACCEL_1, TEST_ACCEL_1);
+
+        setAccel.accept(TEST_ACCEL_2);
+        testAccel.accept(TEST_ACCEL_2, TEST_ACCEL_2);
+
+        setAccel.accept(TEST_ACCEL_3);
+        testAccel.accept(TEST_ACCEL_3, TEST_ACCEL_3);
+
+        setAccel.accept(TEST_ACCEL_4);
+        testAccel.accept(TEST_ACCEL_4, TEST_ACCEL_4);
+
+        setAccel.accept(TEST_ACCEL_5);
+        testAccel.accept(TEST_ACCEL_5, TEST_ACCEL_5);
+    }
+    */
+    //</editor-fold>
+
+    //<editor-fold desc="Misc Tests">
+    @Test
+    void testReset() {
+        sim.setGyroAngleX(33);
+        assertDoesNotThrow(() -> imu.reset());
+        assertEquals(0, wpiIMU.getAccelX(), "X reset failed.");
+        assertEquals(0, wpiIMU.getAccelY(), "Y reset failed.");
+        assertEquals(0, wpiIMU.getAccelZ(), "Z reset failed.");
+    }
+
+    @Test
+    void testIMU() {
+        assertEquals(wpiIMU, imu.getIMU());
+    }
+
+    @Test
+    void testToCartesian() {
+        assertEquals(IMU.CartesianAxis.X, ADIS16470IMU.toCartesian(ADIS16470_IMU.IMUAxis.kX));
+        assertEquals(IMU.CartesianAxis.Y, ADIS16470IMU.toCartesian(ADIS16470_IMU.IMUAxis.kY));
+        assertEquals(IMU.CartesianAxis.Z, ADIS16470IMU.toCartesian(ADIS16470_IMU.IMUAxis.kZ));
+    }
+
+    @Test
+    void testFromCartesian() {
+        assertEquals(ADIS16470_IMU.IMUAxis.kX, ADIS16470IMU.fromCartesian(IMU.CartesianAxis.X));
+        assertEquals(ADIS16470_IMU.IMUAxis.kY, ADIS16470IMU.fromCartesian(IMU.CartesianAxis.Y));
+        assertEquals(ADIS16470_IMU.IMUAxis.kZ, ADIS16470IMU.fromCartesian(IMU.CartesianAxis.Z));
+    }
+}
+
+
diff --git a/src/test/java/net/frc5183/librobot/hardware/gyro/IMUTests.java b/src/test/java/net/frc5183/librobot/hardware/gyro/IMUTests.java
new file mode 100644
index 0000000..62d9a4c
--- /dev/null
+++ b/src/test/java/net/frc5183/librobot/hardware/gyro/IMUTests.java
@@ -0,0 +1,22 @@
+package net.frc5183.librobot.hardware.gyro;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class IMUTests {
+    @Test
+    void testAssignCartesianAxes() {
+        IMU.CartesianAxis[] yawX = IMU.CartesianAxis.assignAxes(IMU.CartesianAxis.X);
+        assertEquals(IMU.CartesianAxis.Y, yawX[0], "Incorrect pitch when yaw is X.");
+        assertEquals(IMU.CartesianAxis.Z, yawX[1], "Incorrect roll when yaw is X.");
+
+        IMU.CartesianAxis[] yawY = IMU.CartesianAxis.assignAxes(IMU.CartesianAxis.Y);
+        assertEquals(IMU.CartesianAxis.X, yawY[0], "Incorrect pitch when yaw is Y.");
+        assertEquals(IMU.CartesianAxis.Z, yawY[1], "Incorrect roll when yaw is Y.");
+
+        IMU.CartesianAxis[] yawZ = IMU.CartesianAxis.assignAxes(IMU.CartesianAxis.Z);
+        assertEquals(IMU.CartesianAxis.X, yawZ[0], "Incorrect pitch when yaw is Z.");
+        assertEquals(IMU.CartesianAxis.Y, yawZ[1], "Incorrect roll when yaw is Z.");
+    }
+}

From ed1a44c07c2c11f9a4ecff3042450733ca5bf5ba Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Mon, 14 Oct 2024 09:25:27 -0400
Subject: [PATCH 23/27] fix(hardware/gyro): implement getRate() function

---
 .../net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java    | 5 +++++
 .../net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java    | 5 +++++
 src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java   | 6 ++++++
 .../java/net/frc5183/librobot/hardware/gyro/NavXIMU.java    | 5 +++++
 4 files changed, 21 insertions(+)

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java
index 761d30d..a600704 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java
@@ -114,6 +114,11 @@ public double getRawAngleRadians(CartesianAxis axis) {
         );
     }
 
+    @Override
+    public double getRate() {
+        return imu.getRate();
+    }
+
     @Override
     public @NotNull Translation3d getAccelerationMetersPerSecondSquared() {
         return new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ());
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java
index b593789..f1bae55 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java
@@ -129,6 +129,11 @@ public double getRawAngleRadians(@NotNull CartesianAxis axis) {
         );
     }
 
+    @Override
+    public double getRate() {
+        return imu.getRate();
+    }
+
     @Override
     public @NotNull Translation3d getAccelerationMetersPerSecondSquared() {
         return new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ());
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java
index 1c46f74..a9c6d59 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java
@@ -114,6 +114,12 @@ public Rotation3d getRotation3d() {
         return getRotation3dRadians();
     }
 
+    /**
+     * Returns the rotation rate of the IMU in degrees per second.
+     * @return the rotation rate of the IMU in degrees per second.
+     */
+    public abstract double getRate();
+
     /**
      * Gets the acceleration from the IMU in meters per second squared.
      * @return {@link Translation3d} of the acceleration from the IMU in meters per second squared.
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java
index 390f411..e87f48a 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java
@@ -119,6 +119,11 @@ public double getRawAngleRadians(@NotNull CartesianAxis axis) {
         );
     }
 
+    @Override
+    public double getRate() {
+        return imu.getRate();
+    }
+
     @Override
     public @NotNull Translation3d getAccelerationMetersPerSecondSquared() {
         return new Translation3d(

From 4e72ffa1b59bdcf756688974c62f40c4c6f913bc Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Mon, 14 Oct 2024 12:19:51 -0400
Subject: [PATCH 24/27] fix(hardware/gyro): (re)implement angle rate logic

---
 .../librobot/hardware/gyro/ADIS16448IMU.java  | 19 ++++++++-
 .../librobot/hardware/gyro/ADIS16470IMU.java  |  9 +++-
 .../frc5183/librobot/hardware/gyro/IMU.java   | 24 +++++++++--
 .../librobot/hardware/gyro/NavXIMU.java       | 41 ++++++++++++++++---
 4 files changed, 81 insertions(+), 12 deletions(-)

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java
index a600704..c9e47c2 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java
@@ -115,8 +115,23 @@ public double getRawAngleRadians(CartesianAxis axis) {
     }
 
     @Override
-    public double getRate() {
-        return imu.getRate();
+    public double getRateDegreesPerSecond(Attitude axis) {
+        CartesianAxis cartesianAxis = switch (axis) {
+            case YAW -> yaw;
+            case PITCH -> pitch;
+            case ROLL -> roll;
+        };
+
+        return getRateDegreesPerSecond(cartesianAxis);
+    }
+
+    @Override
+    public double getRateDegreesPerSecond(CartesianAxis axis) {
+        return switch (axis) {
+            case X -> imu.getGyroRateX();
+            case Y -> imu.getGyroRateY();
+            case Z -> imu.getGyroRateZ();
+        };
     }
 
     @Override
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java
index f1bae55..731673c 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java
@@ -130,8 +130,13 @@ public double getRawAngleRadians(@NotNull CartesianAxis axis) {
     }
 
     @Override
-    public double getRate() {
-        return imu.getRate();
+    public double getRateDegreesPerSecond(Attitude axis) {
+        return imu.getRate(fromAttitude(axis));
+    }
+
+    @Override
+    public double getRateDegreesPerSecond(CartesianAxis axis) {
+        return imu.getRate(fromCartesian(axis));
     }
 
     @Override
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java
index a9c6d59..ae406ae 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java
@@ -115,10 +115,28 @@ public Rotation3d getRotation3d() {
     }
 
     /**
-     * Returns the rotation rate of the IMU in degrees per second.
-     * @return the rotation rate of the IMU in degrees per second.
+     * Returns the rotation rate of the specified axis in degrees per second.
+     * @param axis the {@link Attitude} to get the rotation rate of.
+     * @return the rotation rate of the specified axis in degrees per second.
      */
-    public abstract double getRate();
+    public abstract double getRateDegreesPerSecond(Attitude axis);
+
+    /**
+     * Returns the rotation rate of the specified axis in degrees per second.
+     * @param axis the {@link CartesianAxis} to get the rotation rate of.
+     * @return the rotation rate of the specified axis in degrees per second.
+     */
+    public abstract double getRateDegreesPerSecond(CartesianAxis axis);
+
+    /**
+     * Returns the rotation rate of the yaw axis in degrees per second.
+     * @return the rotation rate of the yaw axis in degrees per second.
+     * @deprecated this only exists for YAGSL, use {@link #getRateDegreesPerSecond(Attitude)} instead (since that's what this method calls internally).
+     * @see #getRateDegreesPerSecond(Attitude)
+     */
+    public double getRate() {
+        return getRateDegreesPerSecond(Attitude.YAW);
+    }
 
     /**
      * Gets the acceleration from the IMU in meters per second squared.
diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java
index e87f48a..86786a6 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java
@@ -89,9 +89,9 @@ public NavXIMU(@NotNull SerialPort.Port serialPort) {
     @Override
     public double getRawAngleRadians(@NotNull Attitude axis) {
         return switch (axis) {
-            case YAW -> imu.getYaw();
-            case PITCH -> imu.getPitch();
-            case ROLL -> imu.getRoll();
+            case YAW -> Math.toRadians(convertNavXDegrees(imu.getYaw()));
+            case PITCH -> Math.toRadians(convertNavXDegrees(imu.getPitch()));
+            case ROLL -> Math.toRadians(convertNavXDegrees(imu.getRoll()));
         };
     }
 
@@ -120,8 +120,27 @@ public double getRawAngleRadians(@NotNull CartesianAxis axis) {
     }
 
     @Override
-    public double getRate() {
-        return imu.getRate();
+    public double getRateDegreesPerSecond(Attitude axis) {
+        return switch (axis) {
+            case YAW -> imu.getRate();
+            case PITCH -> throw new UnsupportedOperationException("NavX does not support getting pitch rate.");
+            case ROLL -> throw new UnsupportedOperationException("NavX does not support getting roll rate.");
+        };
+    }
+
+    @Override
+    public double getRateDegreesPerSecond(CartesianAxis axis) {
+        Attitude attitude;
+
+        if (axis == yaw) {
+            attitude = Attitude.YAW;
+        } else if (axis == pitch) {
+            attitude = Attitude.PITCH;
+        } else {
+            attitude = Attitude.ROLL;
+        }
+
+        return getRateDegreesPerSecond(attitude);
     }
 
     @Override
@@ -183,6 +202,18 @@ public void clearStickyFaults() {
         return imu;
     }
 
+    /**
+     * The NavX returns degrees from -180 to 180, however we return degrees from 0 to 360, so we need to convert it.
+     * @param navXDegrees the angle from -180 to 180 degrees
+     * @return the angle from 0 to 360 degrees.
+     */
+    private double convertNavXDegrees(double navXDegrees) {
+        if (navXDegrees < 0) {
+            return navXDegrees + 360;
+        }
+
+        return navXDegrees;
+    }
 
     /**
      * Returns a new {@link CartesianAxis} from an {@link AHRS.BoardYawAxis}.

From 7ae15eb6d77a2be8f994379d2b3c10931a6f256e Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Mon, 14 Oct 2024 12:20:28 -0400
Subject: [PATCH 25/27] test(hardware/gyro): finish implementing unit tests for
 all implemented gyroscopes

---
 .../hardware/gyro/ADIS16448Tests.java         | 111 +++
 .../hardware/gyro/ADIS16470Tests.java         | 110 +++
 .../librobot/hardware/gyro/NavXTests.java     | 639 ++++++++++++++++++
 3 files changed, 860 insertions(+)
 create mode 100644 src/test/java/net/frc5183/librobot/hardware/gyro/NavXTests.java

diff --git a/src/test/java/net/frc5183/librobot/hardware/gyro/ADIS16448Tests.java b/src/test/java/net/frc5183/librobot/hardware/gyro/ADIS16448Tests.java
index 2eda694..5766e7c 100644
--- a/src/test/java/net/frc5183/librobot/hardware/gyro/ADIS16448Tests.java
+++ b/src/test/java/net/frc5183/librobot/hardware/gyro/ADIS16448Tests.java
@@ -376,6 +376,116 @@ void testAngleOffset() {
     }
     //</editor-fold>
 
+    //<editor-fold desc="Angle Rate Tests">
+    @Test
+    void testRateCartesianX() {
+        sim.setGyroRateX(TEST_ANGLE_1/2);
+        assertEquals(TEST_ANGLE_1/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.X), DELTA);
+
+        sim.setGyroRateX(TEST_ANGLE_2/2);
+        assertEquals(TEST_ANGLE_2/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.X), DELTA);
+
+        sim.setGyroRateX(TEST_ANGLE_3/2);
+        assertEquals(TEST_ANGLE_3/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.X), DELTA);
+
+        sim.setGyroRateX(TEST_ANGLE_4/2);
+        assertEquals(TEST_ANGLE_4/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.X), DELTA);
+
+        sim.setGyroRateX(TEST_ANGLE_5/2);
+        assertEquals(TEST_ANGLE_5/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.X), DELTA);
+    }
+
+    @Test
+    void testRateCartesianY() {
+        sim.setGyroRateY(TEST_ANGLE_1/2);
+        assertEquals(TEST_ANGLE_1/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Y), DELTA);
+
+        sim.setGyroRateY(TEST_ANGLE_2/2);
+        assertEquals(TEST_ANGLE_2/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Y), DELTA);
+
+        sim.setGyroRateY(TEST_ANGLE_3/2);
+        assertEquals(TEST_ANGLE_3/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Y), DELTA);
+
+        sim.setGyroRateY(TEST_ANGLE_4/2);
+        assertEquals(TEST_ANGLE_4/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Y), DELTA);
+
+        sim.setGyroRateY(TEST_ANGLE_5/2);
+        assertEquals(TEST_ANGLE_5/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Y), DELTA);
+    }
+
+    @Test
+    void testRateCartesianZ() {
+        sim.setGyroRateZ(TEST_ANGLE_1/2);
+        assertEquals(TEST_ANGLE_1/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Z), DELTA);
+
+        sim.setGyroRateZ(TEST_ANGLE_2/2);
+        assertEquals(TEST_ANGLE_2/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Z), DELTA);
+
+        sim.setGyroRateZ(TEST_ANGLE_3/2);
+        assertEquals(TEST_ANGLE_3/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Z), DELTA);
+
+        sim.setGyroRateZ(TEST_ANGLE_4/2);
+        assertEquals(TEST_ANGLE_4/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Z), DELTA);
+
+        sim.setGyroRateZ(TEST_ANGLE_5/2);
+        assertEquals(TEST_ANGLE_5/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Z), DELTA);
+    }
+
+    @Test
+    void testRateYaw() {
+        sim.setGyroRateZ(TEST_ANGLE_1/2);
+        assertEquals(TEST_ANGLE_1/2, imu.getRateDegreesPerSecond(IMU.Attitude.YAW), DELTA);
+
+        sim.setGyroRateZ(TEST_ANGLE_2/2);
+        assertEquals(TEST_ANGLE_2/2, imu.getRateDegreesPerSecond(IMU.Attitude.YAW), DELTA);
+
+        sim.setGyroRateZ(TEST_ANGLE_3/2);
+        assertEquals(TEST_ANGLE_3/2, imu.getRateDegreesPerSecond(IMU.Attitude.YAW), DELTA);
+
+        sim.setGyroRateZ(TEST_ANGLE_4/2);
+        assertEquals(TEST_ANGLE_4/2, imu.getRateDegreesPerSecond(IMU.Attitude.YAW), DELTA);
+
+        sim.setGyroRateZ(TEST_ANGLE_5/2);
+        assertEquals(TEST_ANGLE_5/2, imu.getRateDegreesPerSecond(IMU.Attitude.YAW), DELTA);
+    }
+
+    @Test
+    void testRatePitch() {
+        sim.setGyroRateX(TEST_ANGLE_1/2);
+        assertEquals(TEST_ANGLE_1/2, imu.getRateDegreesPerSecond(IMU.Attitude.PITCH), DELTA);
+
+        sim.setGyroRateX(TEST_ANGLE_2/2);
+        assertEquals(TEST_ANGLE_2/2, imu.getRateDegreesPerSecond(IMU.Attitude.PITCH), DELTA);
+
+        sim.setGyroRateX(TEST_ANGLE_3/2);
+        assertEquals(TEST_ANGLE_3/2, imu.getRateDegreesPerSecond(IMU.Attitude.PITCH), DELTA);
+
+        sim.setGyroRateX(TEST_ANGLE_4/2);
+        assertEquals(TEST_ANGLE_4/2, imu.getRateDegreesPerSecond(IMU.Attitude.PITCH), DELTA);
+
+        sim.setGyroRateX(TEST_ANGLE_5/2);
+        assertEquals(TEST_ANGLE_5/2, imu.getRateDegreesPerSecond(IMU.Attitude.PITCH), DELTA);
+    }
+
+    @Test
+    void testRateRoll() {
+        sim.setGyroRateY(TEST_ANGLE_1 / 2);
+        assertEquals(TEST_ANGLE_1 / 2, imu.getRateDegreesPerSecond(IMU.Attitude.ROLL), DELTA);
+
+        sim.setGyroRateY(TEST_ANGLE_2 / 2);
+        assertEquals(TEST_ANGLE_2 / 2, imu.getRateDegreesPerSecond(IMU.Attitude.ROLL), DELTA);
+
+        sim.setGyroRateY(TEST_ANGLE_3 / 2);
+        assertEquals(TEST_ANGLE_3 / 2, imu.getRateDegreesPerSecond(IMU.Attitude.ROLL), DELTA);
+
+        sim.setGyroRateY(TEST_ANGLE_4 / 2);
+        assertEquals(TEST_ANGLE_4 / 2, imu.getRateDegreesPerSecond(IMU.Attitude.ROLL), DELTA);
+
+        sim.setGyroRateY(TEST_ANGLE_5 / 2);
+        assertEquals(TEST_ANGLE_5 / 2, imu.getRateDegreesPerSecond(IMU.Attitude.ROLL), DELTA);
+    }
+    //</editor-fold>
+
     //<editor-fold desc="Acceleration Tests">
     static final double TEST_ACCEL_1 = 0;
     static final double TEST_ACCEL_2 = 1;
@@ -523,6 +633,7 @@ void testFromCartesian() {
         assertEquals(ADIS16448_IMU.IMUAxis.kY, ADIS16448IMU.fromCartesian(IMU.CartesianAxis.Y));
         assertEquals(ADIS16448_IMU.IMUAxis.kZ, ADIS16448IMU.fromCartesian(IMU.CartesianAxis.Z));
     }
+    //</editor-fold>
 }
 
 
diff --git a/src/test/java/net/frc5183/librobot/hardware/gyro/ADIS16470Tests.java b/src/test/java/net/frc5183/librobot/hardware/gyro/ADIS16470Tests.java
index e3de762..0640241 100644
--- a/src/test/java/net/frc5183/librobot/hardware/gyro/ADIS16470Tests.java
+++ b/src/test/java/net/frc5183/librobot/hardware/gyro/ADIS16470Tests.java
@@ -382,6 +382,116 @@ void testAngleOffset() {
     }
     //</editor-fold>
 
+    //<editor-fold desc="Angle Rate Tests">
+    @Test
+    void testRateCartesianX() {
+        sim.setGyroRateX(TEST_ANGLE_1/2);
+        assertEquals(TEST_ANGLE_1/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.X), DELTA);
+
+        sim.setGyroRateX(TEST_ANGLE_2/2);
+        assertEquals(TEST_ANGLE_2/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.X), DELTA);
+
+        sim.setGyroRateX(TEST_ANGLE_3/2);
+        assertEquals(TEST_ANGLE_3/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.X), DELTA);
+
+        sim.setGyroRateX(TEST_ANGLE_4/2);
+        assertEquals(TEST_ANGLE_4/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.X), DELTA);
+
+        sim.setGyroRateX(TEST_ANGLE_5/2);
+        assertEquals(TEST_ANGLE_5/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.X), DELTA);
+    }
+
+    @Test
+    void testRateCartesianY() {
+        sim.setGyroRateY(TEST_ANGLE_1/2);
+        assertEquals(TEST_ANGLE_1/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Y), DELTA);
+
+        sim.setGyroRateY(TEST_ANGLE_2/2);
+        assertEquals(TEST_ANGLE_2/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Y), DELTA);
+
+        sim.setGyroRateY(TEST_ANGLE_3/2);
+        assertEquals(TEST_ANGLE_3/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Y), DELTA);
+
+        sim.setGyroRateY(TEST_ANGLE_4/2);
+        assertEquals(TEST_ANGLE_4/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Y), DELTA);
+
+        sim.setGyroRateY(TEST_ANGLE_5/2);
+        assertEquals(TEST_ANGLE_5/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Y), DELTA);
+    }
+
+    @Test
+    void testRateCartesianZ() {
+        sim.setGyroRateZ(TEST_ANGLE_1/2);
+        assertEquals(TEST_ANGLE_1/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Z), DELTA);
+
+        sim.setGyroRateZ(TEST_ANGLE_2/2);
+        assertEquals(TEST_ANGLE_2/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Z), DELTA);
+
+        sim.setGyroRateZ(TEST_ANGLE_3/2);
+        assertEquals(TEST_ANGLE_3/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Z), DELTA);
+
+        sim.setGyroRateZ(TEST_ANGLE_4/2);
+        assertEquals(TEST_ANGLE_4/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Z), DELTA);
+
+        sim.setGyroRateZ(TEST_ANGLE_5/2);
+        assertEquals(TEST_ANGLE_5/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Z), DELTA);
+    }
+
+    @Test
+    void testRateYaw() {
+        sim.setGyroRateZ(TEST_ANGLE_1/2);
+        assertEquals(TEST_ANGLE_1/2, imu.getRateDegreesPerSecond(IMU.Attitude.YAW), DELTA);
+
+        sim.setGyroRateZ(TEST_ANGLE_2/2);
+        assertEquals(TEST_ANGLE_2/2, imu.getRateDegreesPerSecond(IMU.Attitude.YAW), DELTA);
+
+        sim.setGyroRateZ(TEST_ANGLE_3/2);
+        assertEquals(TEST_ANGLE_3/2, imu.getRateDegreesPerSecond(IMU.Attitude.YAW), DELTA);
+
+        sim.setGyroRateZ(TEST_ANGLE_4/2);
+        assertEquals(TEST_ANGLE_4/2, imu.getRateDegreesPerSecond(IMU.Attitude.YAW), DELTA);
+
+        sim.setGyroRateZ(TEST_ANGLE_5/2);
+        assertEquals(TEST_ANGLE_5/2, imu.getRateDegreesPerSecond(IMU.Attitude.YAW), DELTA);
+    }
+
+    @Test
+    void testRatePitch() {
+        sim.setGyroRateX(TEST_ANGLE_1/2);
+        assertEquals(TEST_ANGLE_1/2, imu.getRateDegreesPerSecond(IMU.Attitude.PITCH), DELTA);
+
+        sim.setGyroRateX(TEST_ANGLE_2/2);
+        assertEquals(TEST_ANGLE_2/2, imu.getRateDegreesPerSecond(IMU.Attitude.PITCH), DELTA);
+
+        sim.setGyroRateX(TEST_ANGLE_3/2);
+        assertEquals(TEST_ANGLE_3/2, imu.getRateDegreesPerSecond(IMU.Attitude.PITCH), DELTA);
+
+        sim.setGyroRateX(TEST_ANGLE_4/2);
+        assertEquals(TEST_ANGLE_4/2, imu.getRateDegreesPerSecond(IMU.Attitude.PITCH), DELTA);
+
+        sim.setGyroRateX(TEST_ANGLE_5/2);
+        assertEquals(TEST_ANGLE_5/2, imu.getRateDegreesPerSecond(IMU.Attitude.PITCH), DELTA);
+    }
+
+    @Test
+    void testRateRoll() {
+        sim.setGyroRateY(TEST_ANGLE_1 / 2);
+        assertEquals(TEST_ANGLE_1 / 2, imu.getRateDegreesPerSecond(IMU.Attitude.ROLL), DELTA);
+
+        sim.setGyroRateY(TEST_ANGLE_2 / 2);
+        assertEquals(TEST_ANGLE_2 / 2, imu.getRateDegreesPerSecond(IMU.Attitude.ROLL), DELTA);
+
+        sim.setGyroRateY(TEST_ANGLE_3 / 2);
+        assertEquals(TEST_ANGLE_3 / 2, imu.getRateDegreesPerSecond(IMU.Attitude.ROLL), DELTA);
+
+        sim.setGyroRateY(TEST_ANGLE_4 / 2);
+        assertEquals(TEST_ANGLE_4 / 2, imu.getRateDegreesPerSecond(IMU.Attitude.ROLL), DELTA);
+
+        sim.setGyroRateY(TEST_ANGLE_5 / 2);
+        assertEquals(TEST_ANGLE_5 / 2, imu.getRateDegreesPerSecond(IMU.Attitude.ROLL), DELTA);
+    }
+    //</editor-fold>
+
     // TODO: Acceleration tests are currently disabled due to a bug in wpilibj.
     // https://github.com/wpilibsuite/allwpilib/issues/7180
     //<editor-fold desc="Acceleration Tests">
diff --git a/src/test/java/net/frc5183/librobot/hardware/gyro/NavXTests.java b/src/test/java/net/frc5183/librobot/hardware/gyro/NavXTests.java
new file mode 100644
index 0000000..5325fd0
--- /dev/null
+++ b/src/test/java/net/frc5183/librobot/hardware/gyro/NavXTests.java
@@ -0,0 +1,639 @@
+package net.frc5183.librobot.hardware.gyro;
+
+import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.hal.simulation.SimDeviceDataJNI;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.TestInfo;
+
+import java.util.function.BiConsumer;
+import java.util.function.Consumer;
+
+import static org.junit.jupiter.api.Assertions.*;
+
+class NavXTests {
+    static final double DELTA = 1E-4;
+
+    static boolean canResetSimData = false;
+
+    NavXIMU imu;
+    AHRS wpiIMU;
+    NavXSim sim;
+
+    @BeforeEach
+    void setup(TestInfo info) {
+        if (info.getTags().contains("noBefore")) return;
+
+        if (canResetSimData)
+            SimDeviceSim.resetData();
+        else canResetSimData = true;
+
+        wpiIMU = new AHRS();
+        sim = new NavXSim();
+        imu = new NavXIMU(wpiIMU);
+    }
+
+    //<editor-fold desc="Angle Tests">
+    static final double TEST_ANGLE_1 = 0;
+    static final double TEST_ANGLE_2 = 90;
+    static final double TEST_ANGLE_3 = 180;
+    static final double TEST_ANGLE_4 = 270;
+    static final double TEST_ANGLE_5 = Math.random() * 360;
+
+    @Test
+    void testAngleYaw() {
+        sim.setYaw(TEST_ANGLE_1);
+        assertEquals(TEST_ANGLE_1, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.YAW)), DELTA);
+
+        sim.setYaw(TEST_ANGLE_2);
+        assertEquals(TEST_ANGLE_2, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.YAW)), DELTA);
+
+        sim.setYaw(TEST_ANGLE_3);
+        assertEquals(TEST_ANGLE_3, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.YAW)), DELTA);
+
+        sim.setYaw(TEST_ANGLE_4);
+        assertEquals(TEST_ANGLE_4, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.YAW)), DELTA);
+
+        sim.setYaw(TEST_ANGLE_5);
+        assertEquals(TEST_ANGLE_5, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.YAW)), DELTA);
+    }
+
+    @Test
+    void testAnglePitch() {
+        sim.setPitch(TEST_ANGLE_1);
+        assertEquals(TEST_ANGLE_1, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.PITCH)), DELTA);
+
+        sim.setPitch(TEST_ANGLE_2);
+        assertEquals(TEST_ANGLE_2, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.PITCH)), DELTA);
+
+        sim.setPitch(TEST_ANGLE_3);
+        assertEquals(TEST_ANGLE_3, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.PITCH)), DELTA);
+
+        sim.setPitch(TEST_ANGLE_4);
+        assertEquals(TEST_ANGLE_4, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.PITCH)), DELTA);
+
+        sim.setPitch(TEST_ANGLE_5);
+        assertEquals(TEST_ANGLE_5, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.PITCH)), DELTA);
+    }
+
+    @Test
+    void testAngleRoll() {
+        sim.setRoll(TEST_ANGLE_1);
+        assertEquals(TEST_ANGLE_1, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.ROLL)), DELTA);
+
+        sim.setRoll(TEST_ANGLE_2);
+        assertEquals(TEST_ANGLE_2, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.ROLL)), DELTA);
+
+        sim.setRoll(TEST_ANGLE_3);
+        assertEquals(TEST_ANGLE_3, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.ROLL)), DELTA);
+
+        sim.setRoll(TEST_ANGLE_4);
+        assertEquals(TEST_ANGLE_4, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.ROLL)), DELTA);
+
+        sim.setRoll(TEST_ANGLE_5);
+        assertEquals(TEST_ANGLE_5, Math.toDegrees(imu.getAngleRadians(IMU.Attitude.ROLL)), DELTA);
+    }
+
+
+    // Default Omnimount for NavX is X = PITCH, Y = ROLL, Z = YAW.
+    @Test
+    void testAngleCartesianX() {
+        sim.setPitch(TEST_ANGLE_1);
+        assertEquals(TEST_ANGLE_1, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.X)), DELTA);
+
+        sim.setPitch(TEST_ANGLE_2);
+        assertEquals(TEST_ANGLE_2, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.X)), DELTA);
+
+        sim.setPitch(TEST_ANGLE_3);
+        assertEquals(TEST_ANGLE_3, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.X)), DELTA);
+
+        sim.setPitch(TEST_ANGLE_4);
+        assertEquals(TEST_ANGLE_4, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.X)), DELTA);
+
+        sim.setPitch(TEST_ANGLE_5);
+        assertEquals(TEST_ANGLE_5, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.X)), DELTA);
+    }
+
+    @Test
+    void testAngleCartesianY() {
+        sim.setRoll(TEST_ANGLE_1);
+        assertEquals(TEST_ANGLE_1, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Y)), DELTA);
+
+        sim.setRoll(TEST_ANGLE_2);
+        assertEquals(TEST_ANGLE_2, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Y)), DELTA);
+
+        sim.setRoll(TEST_ANGLE_3);
+        assertEquals(TEST_ANGLE_3, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Y)), DELTA);
+
+        sim.setRoll(TEST_ANGLE_4);
+        assertEquals(TEST_ANGLE_4, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Y)), DELTA);
+
+        sim.setRoll(TEST_ANGLE_5);
+        assertEquals(TEST_ANGLE_5, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Y)), DELTA);
+    }
+
+    @Test
+    void testAngleCartesianZ() {
+        sim.setYaw(TEST_ANGLE_1);
+        assertEquals(TEST_ANGLE_1, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Z)), DELTA);
+
+        sim.setYaw(TEST_ANGLE_2);
+        assertEquals(TEST_ANGLE_2, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Z)), DELTA);
+
+        sim.setYaw(TEST_ANGLE_3);
+        assertEquals(TEST_ANGLE_3, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Z)), DELTA);
+
+        sim.setYaw(TEST_ANGLE_4);
+        assertEquals(TEST_ANGLE_4, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Z)), DELTA);
+
+        sim.setYaw(TEST_ANGLE_5);
+        assertEquals(TEST_ANGLE_5, Math.toDegrees(imu.getAngleRadians(IMU.CartesianAxis.Z)), DELTA);
+    }
+
+    @Test
+    void testAngleRotation3d() {
+        // Degrees
+        Consumer<Double> setAngle = (Double angle) -> {
+            sim.setYaw(angle);
+            sim.setPitch(angle);
+            sim.setRoll(angle);
+        };
+
+        // Radians
+        Consumer<Double> verifyRot = (Double angle) -> {
+            assertEquals(
+                    new Rotation3d(
+                            Math.toRadians(angle),
+                            Math.toRadians(angle),
+                            Math.toRadians(angle)
+                    ),
+                    imu.getRotation3dRadians(),
+                    "Rotation3d failed."
+            );
+        };
+
+        setAngle.accept(TEST_ANGLE_1);
+        verifyRot.accept(TEST_ANGLE_1);
+
+        setAngle.accept(TEST_ANGLE_2);
+        verifyRot.accept(TEST_ANGLE_2);
+
+        setAngle.accept(TEST_ANGLE_3);
+        verifyRot.accept(TEST_ANGLE_3);
+
+        setAngle.accept(TEST_ANGLE_4);
+        verifyRot.accept(TEST_ANGLE_4);
+
+        setAngle.accept(TEST_ANGLE_5);
+        verifyRot.accept(TEST_ANGLE_5);
+    }
+
+    @Test
+    void testAngleOffset() {
+        // Radians.
+        Consumer<Double> setOffset = (Double offset) -> {
+            imu.setOffsetX((double) offset);
+            imu.setOffsetY((double) offset);
+            imu.setOffsetZ((double) offset);
+        };
+
+        // Degrees.
+        Consumer<Double> setAngle = (Double angle) -> {
+            sim.setYaw(angle);
+            sim.setPitch(angle);
+            sim.setRoll(angle);
+        };
+
+        // Radians.
+        BiConsumer<Double, Double> testAngles = (Double angle, Double offset) -> {
+            double expected = angle - offset;
+
+            assertEquals(
+                    expected,
+                    imu.getAngleRadians(IMU.CartesianAxis.X),
+                    DELTA,
+                    "X offset failed."
+            );
+
+            assertEquals(
+                    expected,
+                    imu.getAngleRadians(IMU.CartesianAxis.Y),
+                    DELTA,
+                    "Y offset failed."
+            );
+
+            assertEquals(
+                    expected,
+                    imu.getAngleRadians(IMU.CartesianAxis.Z),
+                    DELTA,
+                    "Z offset failed."
+            );
+
+            assertEquals(
+                    new Rotation3d(
+                            angle, angle, angle
+                    ).minus(new Rotation3d(offset, offset, offset)),
+                    imu.getRotation3dRadians(),
+                    "Rotation3d offset failed."
+            );
+        };
+
+
+        setOffset.accept(Math.toRadians(TEST_ANGLE_1));
+        //<editor-fold desc="TEST_ANGLE_1 as offset.">
+        setAngle.accept(TEST_ANGLE_1);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_1), Math.toRadians(TEST_ANGLE_1));
+
+        setAngle.accept(TEST_ANGLE_2);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_2), Math.toRadians(TEST_ANGLE_1));
+
+        setAngle.accept(TEST_ANGLE_3);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_3), Math.toRadians(TEST_ANGLE_1));
+
+        setAngle.accept(TEST_ANGLE_4);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_4), Math.toRadians(TEST_ANGLE_1));
+
+        setAngle.accept(TEST_ANGLE_5);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_5), Math.toRadians(TEST_ANGLE_1));
+        //</editor-fold>
+
+        setOffset.accept(Math.toRadians(TEST_ANGLE_2));
+        //<editor-fold desc="TEST_ANGLE_2 as offset.">
+        setAngle.accept(TEST_ANGLE_1);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_1), Math.toRadians(TEST_ANGLE_2));
+
+        setAngle.accept(TEST_ANGLE_2);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_2), Math.toRadians(TEST_ANGLE_2));
+
+        setAngle.accept(TEST_ANGLE_3);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_3), Math.toRadians(TEST_ANGLE_2));
+
+        setAngle.accept(TEST_ANGLE_4);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_4), Math.toRadians(TEST_ANGLE_2));
+
+        setAngle.accept(TEST_ANGLE_5);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_5), Math.toRadians(TEST_ANGLE_2));
+        //</editor-fold>
+
+        setOffset.accept(Math.toRadians(TEST_ANGLE_3));
+        //<editor-fold desc="TEST_ANGLE_3 as offset.">
+        setAngle.accept(TEST_ANGLE_1);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_1), Math.toRadians(TEST_ANGLE_3));
+
+        setAngle.accept(TEST_ANGLE_2);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_2), Math.toRadians(TEST_ANGLE_3));
+
+        setAngle.accept(TEST_ANGLE_3);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_3), Math.toRadians(TEST_ANGLE_3));
+
+        setAngle.accept(TEST_ANGLE_4);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_4), Math.toRadians(TEST_ANGLE_3));
+
+        setAngle.accept(TEST_ANGLE_5);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_5), Math.toRadians(TEST_ANGLE_3));
+        //</editor-fold>
+
+        setOffset.accept(Math.toRadians(TEST_ANGLE_4));
+        //<editor-fold desc="TEST_ANGLE_4 as offset.">
+        setAngle.accept(TEST_ANGLE_1);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_1), Math.toRadians(TEST_ANGLE_4));
+
+        setAngle.accept(TEST_ANGLE_2);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_2), Math.toRadians(TEST_ANGLE_4));
+
+        setAngle.accept(TEST_ANGLE_3);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_3), Math.toRadians(TEST_ANGLE_4));
+
+        setAngle.accept(TEST_ANGLE_4);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_4), Math.toRadians(TEST_ANGLE_4));
+
+        setAngle.accept(TEST_ANGLE_5);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_5), Math.toRadians(TEST_ANGLE_4));
+        //</editor-fold>
+
+        setOffset.accept(Math.toRadians(TEST_ANGLE_5));
+        //<editor-fold desc="TEST_ANGLE_5 as offset.">
+        setAngle.accept(TEST_ANGLE_1);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_1), Math.toRadians(TEST_ANGLE_5));
+
+        setAngle.accept(TEST_ANGLE_2);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_2), Math.toRadians(TEST_ANGLE_5));
+
+        setAngle.accept(TEST_ANGLE_3);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_3), Math.toRadians(TEST_ANGLE_5));
+
+        setAngle.accept(TEST_ANGLE_4);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_4), Math.toRadians(TEST_ANGLE_5));
+
+        setAngle.accept(TEST_ANGLE_5);
+        testAngles.accept(Math.toRadians(TEST_ANGLE_5), Math.toRadians(TEST_ANGLE_5));
+        //</editor-fold>
+    }
+
+    //</editor-fold>
+
+    //<editor-fold desc="Angle Rate Tests">
+    @Test
+    void testRateCartesianX() {
+        assertThrowsExactly(
+                UnsupportedOperationException.class,
+                () -> imu.getRateDegreesPerSecond(IMU.CartesianAxis.X)
+        );
+    }
+
+    @Test
+    void testRateCartesianY() {
+        assertThrowsExactly(
+                UnsupportedOperationException.class,
+                () -> imu.getRateDegreesPerSecond(IMU.CartesianAxis.Y)
+        );
+    }
+
+    // Only test YAW (aka Z with default omnimount) because the other axes are unsupported.
+    @Test
+    void testRateCartesianZ() {
+        sim.setRate(TEST_ANGLE_1/2);
+        assertEquals(TEST_ANGLE_1/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Z), DELTA);
+
+        sim.setRate(TEST_ANGLE_2/2);
+        assertEquals(TEST_ANGLE_2/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Z), DELTA);
+
+        sim.setRate(TEST_ANGLE_3/2);
+        assertEquals(TEST_ANGLE_3/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Z), DELTA);
+
+        sim.setRate(TEST_ANGLE_4/2);
+        assertEquals(TEST_ANGLE_4/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Z), DELTA);
+
+        sim.setRate(TEST_ANGLE_5/2);
+        assertEquals(TEST_ANGLE_5/2, imu.getRateDegreesPerSecond(IMU.CartesianAxis.Z), DELTA);
+    }
+
+    @Test
+    void testRateYaw() {
+        sim.setRate(TEST_ANGLE_1/2);
+        assertEquals(TEST_ANGLE_1/2, imu.getRateDegreesPerSecond(IMU.Attitude.YAW), DELTA);
+
+        sim.setRate(TEST_ANGLE_2/2);
+        assertEquals(TEST_ANGLE_2/2, imu.getRateDegreesPerSecond(IMU.Attitude.YAW), DELTA);
+
+        sim.setRate(TEST_ANGLE_3/2);
+        assertEquals(TEST_ANGLE_3/2, imu.getRateDegreesPerSecond(IMU.Attitude.YAW), DELTA);
+
+        sim.setRate(TEST_ANGLE_4/2);
+        assertEquals(TEST_ANGLE_4/2, imu.getRateDegreesPerSecond(IMU.Attitude.YAW), DELTA);
+
+        sim.setRate(TEST_ANGLE_5/2);
+        assertEquals(TEST_ANGLE_5/2, imu.getRateDegreesPerSecond(IMU.Attitude.YAW), DELTA);
+    }
+
+    @Test
+    void testRatePitch() {
+        assertThrowsExactly(
+                UnsupportedOperationException.class,
+                () -> imu.getRateDegreesPerSecond(IMU.Attitude.PITCH)
+        );
+    }
+
+    @Test
+    void testRateRoll() {
+        assertThrowsExactly(
+                UnsupportedOperationException.class,
+                () -> imu.getRateDegreesPerSecond(IMU.Attitude.ROLL)
+        );
+    }
+    //</editor-fold>
+
+    //<editor-fold desc="Acceleration Tests">
+    static final double TEST_ACCEL_1 = 0;
+    static final double TEST_ACCEL_2 = 1;
+    static final double TEST_ACCEL_3 = 2;
+    static final double TEST_ACCEL_4 = 3;
+    static final double TEST_ACCEL_5 = Math.random() * 3;
+
+    @Test
+    void testAccelerationCartesianX() {
+        sim.setAccelX(TEST_ACCEL_1);
+        assertEquals(TEST_ACCEL_1, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X), DELTA);
+
+        sim.setAccelX(TEST_ACCEL_2);
+        assertEquals(TEST_ACCEL_2, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X), DELTA);
+
+        sim.setAccelX(TEST_ACCEL_3);
+        assertEquals(TEST_ACCEL_3, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X), DELTA);
+
+        sim.setAccelX(TEST_ACCEL_4);
+        assertEquals(TEST_ACCEL_4, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X), DELTA);
+
+        sim.setAccelX(TEST_ACCEL_5);
+        assertEquals(TEST_ACCEL_5, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X), DELTA);
+    }
+
+    @Test
+    void testAccelerationCartesianY() {
+        sim.setAccelY(TEST_ACCEL_1);
+        assertEquals(TEST_ACCEL_1, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y), DELTA);
+
+        sim.setAccelY(TEST_ACCEL_2);
+        assertEquals(TEST_ACCEL_2, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y), DELTA);
+
+        sim.setAccelY(TEST_ACCEL_3);
+        assertEquals(TEST_ACCEL_3, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y), DELTA);
+
+        sim.setAccelY(TEST_ACCEL_4);
+        assertEquals(TEST_ACCEL_4, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y), DELTA);
+
+        sim.setAccelY(TEST_ACCEL_5);
+        assertEquals(TEST_ACCEL_5, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y), DELTA);
+    }
+
+    @Test
+    void testAccelerationCartesianZ() {
+        sim.setAccelZ(TEST_ACCEL_1);
+        assertEquals(TEST_ACCEL_1, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z), DELTA);
+
+        sim.setAccelZ(TEST_ACCEL_2);
+        assertEquals(TEST_ACCEL_2, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z), DELTA);
+
+        sim.setAccelZ(TEST_ACCEL_3);
+        assertEquals(TEST_ACCEL_3, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z), DELTA);
+
+        sim.setAccelZ(TEST_ACCEL_4);
+        assertEquals(TEST_ACCEL_4, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z), DELTA);
+
+        sim.setAccelZ(TEST_ACCEL_5);
+        assertEquals(TEST_ACCEL_5, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z), DELTA);
+    }
+
+    @Test
+    void testAccelerationTranslation3d() {
+        // Meters per second squared.
+        Consumer<Double> setAccel = (Double accel) -> {
+            sim.setAccelX(accel);
+            sim.setAccelY(accel);
+            sim.setAccelZ(accel);
+        };
+
+        // Meters per second squared.
+        BiConsumer<Double, Double> testAccel = (Double accel, Double expected) -> {
+            Translation3d translation = imu.getAccelerationMetersPerSecondSquared();
+
+            assertEquals(
+                    accel,
+                    translation.getX(),
+                    DELTA,
+                    "X failed."
+            );
+
+            assertEquals(
+                    accel,
+                    translation.getY(),
+                    DELTA,
+                    "Y failed."
+            );
+
+            assertEquals(
+                    accel,
+                    translation.getZ(),
+                    DELTA,
+                    "Z failed."
+            );
+        };
+
+        setAccel.accept(TEST_ACCEL_1);
+        testAccel.accept(TEST_ACCEL_1, TEST_ACCEL_1);
+
+        setAccel.accept(TEST_ACCEL_2);
+        testAccel.accept(TEST_ACCEL_2, TEST_ACCEL_2);
+
+        setAccel.accept(TEST_ACCEL_3);
+        testAccel.accept(TEST_ACCEL_3, TEST_ACCEL_3);
+
+        setAccel.accept(TEST_ACCEL_4);
+        testAccel.accept(TEST_ACCEL_4, TEST_ACCEL_4);
+
+        setAccel.accept(TEST_ACCEL_5);
+        testAccel.accept(TEST_ACCEL_5, TEST_ACCEL_5);
+    }
+    //</editor-fold>
+
+    //<editor-fold desc="Misc Tests">
+    @Test
+    void testReset() {
+        sim.setYaw(33);
+        assertDoesNotThrow(() -> imu.reset());
+    }
+
+    @Test
+    void testIMU() {
+        assertEquals(wpiIMU, imu.getIMU());
+    }
+
+    @Test
+    void testToCartesian() {
+        assertEquals(IMU.CartesianAxis.X, NavXIMU.toCartesian(AHRS.BoardAxis.kBoardAxisX));
+        assertEquals(IMU.CartesianAxis.Y, NavXIMU.toCartesian(AHRS.BoardAxis.kBoardAxisY));
+        assertEquals(IMU.CartesianAxis.Z, NavXIMU.toCartesian(AHRS.BoardAxis.kBoardAxisZ));
+    }
+
+    @Test
+    void testFromCartesian() {
+        assertEquals(AHRS.BoardAxis.kBoardAxisX, NavXIMU.fromCartesian(IMU.CartesianAxis.X));
+        assertEquals(AHRS.BoardAxis.kBoardAxisY, NavXIMU.fromCartesian(IMU.CartesianAxis.Y));
+        assertEquals(AHRS.BoardAxis.kBoardAxisZ, NavXIMU.fromCartesian(IMU.CartesianAxis.Z));
+    }
+    //</editor-fold>
+
+    /**
+     * Class to control a simulated NavX gyroscope.
+     */
+    class NavXSim {
+        private static final int waitDuration = 45;
+
+        private final SimDouble yaw;
+        private final SimDouble pitch;
+        private final SimDouble roll;
+
+        private final SimDouble rate;
+
+        private final SimDouble accelX;
+        private final SimDouble accelY;
+        private final SimDouble accelZ;
+
+        public NavXSim() {
+            int deviceHandle = SimDeviceDataJNI.getSimDeviceHandle("navX-Sensor[0]");
+
+            yaw = new SimDouble(SimDeviceDataJNI.getSimValueHandle(deviceHandle, "Yaw"));
+            pitch = new SimDouble(SimDeviceDataJNI.getSimValueHandle(deviceHandle, "Pitch"));
+            roll = new SimDouble(SimDeviceDataJNI.getSimValueHandle(deviceHandle, "Roll"));
+
+            rate = new SimDouble(SimDeviceDataJNI.getSimValueHandle(deviceHandle, "Rate"));
+
+            accelX = new SimDouble(SimDeviceDataJNI.getSimValueHandle(deviceHandle, "LinearWorldAccelX"));
+            accelY = new SimDouble(SimDeviceDataJNI.getSimValueHandle(deviceHandle, "LinearWorldAccelY"));
+            accelZ = new SimDouble(SimDeviceDataJNI.getSimValueHandle(deviceHandle, "LinearWorldAccelZ"));
+        }
+
+        /**
+         * NavX simulation does not update on-demand, so we have to wait until it updates next.
+         * I could grab this value programmatically, but I'm lazy and the current delay seems fine.
+         */
+        private void delay() {
+            try {
+                Thread.sleep(waitDuration);
+            } catch (InterruptedException e) {
+                throw new RuntimeException(e);
+            }
+        }
+
+        public void setYaw(double angle) {
+            yaw.set(angle);
+            delay();
+        }
+
+        public void setPitch(double angle) {
+            pitch.set(angle);
+            delay();
+        }
+
+        public void setRoll(double angle) {
+            roll.set(angle);
+            delay();
+        }
+
+        public void setRate(double rate) {
+            this.rate.set(rate);
+            delay();
+        }
+
+        public void setAccelX(double accel) {
+            accelX.set(accel);
+            delay();
+        }
+
+        public void setAccelY(double accel) {
+            accelY.set(accel);
+            delay();
+        }
+
+        public void setAccelZ(double accel) {
+            accelZ.set(accel);
+            delay();
+        }
+
+        public void reset() {
+            yaw.set(0);
+            pitch.set(0);
+            roll.set(0);
+            accelX.set(0);
+            accelY.set(0);
+            accelZ.set(0);
+            delay();
+        }
+    }
+}
+
+

From e6b51dbd281ed0cfda1fec97d03f62f809cc73d2 Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Mon, 14 Oct 2024 12:34:28 -0400
Subject: [PATCH 26/27] fix(hardware/gyro/navx): use Gs not m/ss

---
 .../librobot/hardware/gyro/NavXIMU.java       | 13 ++++---
 .../librobot/hardware/gyro/NavXTests.java     | 39 ++++++++++---------
 2 files changed, 27 insertions(+), 25 deletions(-)

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java
index 86786a6..c485fd5 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java
@@ -3,6 +3,7 @@
 import com.kauailabs.navx.frc.AHRS;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.units.Units;
 import edu.wpi.first.wpilibj.I2C;
 import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.SerialPort;
@@ -146,18 +147,18 @@ public double getRateDegreesPerSecond(CartesianAxis axis) {
     @Override
     public @NotNull Translation3d getAccelerationMetersPerSecondSquared() {
         return new Translation3d(
-                imu.getWorldLinearAccelX(),
-                imu.getWorldLinearAccelY(),
-                imu.getWorldLinearAccelZ()
+                Units.MetersPerSecondPerSecond.convertFrom(imu.getWorldLinearAccelX(), Units.Gs),
+                Units.MetersPerSecondPerSecond.convertFrom(imu.getWorldLinearAccelY(), Units.Gs),
+                Units.MetersPerSecondPerSecond.convertFrom(imu.getWorldLinearAccelZ(), Units.Gs)
         );
     }
 
     @Override
     public double getAccelerationMetersPerSecondSquared(@NotNull CartesianAxis axis) {
         return switch (axis) {
-            case X -> imu.getWorldLinearAccelX();
-            case Y -> imu.getWorldLinearAccelY();
-            case Z -> imu.getWorldLinearAccelZ();
+            case X -> Units.MetersPerSecondPerSecond.convertFrom(imu.getWorldLinearAccelX(), Units.Gs);
+            case Y -> Units.MetersPerSecondPerSecond.convertFrom(imu.getWorldLinearAccelY(), Units.Gs);
+            case Z -> Units.MetersPerSecondPerSecond.convertFrom(imu.getWorldLinearAccelZ(), Units.Gs);
         };
     }
 
diff --git a/src/test/java/net/frc5183/librobot/hardware/gyro/NavXTests.java b/src/test/java/net/frc5183/librobot/hardware/gyro/NavXTests.java
index 5325fd0..a7fb302 100644
--- a/src/test/java/net/frc5183/librobot/hardware/gyro/NavXTests.java
+++ b/src/test/java/net/frc5183/librobot/hardware/gyro/NavXTests.java
@@ -5,6 +5,7 @@
 import edu.wpi.first.hal.simulation.SimDeviceDataJNI;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.units.Units;
 import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
@@ -412,59 +413,59 @@ void testRateRoll() {
     static final double TEST_ACCEL_2 = 1;
     static final double TEST_ACCEL_3 = 2;
     static final double TEST_ACCEL_4 = 3;
-    static final double TEST_ACCEL_5 = Math.random() * 3;
+    static final double TEST_ACCEL_5 = Math.random() * 6;
 
     @Test
     void testAccelerationCartesianX() {
-        sim.setAccelX(TEST_ACCEL_1);
+        sim.setAccelX(Units.Gs.convertFrom(TEST_ACCEL_1, Units.MetersPerSecondPerSecond));
         assertEquals(TEST_ACCEL_1, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X), DELTA);
 
-        sim.setAccelX(TEST_ACCEL_2);
+        sim.setAccelX(Units.Gs.convertFrom(TEST_ACCEL_2, Units.MetersPerSecondPerSecond));
         assertEquals(TEST_ACCEL_2, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X), DELTA);
 
-        sim.setAccelX(TEST_ACCEL_3);
+        sim.setAccelX(Units.Gs.convertFrom(TEST_ACCEL_3, Units.MetersPerSecondPerSecond));
         assertEquals(TEST_ACCEL_3, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X), DELTA);
 
-        sim.setAccelX(TEST_ACCEL_4);
+        sim.setAccelX(Units.Gs.convertFrom(TEST_ACCEL_4, Units.MetersPerSecondPerSecond));
         assertEquals(TEST_ACCEL_4, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X), DELTA);
 
-        sim.setAccelX(TEST_ACCEL_5);
+        sim.setAccelX(Units.Gs.convertFrom(TEST_ACCEL_5, Units.MetersPerSecondPerSecond));
         assertEquals(TEST_ACCEL_5, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X), DELTA);
     }
 
     @Test
     void testAccelerationCartesianY() {
-        sim.setAccelY(TEST_ACCEL_1);
+        sim.setAccelY(Units.Gs.convertFrom(TEST_ACCEL_1, Units.MetersPerSecondPerSecond));
         assertEquals(TEST_ACCEL_1, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y), DELTA);
 
-        sim.setAccelY(TEST_ACCEL_2);
+        sim.setAccelY(Units.Gs.convertFrom(TEST_ACCEL_2, Units.MetersPerSecondPerSecond));
         assertEquals(TEST_ACCEL_2, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y), DELTA);
 
-        sim.setAccelY(TEST_ACCEL_3);
+        sim.setAccelY(Units.Gs.convertFrom(TEST_ACCEL_3, Units.MetersPerSecondPerSecond));
         assertEquals(TEST_ACCEL_3, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y), DELTA);
 
-        sim.setAccelY(TEST_ACCEL_4);
+        sim.setAccelY(Units.Gs.convertFrom(TEST_ACCEL_4, Units.MetersPerSecondPerSecond));
         assertEquals(TEST_ACCEL_4, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y), DELTA);
 
-        sim.setAccelY(TEST_ACCEL_5);
+        sim.setAccelY(Units.Gs.convertFrom(TEST_ACCEL_5, Units.MetersPerSecondPerSecond));
         assertEquals(TEST_ACCEL_5, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y), DELTA);
     }
 
     @Test
     void testAccelerationCartesianZ() {
-        sim.setAccelZ(TEST_ACCEL_1);
+        sim.setAccelZ(Units.Gs.convertFrom(TEST_ACCEL_1, Units.MetersPerSecondPerSecond));
         assertEquals(TEST_ACCEL_1, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z), DELTA);
 
-        sim.setAccelZ(TEST_ACCEL_2);
+        sim.setAccelZ(Units.Gs.convertFrom(TEST_ACCEL_2, Units.MetersPerSecondPerSecond));
         assertEquals(TEST_ACCEL_2, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z), DELTA);
 
-        sim.setAccelZ(TEST_ACCEL_3);
+        sim.setAccelZ(Units.Gs.convertFrom(TEST_ACCEL_3, Units.MetersPerSecondPerSecond));
         assertEquals(TEST_ACCEL_3, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z), DELTA);
 
-        sim.setAccelZ(TEST_ACCEL_4);
+        sim.setAccelZ(Units.Gs.convertFrom(TEST_ACCEL_4, Units.MetersPerSecondPerSecond));
         assertEquals(TEST_ACCEL_4, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z), DELTA);
 
-        sim.setAccelZ(TEST_ACCEL_5);
+        sim.setAccelZ(Units.Gs.convertFrom(TEST_ACCEL_5, Units.MetersPerSecondPerSecond));
         assertEquals(TEST_ACCEL_5, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z), DELTA);
     }
 
@@ -472,9 +473,9 @@ void testAccelerationCartesianZ() {
     void testAccelerationTranslation3d() {
         // Meters per second squared.
         Consumer<Double> setAccel = (Double accel) -> {
-            sim.setAccelX(accel);
-            sim.setAccelY(accel);
-            sim.setAccelZ(accel);
+            sim.setAccelX(Units.Gs.convertFrom(accel, Units.MetersPerSecondPerSecond));
+            sim.setAccelY(Units.Gs.convertFrom(accel, Units.MetersPerSecondPerSecond));
+            sim.setAccelZ(Units.Gs.convertFrom(accel, Units.MetersPerSecondPerSecond));
         };
 
         // Meters per second squared.

From 2de1188916a8b669cd18043036f9ac671ee18dd5 Mon Sep 17 00:00:00 2001
From: Baconing <iam@baconi.ng>
Date: Mon, 14 Oct 2024 12:38:14 -0400
Subject: [PATCH 27/27] docs(hardware/gyro): annotate deprecated members

---
 src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java
index ae406ae..a13f0dc 100644
--- a/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java
+++ b/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java
@@ -94,6 +94,7 @@ public Rotation3d getRotation3dRadians() {
      * @deprecated this only exists for YAGSL, use {@link #getRotation3dRadians()} instead (since that's what this method calls internally).
      * @see #getRotation3dRadians()
      */
+    @Deprecated
     public Rotation3d getRawRotation3d() {
         return getRawRotation3dRadians();
     }
@@ -110,6 +111,7 @@ public Rotation3d getRawRotation3d() {
      * @deprecated this only exists for YAGSL, use {@link #getRotation3dRadians()} instead (since that's what this method calls internally).
      * @see #getRotation3dRadians()
      */
+    @Deprecated
     public Rotation3d getRotation3d() {
         return getRotation3dRadians();
     }
@@ -134,6 +136,7 @@ public Rotation3d getRotation3d() {
      * @deprecated this only exists for YAGSL, use {@link #getRateDegreesPerSecond(Attitude)} instead (since that's what this method calls internally).
      * @see #getRateDegreesPerSecond(Attitude)
      */
+    @Deprecated
     public double getRate() {
         return getRateDegreesPerSecond(Attitude.YAW);
     }
@@ -150,6 +153,7 @@ public double getRate() {
      * @see #getAccelerationMetersPerSecondSquared()
      * @deprecated this only exists for use by YAGSL, use {@link #getAccelerationMetersPerSecondSquared()} instead (since that's what this method calls internally).
      */
+    @Deprecated
     public Optional<Translation3d> getAccel() {
         return Optional.of(getAccelerationMetersPerSecondSquared());
     }