feat(hardware/gyro): implement gyroscopes

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;
+        };
+    }
+}

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;
+        };
+    }
+}

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
+    }
+}

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
+    }
+}