[wpilibj] Add tests for ADIS16448 simulation (#6152)

2026-06-25 01:41:43 +00:00 · 2024-05-24 17:33:25 -06:00
parent c62396ce4e
commit 221d568bd9
1 changed files with 148 additions and 0 deletions
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADIS16448SimTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADIS16448SimTest.java
@@ -0,0 +1,148 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.fail;
+import static org.junit.jupiter.params.provider.Arguments.arguments;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.ADIS16448_IMU;
+import edu.wpi.first.wpilibj.SPI;
+import java.util.stream.Stream;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+class ADIS16448SimTest {
+  @ParameterizedTest
+  @MethodSource("provideEnumCombinations")
+  void testYawAxis(ADIS16448_IMU.IMUAxis yawAxis, ADIS16448_IMU.CalibrationTime calibrationTime) {
+    HAL.initialize(500, 0);
+
+    try (ADIS16448_IMU imu = new ADIS16448_IMU(yawAxis, SPI.Port.kMXP, calibrationTime)) {
+      assertEquals(yawAxis, imu.getYawAxis());
+    }
+  }
+
+  @ParameterizedTest
+  @MethodSource("provideEnumCombinations")
+  void testGyroAngles(
+      ADIS16448_IMU.IMUAxis yawAxis, ADIS16448_IMU.CalibrationTime calibrationTime) {
+    HAL.initialize(500, 0);
+
+    try (ADIS16448_IMU imu = new ADIS16448_IMU(yawAxis, SPI.Port.kMXP, calibrationTime)) {
+      ADIS16448_IMUSim sim = new ADIS16448_IMUSim(imu);
+      sim.setGyroAngleX(123.45);
+      sim.setGyroAngleY(-67.89);
+      sim.setGyroAngleZ(4.13);
+
+      assertEquals(123.45, imu.getGyroAngleX());
+      assertEquals(-67.89, imu.getGyroAngleY());
+      assertEquals(4.13, imu.getGyroAngleZ());
+    }
+  }
+
+  @ParameterizedTest
+  @MethodSource("provideEnumCombinations")
+  void testGyroRates(ADIS16448_IMU.IMUAxis yawAxis, ADIS16448_IMU.CalibrationTime calibrationTime) {
+    HAL.initialize(500, 0);
+
+    try (ADIS16448_IMU imu = new ADIS16448_IMU(yawAxis, SPI.Port.kMXP, calibrationTime)) {
+      ADIS16448_IMUSim sim = new ADIS16448_IMUSim(imu);
+      sim.setGyroRateX(15.92);
+      sim.setGyroRateY(-65.35);
+      sim.setGyroRateZ(2.71);
+
+      assertEquals(15.92, imu.getGyroRateX());
+      assertEquals(-65.35, imu.getGyroRateY());
+      assertEquals(2.71, imu.getGyroRateZ());
+    }
+  }
+
+  @ParameterizedTest
+  @MethodSource("provideEnumCombinations")
+  void testAccelValues(
+      ADIS16448_IMU.IMUAxis yawAxis, ADIS16448_IMU.CalibrationTime calibrationTime) {
+    HAL.initialize(500, 0);
+
+    try (ADIS16448_IMU imu = new ADIS16448_IMU(yawAxis, SPI.Port.kMXP, calibrationTime)) {
+      ADIS16448_IMUSim sim = new ADIS16448_IMUSim(imu);
+      sim.setAccelX(6.85);
+      sim.setAccelY(-1.82);
+      sim.setAccelZ(8.69);
+
+      assertEquals(6.85, imu.getAccelX());
+      assertEquals(-1.82, imu.getAccelY());
+      assertEquals(8.69, imu.getAccelZ());
+    }
+  }
+
+  @ParameterizedTest
+  @MethodSource("provideEnumCombinations")
+  void testAngleBasedOnYawAxis(
+      ADIS16448_IMU.IMUAxis yawAxis, ADIS16448_IMU.CalibrationTime calibrationTime) {
+    HAL.initialize(500, 0);
+
+    try (ADIS16448_IMU imu = new ADIS16448_IMU(yawAxis, SPI.Port.kMXP, calibrationTime)) {
+      ADIS16448_IMUSim sim = new ADIS16448_IMUSim(imu);
+      sim.setGyroAngleX(123.45);
+      sim.setGyroAngleY(-67.89);
+      sim.setGyroAngleZ(4.13);
+
+      switch (yawAxis) {
+        case kX:
+          assertEquals(imu.getAngle(), imu.getGyroAngleX());
+          break;
+        case kY:
+          assertEquals(imu.getAngle(), imu.getGyroAngleY());
+          break;
+        case kZ:
+          assertEquals(imu.getAngle(), imu.getGyroAngleZ());
+          break;
+        default:
+          fail("invalid YawAxis!");
+          break;
+      }
+    }
+  }
+
+  @ParameterizedTest
+  @MethodSource("provideEnumCombinations")
+  void testRateBasedOnYawAxis(
+      ADIS16448_IMU.IMUAxis yawAxis, ADIS16448_IMU.CalibrationTime calibrationTime) {
+    HAL.initialize(500, 0);
+
+    try (ADIS16448_IMU imu = new ADIS16448_IMU(yawAxis, SPI.Port.kMXP, calibrationTime)) {
+      ADIS16448_IMUSim sim = new ADIS16448_IMUSim(imu);
+      sim.setGyroRateX(1.92);
+      sim.setGyroRateY(-6.35);
+      sim.setGyroRateZ(20.71);
+
+      switch (yawAxis) {
+        case kX:
+          assertEquals(imu.getRate(), imu.getGyroRateX());
+          break;
+        case kY:
+          assertEquals(imu.getRate(), imu.getGyroRateY());
+          break;
+        case kZ:
+          assertEquals(imu.getRate(), imu.getGyroRateZ());
+          break;
+        default:
+          fail("invalid YawAxis!");
+          break;
+      }
+    }
+  }
+
+  static Stream<Arguments> provideEnumCombinations() {
+    return Stream.of(ADIS16448_IMU.IMUAxis.values())
+        .flatMap(
+            imuAxis ->
+                Stream.of(ADIS16448_IMU.CalibrationTime.values())
+                    .map(calibrationTime -> arguments(imuAxis, calibrationTime)));
+  }
+}