diff --git a/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java new file mode 100644 index 0000000..c9e47c2 --- /dev/null +++ b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16448IMU.java @@ -0,0 +1,222 @@ +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 org.jetbrains.annotations.NotNull; + +/** + * Represents an ADIS16448 {@link IMU}. + */ +public class ADIS16448IMU extends IMU { + /** + * The ADIS16448 IMU. + */ + @NotNull + private final ADIS16448_IMU 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 ADIS16448IMU} using the RIO's Onboard MXP port, 500ms calibration time, and Z axis as yaw. + * @see ADIS16448_IMU#ADIS16448_IMU() + */ + public ADIS16448IMU() { + this(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) { + yaw = toCartesian(imu.getYawAxis()); + CartesianAxis[] axes = CartesianAxis.assignAxes(yaw); + pitch = axes[0]; + roll = axes[1]; + + this.imu = imu; + factoryDefault(); + } + + /** + * 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(@NotNull CartesianAxis yaw) { + this(yaw, SPI.Port.kMXP); + } + + /** + * 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(@NotNull CartesianAxis yaw, @NotNull SPI.Port port) { + this(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(@NotNull CartesianAxis yaw, @NotNull SPI.Port port, @NotNull ADIS16448_IMU.CalibrationTime calibrationTime) { + this(new ADIS16448_IMU(fromCartesian(yaw), port, calibrationTime)); + } + + @Override + public double getRawAngleRadians(@NotNull Attitude axis) { + CartesianAxis cartesianAxis = switch (axis) { + case YAW -> yaw; + case PITCH -> pitch; + case ROLL -> roll; + }; + + return getRawAngleRadians(cartesianAxis); + } + + @Override + public double getRawAngleRadians(CartesianAxis axis) { + return switch (axis) { + case X -> Math.toRadians(imu.getGyroAngleX()); + case Y -> Math.toRadians(imu.getGyroAngleY()); + case Z -> Math.toRadians(imu.getGyroAngleZ()); + }; + } + + @Override + public @NotNull Rotation3d getRawRotation3dRadians() { + return new Rotation3d( + getRawAngleRadians(this.roll), + getRawAngleRadians(this.pitch), + getRawAngleRadians(this.yaw) + ); + } + + @Override + 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 + public @NotNull Translation3d getAccelerationMetersPerSecondSquared() { + return new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ()); + } + + @Override + public double getAccelerationMetersPerSecondSquared(CartesianAxis axis) { + return switch (axis) { + case X -> imu.getAccelX(); + case Y -> imu.getAccelY(); + case Z -> imu.getAccelZ(); + }; + } + + @Override + public void calibrate() { + imu.calibrate(); + } + + @Override + public void reset() { + imu.reset(); + } + + @Override + public void factoryDefault() { + imu.calibrate(); + super.setOffset(null); + super.setOffsetX(0); + super.setOffsetY(0); + super.setOffsetZ(0); + } + + @Override + public void clearStickyFaults() { + // The ADIS16448 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 CartesianAxis getRollAxis() { + return roll; + } + + @Override + public @NotNull ADIS16448_IMU getIMU() { + return imu; + } + + /** + * 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 fromCartesian(@NotNull CartesianAxis axis) { + return switch (axis) { + case X -> ADIS16448_IMU.IMUAxis.kX; + case Y -> ADIS16448_IMU.IMUAxis.kY; + case Z -> ADIS16448_IMU.IMUAxis.kZ; + }; + } + + /** + * 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 toCartesian(@NotNull ADIS16448_IMU.IMUAxis axis) { + return switch (axis) { + 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/ADIS16470IMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java new file mode 100644 index 0000000..731673c --- /dev/null +++ b/src/main/java/net/frc5183/librobot/hardware/gyro/ADIS16470IMU.java @@ -0,0 +1,254 @@ +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 org.jetbrains.annotations.NotNull; + +/** + * Represents an ADIS16470 {@link IMU}. + */ +public class ADIS16470IMU extends IMU { + /** + * The ADIS16470 IMU. + */ + @NotNull + private final ADIS16470_IMU imu; + + /** + * The YAW axis. + */ + private final CartesianAxis yaw; + + /** + * The PITCH axis. + */ + private final CartesianAxis pitch; + + /** + * The ROLL axis. + */ + private final CartesianAxis roll; + + /** + * 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() { + this(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; + + yaw = toCartesian(imu.getYawAxis()); + pitch = toCartesian(imu.getPitchAxis()); + roll = toCartesian(imu.getRollAxis()); + } + + /** + * Creates a new {@link ADIS16470IMU} with a specified yaw, pitch, and roll axis. + * 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( + @NotNull CartesianAxis yaw, + @NotNull CartesianAxis pitch, + @NotNull CartesianAxis roll) { + this(yaw, pitch, roll, SPI.Port.kMXP); + } + + /** + * 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( + @NotNull CartesianAxis yaw, + @NotNull CartesianAxis pitch, + @NotNull CartesianAxis roll, + @NotNull SPI.Port port + ) { + this(yaw, pitch, 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( + @NotNull CartesianAxis yaw, + @NotNull CartesianAxis pitch, + @NotNull CartesianAxis roll, + @NotNull SPI.Port port, + @NotNull ADIS16470_IMU.CalibrationTime calibrationTime + ) { + imu = new ADIS16470_IMU( + fromCartesian(yaw), + fromCartesian(pitch), + fromCartesian(roll), + port, + calibrationTime + ); + + this.yaw = yaw; + this.pitch = pitch; + this.roll = roll; + } + + @Override + public double getRawAngleRadians(@NotNull Attitude axis) { + return Math.toRadians(imu.getAngle(fromAttitude(axis))); + } + + @Override + public double getRawAngleRadians(@NotNull CartesianAxis axis) { + return Math.toRadians(imu.getAngle(fromCartesian(axis))); + } + + @Override + public @NotNull Rotation3d getRawRotation3dRadians() { + return new Rotation3d( + getRawAngleRadians(this.roll), + getRawAngleRadians(this.pitch), + getRawAngleRadians(this.yaw) + ); + } + + @Override + public double getRateDegreesPerSecond(Attitude axis) { + return imu.getRate(fromAttitude(axis)); + } + + @Override + public double getRateDegreesPerSecond(CartesianAxis axis) { + return imu.getRate(fromCartesian(axis)); + } + + @Override + public @NotNull Translation3d getAccelerationMetersPerSecondSquared() { + return new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ()); + } + + @Override + public double getAccelerationMetersPerSecondSquared(CartesianAxis axis) { + return switch (axis) { + case X -> imu.getAccelX(); + case Y -> imu.getAccelY(); + case Z -> imu.getAccelZ(); + }; + } + + @Override + public void calibrate() { + imu.calibrate(); + } + + @Override + public void reset() { + imu.reset(); + } + + @Override + 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 CartesianAxis getYawAxis() { + return yaw; + } + + @Override + public CartesianAxis getPitchAxis() { + return pitch; + } + + @Override + public CartesianAxis getRollAxis() { + return roll; + } + + @Override + public @NotNull ADIS16470_IMU getIMU() { + return imu; + } + + /** + * 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 fromCartesian(@NotNull CartesianAxis axis) { + return switch (axis) { + case X -> ADIS16470_IMU.IMUAxis.kX; + case Y -> ADIS16470_IMU.IMUAxis.kY; + case Z -> ADIS16470_IMU.IMUAxis.kZ; + }; + } + + /** + * 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 fromAttitude(@NotNull Attitude 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 CartesianAxis} from an {@link ADIS16470_IMU.IMUAxis}. + * @param axis the {@link ADIS16470_IMU.IMUAxis} to convert. + * @return the converted {@link CartesianAxis}. + * @throws IllegalArgumentException if {@code axis} is not one of kX, kY, or kZ. + */ + public static CartesianAxis toCartesian(@NotNull ADIS16470_IMU.IMUAxis axis) { + return switch (axis) { + 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 Attitude} from an {@link ADIS16470_IMU.IMUAxis}. + * @param axis the {@link ADIS16470_IMU.IMUAxis} to convert. + * @return the converted {@link Attitude}. + * @throws IllegalArgumentException if {@code axis} is not one of kYaw, kPitch, or kRoll. + */ + public static Attitude toAttitude(@NotNull ADIS16470_IMU.IMUAxis axis) { + return switch (axis) { + 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.java b/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java new file mode 100644 index 0000000..a13f0dc --- /dev/null +++ b/src/main/java/net/frc5183/librobot/hardware/gyro/IMU.java @@ -0,0 +1,350 @@ +package net.frc5183.librobot.hardware.gyro; + +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; + +/** + * Represents an IMU (Inertial Measurement Unit). + */ +public abstract class IMU extends SwerveIMU { + /** + * Whether the IMU's values should be inverted. + */ + private boolean inverted = false; + + /** + * The {@link Rotation3d} offset of the gyroscope. + * @deprecated use {@link #offsetX}, {@link #offsetY}, and {@link #offsetZ} instead, this only exists for YAGSL.
Changes are not reflected on {@link #offsetX}, {@link #offsetY}, and {@link #offsetZ}. + */ + @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. + * @param axis the {@link CartesianAxis} to get the angle's rotation of. + * @return the angle's rotation (with offset) in radians. + */ + public double getAngleRadians(CartesianAxis axis) { + return switch (axis) { + case Z -> getRawAngleRadians(axis) - offsetZ; + case Y -> getRawAngleRadians(axis) - offsetY; + case X -> getRawAngleRadians(axis) - offsetX; + }; + } + + /** + * Gets the specified angle's rotation (with offset) in radians. + * @param axis the {@link Attitude} to get the angle's rotation of. + * @return the angle's rotation (with offset) in radians. + */ + public double getAngleRadians(Attitude axis) { + return switch (axis) { + case YAW -> getAngleRadians(getYawAxis()); + case PITCH -> getAngleRadians(getPitchAxis()); + case ROLL -> getAngleRadians(getRollAxis()); + }; + } + + /** + * Gets the specified angle's rotation (without offset) in radians. + * @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(Attitude axis); + + /** + * Gets the specified angle's rotation (without offset) in radians. + * @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(CartesianAxis 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 !inverted ? getRawRotation3dRadians().minus(getOffset()) : getRawRotation3dRadians().minus(getOffset()).unaryMinus(); + } + + /** + * 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() + */ + @Deprecated + 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() + */ + @Deprecated + public Rotation3d getRotation3d() { + return getRotation3dRadians(); + } + + /** + * 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 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) + */ + @Deprecated + public double getRate() { + return getRateDegreesPerSecond(Attitude.YAW); + } + + /** + * 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 } 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). + */ + @Deprecated + public Optional getAccel() { + return Optional.of(getAccelerationMetersPerSecondSquared()); + } + + /** + * Gets the acceleration from the IMU in meters per second squared on a specific axis. + * @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(CartesianAxis axis); + + /** + * Returns the offset as a {@link Rotation3d}. + *

+ * 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. + */ + @Deprecated + public Rotation3d getOffset() { + if (offset != null) return offset; + + Map 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.
Changes are not reflected on {@link #offsetX}, {@link #offsetY}, and {@link #offsetZ}. + */ + @Deprecated + public void setOffset(@Nullable Rotation3d offset) { + this.offset = offset; + } + + /** + * Returns the offset to be subtracted from the gyroscope's rotation. + * @return the offset to be subtracted from the gyroscope's rotation. + */ + 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; + } + + /** + * 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. + */ + public abstract void calibrate(); + + /** + * Resets the gyroscope. + */ + 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 CartesianAxis} of the yaw. + * @return the {@link CartesianAxis} of the yaw. + */ + public abstract CartesianAxis getYawAxis(); + + /** + * Returns the {@link CartesianAxis} of the pitch. + * @return the {@link CartesianAxis} of the pitch. + */ + public abstract CartesianAxis getPitchAxis(); + + /** + * Returns the {@link CartesianAxis} of the roll. + * @return the {@link CartesianAxis} of the roll. + */ + public abstract CartesianAxis getRollAxis(); + + /** + * Returns the raw gyroscope object. + * Should only be used when you know the subclass of the gyroscope. + *

+ * 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. + */ + public abstract Object getIMU(); + + /** + * Represents the cartesian axes of a gyroscope (xyz). + */ + public enum CartesianAxis { + 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 CartesianAxis[] assignAxes(CartesianAxis yaw) { + CartesianAxis pitch; + CartesianAxis roll; + + if (yaw == X) { + pitch = Y; + roll = Z; + } else if (yaw == Y) { + pitch = X; + roll = Z; + } else { + pitch = X; + roll = Y; + } + + 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/NavXIMU.java b/src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java new file mode 100644 index 0000000..c485fd5 --- /dev/null +++ b/src/main/java/net/frc5183/librobot/hardware/gyro/NavXIMU.java @@ -0,0 +1,247 @@ +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.units.Units; +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 -> Math.toRadians(convertNavXDegrees(imu.getYaw())); + case PITCH -> Math.toRadians(convertNavXDegrees(imu.getPitch())); + case ROLL -> Math.toRadians(convertNavXDegrees(imu.getRoll())); + }; + } + + @Override + public double getRawAngleRadians(@NotNull CartesianAxis axis) { + 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( + getRawAngleRadians(Attitude.ROLL), + getRawAngleRadians(Attitude.PITCH), + getRawAngleRadians(Attitude.YAW) + ); + } + + @Override + 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 + public @NotNull Translation3d getAccelerationMetersPerSecondSquared() { + return new Translation3d( + 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 -> 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); + }; + } + + @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; + } + + /** + * 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}. + * @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; + }; + } +} + 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/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..5766e7c --- /dev/null +++ b/src/test/java/net/frc5183/librobot/hardware/gyro/ADIS16448Tests.java @@ -0,0 +1,639 @@ +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(); + } + + // + @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."); + } + // + + // + 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 setAngle = (Double angle) -> { + sim.setGyroAngleX(angle); + sim.setGyroAngleY(angle); + sim.setGyroAngleZ(angle); + }; + + // Radians + Consumer 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 setOffset = (Double offset) -> { + imu.setOffsetX((double) offset); + imu.setOffsetY((double) offset); + imu.setOffsetZ((double) offset); + }; + + // Degrees. + Consumer setAngle = (Double angle) -> { + sim.setGyroAngleX(angle); + sim.setGyroAngleY(angle); + sim.setGyroAngleZ(angle); + }; + + // Radians. + BiConsumer 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)); + // + 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)); + // + + setOffset.accept(Math.toRadians(TEST_ANGLE_2)); + // + 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)); + // + + setOffset.accept(Math.toRadians(TEST_ANGLE_3)); + // + 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)); + // + + setOffset.accept(Math.toRadians(TEST_ANGLE_4)); + // + 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)); + // + + setOffset.accept(Math.toRadians(TEST_ANGLE_5)); + // + 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)); + // + } + // + + // + @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); + } + // + + // + 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 setAccel = (Double accel) -> { + sim.setAccelX(accel); + sim.setAccelY(accel); + sim.setAccelZ(accel); + }; + + // Meters per second squared. + BiConsumer 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); + } + // + + // + @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..0640241 --- /dev/null +++ b/src/test/java/net/frc5183/librobot/hardware/gyro/ADIS16470Tests.java @@ -0,0 +1,625 @@ +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(); + } + + // + @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."); + } + // + + // + 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 setAngle = (Double angle) -> { + sim.setGyroAngleX(angle); + sim.setGyroAngleY(angle); + sim.setGyroAngleZ(angle); + }; + + // Radians + Consumer 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 setOffset = (Double offset) -> { + imu.setOffsetX((double) offset); + imu.setOffsetY((double) offset); + imu.setOffsetZ((double) offset); + }; + + // Degrees. + Consumer setAngle = (Double angle) -> { + sim.setGyroAngleX(angle); + sim.setGyroAngleY(angle); + sim.setGyroAngleZ(angle); + }; + + // Radians. + BiConsumer 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)); + // + 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)); + // + + setOffset.accept(Math.toRadians(TEST_ANGLE_2)); + // + 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)); + // + + setOffset.accept(Math.toRadians(TEST_ANGLE_3)); + // + 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)); + // + + setOffset.accept(Math.toRadians(TEST_ANGLE_4)); + // + 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)); + // + + setOffset.accept(Math.toRadians(TEST_ANGLE_5)); + // + 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)); + // + } + // + + // + @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); + } + // + + // TODO: Acceleration tests are currently disabled due to a bug in wpilibj. + // https://github.com/wpilibsuite/allwpilib/issues/7180 + // + /* + 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 setAccel = (Double accel) -> { + sim.setAccelX(accel); + sim.setAccelY(accel); + sim.setAccelZ(accel); + }; + + // Meters per second squared. + BiConsumer 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); + } + */ + // + + // + @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."); + } +} 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..a7fb302 --- /dev/null +++ b/src/test/java/net/frc5183/librobot/hardware/gyro/NavXTests.java @@ -0,0 +1,640 @@ +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.units.Units; +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); + } + + // + 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 setAngle = (Double angle) -> { + sim.setYaw(angle); + sim.setPitch(angle); + sim.setRoll(angle); + }; + + // Radians + Consumer 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 setOffset = (Double offset) -> { + imu.setOffsetX((double) offset); + imu.setOffsetY((double) offset); + imu.setOffsetZ((double) offset); + }; + + // Degrees. + Consumer setAngle = (Double angle) -> { + sim.setYaw(angle); + sim.setPitch(angle); + sim.setRoll(angle); + }; + + // Radians. + BiConsumer 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)); + // + 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)); + // + + setOffset.accept(Math.toRadians(TEST_ANGLE_2)); + // + 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)); + // + + setOffset.accept(Math.toRadians(TEST_ANGLE_3)); + // + 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)); + // + + setOffset.accept(Math.toRadians(TEST_ANGLE_4)); + // + 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)); + // + + setOffset.accept(Math.toRadians(TEST_ANGLE_5)); + // + 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)); + // + } + + // + + // + @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) + ); + } + // + + // + 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() * 6; + + @Test + void testAccelerationCartesianX() { + sim.setAccelX(Units.Gs.convertFrom(TEST_ACCEL_1, Units.MetersPerSecondPerSecond)); + assertEquals(TEST_ACCEL_1, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X), DELTA); + + sim.setAccelX(Units.Gs.convertFrom(TEST_ACCEL_2, Units.MetersPerSecondPerSecond)); + assertEquals(TEST_ACCEL_2, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X), DELTA); + + sim.setAccelX(Units.Gs.convertFrom(TEST_ACCEL_3, Units.MetersPerSecondPerSecond)); + assertEquals(TEST_ACCEL_3, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X), DELTA); + + sim.setAccelX(Units.Gs.convertFrom(TEST_ACCEL_4, Units.MetersPerSecondPerSecond)); + assertEquals(TEST_ACCEL_4, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.X), DELTA); + + 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(Units.Gs.convertFrom(TEST_ACCEL_1, Units.MetersPerSecondPerSecond)); + assertEquals(TEST_ACCEL_1, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y), DELTA); + + sim.setAccelY(Units.Gs.convertFrom(TEST_ACCEL_2, Units.MetersPerSecondPerSecond)); + assertEquals(TEST_ACCEL_2, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y), DELTA); + + sim.setAccelY(Units.Gs.convertFrom(TEST_ACCEL_3, Units.MetersPerSecondPerSecond)); + assertEquals(TEST_ACCEL_3, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y), DELTA); + + sim.setAccelY(Units.Gs.convertFrom(TEST_ACCEL_4, Units.MetersPerSecondPerSecond)); + assertEquals(TEST_ACCEL_4, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Y), DELTA); + + 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(Units.Gs.convertFrom(TEST_ACCEL_1, Units.MetersPerSecondPerSecond)); + assertEquals(TEST_ACCEL_1, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z), DELTA); + + sim.setAccelZ(Units.Gs.convertFrom(TEST_ACCEL_2, Units.MetersPerSecondPerSecond)); + assertEquals(TEST_ACCEL_2, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z), DELTA); + + sim.setAccelZ(Units.Gs.convertFrom(TEST_ACCEL_3, Units.MetersPerSecondPerSecond)); + assertEquals(TEST_ACCEL_3, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z), DELTA); + + sim.setAccelZ(Units.Gs.convertFrom(TEST_ACCEL_4, Units.MetersPerSecondPerSecond)); + assertEquals(TEST_ACCEL_4, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z), DELTA); + + sim.setAccelZ(Units.Gs.convertFrom(TEST_ACCEL_5, Units.MetersPerSecondPerSecond)); + assertEquals(TEST_ACCEL_5, imu.getAccelerationMetersPerSecondSquared(IMU.CartesianAxis.Z), DELTA); + } + + @Test + void testAccelerationTranslation3d() { + // Meters per second squared. + Consumer setAccel = (Double 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. + BiConsumer 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); + } + // + + // + @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)); + } + // + + /** + * 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(); + } + } +} + +