[wpilib] Add simulation support to ADIS classes (#3857)

wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java

@@ -15,6 +15,8 @@ package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
 import edu.wpi.first.networktables.NTSendable;
 import edu.wpi.first.networktables.NTSendableBuilder;
 import edu.wpi.first.wpilibj.interfaces.Gyro;
@@ -162,6 +164,17 @@ public class ADIS16448_IMU implements Gyro, NTSendable {
   private DigitalOutput m_status_led;
   private Thread m_acquire_task;
 
+  private SimDevice m_simDevice;
+  private SimDouble m_simGyroAngleX;
+  private SimDouble m_simGyroAngleY;
+  private SimDouble m_simGyroAngleZ;
+  private SimDouble m_simGyroRateX;
+  private SimDouble m_simGyroRateY;
+  private SimDouble m_simGyroRateZ;
+  private SimDouble m_simAccelX;
+  private SimDouble m_simAccelY;
+  private SimDouble m_simAccelZ;
+
   /* CRC-16 Look-Up Table */
   int adiscrc[] =
       new int[] {
@@ -227,52 +240,68 @@ public class ADIS16448_IMU implements Gyro, NTSendable {
 
     m_acquire_task = new Thread(new AcquireTask(this));
 
-    // Force the IMU reset pin to toggle on startup (doesn't require DS enable)
-    m_reset_out = new DigitalOutput(18); // Drive MXP DIO8 low
-    Timer.delay(0.01); // Wait 10ms
-    m_reset_out.close();
-    m_reset_in = new DigitalInput(18); // Set MXP DIO8 high
-    Timer.delay(0.25); // Wait 250ms
-
-    configCalTime(cal_time);
-
-    if (!switchToStandardSPI()) {
-      return;
+    m_simDevice = SimDevice.create("Gyro:ADIS16448", port.value);
+    if (m_simDevice != null) {
+      m_simGyroAngleX = m_simDevice.createDouble("gyro_angle_x", SimDevice.Direction.kInput, 0.0);
+      m_simGyroAngleY = m_simDevice.createDouble("gyro_angle_y", SimDevice.Direction.kInput, 0.0);
+      m_simGyroAngleZ = m_simDevice.createDouble("gyro_angle_z", SimDevice.Direction.kInput, 0.0);
+      m_simGyroRateX = m_simDevice.createDouble("gyro_rate_x", SimDevice.Direction.kInput, 0.0);
+      m_simGyroRateY = m_simDevice.createDouble("gyro_rate_y", SimDevice.Direction.kInput, 0.0);
+      m_simGyroRateZ = m_simDevice.createDouble("gyro_rate_z", SimDevice.Direction.kInput, 0.0);
+      m_simAccelX = m_simDevice.createDouble("accel_x", SimDevice.Direction.kInput, 0.0);
+      m_simAccelY = m_simDevice.createDouble("accel_y", SimDevice.Direction.kInput, 0.0);
+      m_simAccelZ = m_simDevice.createDouble("accel_z", SimDevice.Direction.kInput, 0.0);
     }
 
-    // Set IMU internal decimation to 819.2 SPS
-    writeRegister(SMPL_PRD, 0x0001);
-    // Enable Data Ready (LOW = Good Data) on DIO1 (PWM0 on MXP)
-    writeRegister(MSC_CTRL, 0x0016);
-    // Disable IMU internal Bartlett filter
-    writeRegister(SENS_AVG, 0x0400);
-    // Clear offset registers
-    writeRegister(XGYRO_OFF, 0x0000);
-    writeRegister(YGYRO_OFF, 0x0000);
-    writeRegister(ZGYRO_OFF, 0x0000);
+    if (m_simDevice == null) {
+      // Force the IMU reset pin to toggle on startup (doesn't require DS enable)
+      m_reset_out = new DigitalOutput(18); // Drive MXP DIO8 low
+      Timer.delay(0.01); // Wait 10ms
+      m_reset_out.close();
+      m_reset_in = new DigitalInput(18); // Set MXP DIO8 high
+      Timer.delay(0.25); // Wait 250ms
 
-    // Configure standard SPI
-    if (!switchToAutoSPI()) {
-      return;
+      configCalTime(cal_time);
+
+      if (!switchToStandardSPI()) {
+        return;
+      }
+
+      // Set IMU internal decimation to 819.2 SPS
+      writeRegister(SMPL_PRD, 0x0001);
+      // Enable Data Ready (LOW = Good Data) on DIO1 (PWM0 on MXP)
+      writeRegister(MSC_CTRL, 0x0016);
+      // Disable IMU internal Bartlett filter
+      writeRegister(SENS_AVG, 0x0400);
+      // Clear offset registers
+      writeRegister(XGYRO_OFF, 0x0000);
+      writeRegister(YGYRO_OFF, 0x0000);
+      writeRegister(ZGYRO_OFF, 0x0000);
+
+      // Configure standard SPI
+      if (!switchToAutoSPI()) {
+        return;
+      }
+      // Notify DS that IMU calibration delay is active
+      DriverStation.reportWarning(
+          "ADIS16448 IMU Detected. Starting initial calibration delay.", false);
+      // Wait for whatever time the user set as the start-up delay
+      try {
+        Thread.sleep((long) (m_calibration_time * 1.2 * 1000));
+      } catch (InterruptedException e) {
+      }
+      // Execute calibration routine
+      calibrate();
+      // Reset accumulated offsets
+      reset();
+      // Tell the acquire loop that we're done starting up
+      m_start_up_mode = false;
+      // Let the user know the IMU was initiallized successfully
+      DriverStation.reportWarning("ADIS16448 IMU Successfully Initialized!", false);
+      // Drive MXP PWM5 (IMU ready LED) low (active low)
+      m_status_led = new DigitalOutput(19);
     }
-    // Notify DS that IMU calibration delay is active
-    DriverStation.reportWarning(
-        "ADIS16448 IMU Detected. Starting initial calibration delay.", false);
-    // Wait for whatever time the user set as the start-up delay
-    try {
-      Thread.sleep((long) (m_calibration_time * 1.2 * 1000));
-    } catch (InterruptedException e) {
-    }
-    // Execute calibration routine
-    calibrate();
-    // Reset accumulated offsets
-    reset();
-    // Tell the acquire loop that we're done starting up
-    m_start_up_mode = false;
-    // Let the user know the IMU was initiallized successfully
-    DriverStation.reportWarning("ADIS16448 IMU Successfully Initialized!", false);
-    // Drive MXP PWM5 (IMU ready LED) low (active low)
-    m_status_led = new DigitalOutput(19);
+
     // Report usage and post data to DS
     HAL.report(tResourceType.kResourceType_ADIS16448, 0);
   }
@@ -908,11 +937,11 @@ public class ADIS16448_IMU implements Gyro, NTSendable {
   public synchronized double getRate() {
     switch (m_yaw_axis) {
       case kX:
-        return getGyroInstantX();
+        return getGyroRateX();
       case kY:
-        return getGyroInstantY();
+        return getGyroRateY();
       case kZ:
-        return getGyroInstantZ();
+        return getGyroRateZ();
       default:
         return 0.0;
     }
@@ -925,46 +954,73 @@ public class ADIS16448_IMU implements Gyro, NTSendable {
 
   /** @return accumulated gyro angle in the X axis */
   public synchronized double getGyroAngleX() {
+    if (m_simGyroAngleX != null) {
+      return m_simGyroAngleX.get();
+    }
     return m_integ_gyro_x;
   }
 
   /** @return accumulated gyro angle in the Y axis */
   public synchronized double getGyroAngleY() {
+    if (m_simGyroAngleY != null) {
+      return m_simGyroAngleY.get();
+    }
     return m_integ_gyro_y;
   }
 
   /** @return accumulated gyro angle in the Z axis */
   public synchronized double getGyroAngleZ() {
+    if (m_simGyroAngleZ != null) {
+      return m_simGyroAngleZ.get();
+    }
     return m_integ_gyro_z;
   }
 
   /** @return current gyro angle in the X axis */
-  public synchronized double getGyroInstantX() {
+  public synchronized double getGyroRateX() {
+    if (m_simGyroRateX != null) {
+      return m_simGyroRateX.get();
+    }
     return m_gyro_x;
   }
 
   /** @return current gyro angle in the Y axis */
-  public synchronized double getGyroInstantY() {
+  public synchronized double getGyroRateY() {
+    if (m_simGyroRateY != null) {
+      return m_simGyroRateY.get();
+    }
     return m_gyro_y;
   }
 
   /** @return current gyro angle in the Z axis */
-  public synchronized double getGyroInstantZ() {
+  public synchronized double getGyroRateZ() {
+    if (m_simGyroRateZ != null) {
+      return m_simGyroRateZ.get();
+    }
     return m_gyro_z;
   }
 
   /** @return urrent acceleration in the X axis */
-  public synchronized double getAccelInstantX() {
+  public synchronized double getAccelX() {
+    if (m_simAccelX != null) {
+      return m_simAccelX.get();
+    }
     return m_accel_x;
   }
 
   /** @return current acceleration in the Y axis */
-  public synchronized double getAccelInstantY() {
+  public synchronized double getAccelY() {
+    if (m_simAccelY != null) {
+      return m_simAccelY.get();
+    }
     return m_accel_y;
   }
 
   /** @return current acceleration in the Z axis */
-  public synchronized double getAccelInstantZ() {
+  public synchronized double getAccelZ() {
+    if (m_simAccelZ != null) {
+      return m_simAccelZ.get();
+    }
     return m_accel_z;
   }
 
@@ -1013,6 +1069,15 @@ public class ADIS16448_IMU implements Gyro, NTSendable {
     return m_temp;
   }
 
+  /**
+   * Get the SPI port number.
+   *
+   * @return The SPI port number.
+   */
+  public int getPort() {
+    return m_spi_port.value;
+  }
+
   @Override
   public void initSendable(NTSendableBuilder builder) {
     builder.setSmartDashboardType("Gyro");

wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java

@@ -14,6 +14,8 @@ package edu.wpi.first.wpilibj;
 // import java.lang.FdLibm.Pow;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
 import edu.wpi.first.networktables.NTSendable;
 import edu.wpi.first.networktables.NTSendableBuilder;
 import edu.wpi.first.wpilibj.interfaces.Gyro;
@@ -250,6 +252,17 @@ public class ADIS16470_IMU implements Gyro, NTSendable {
   private DigitalOutput m_status_led;
   private Thread m_acquire_task;
 
+  private SimDevice m_simDevice;
+  private SimDouble m_simGyroAngleX;
+  private SimDouble m_simGyroAngleY;
+  private SimDouble m_simGyroAngleZ;
+  private SimDouble m_simGyroRateX;
+  private SimDouble m_simGyroRateY;
+  private SimDouble m_simGyroRateZ;
+  private SimDouble m_simAccelX;
+  private SimDouble m_simAccelY;
+  private SimDouble m_simAccelZ;
+
   private static class AcquireTask implements Runnable {
     private ADIS16470_IMU imu;
 
@@ -279,59 +292,74 @@ public class ADIS16470_IMU implements Gyro, NTSendable {
 
     m_acquire_task = new Thread(new AcquireTask(this));
 
-    // Force the IMU reset pin to toggle on startup (doesn't require DS enable)
-    // Relies on the RIO hardware by default configuring an output as low
-    // and configuring an input as high Z. The 10k pull-up resistor internal to the
-    // IMU then forces the reset line high for normal operation.
-    m_reset_out = new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low
-    Timer.delay(0.01); // Wait 10ms
-    m_reset_out.close();
-    m_reset_in = new DigitalInput(27); // Set SPI CS2 (IMU RST) high
-    Timer.delay(0.25); // Wait 250ms for reset to complete
-
-    if (!switchToStandardSPI()) {
-      return;
+    m_simDevice = SimDevice.create("Gyro:ADIS16470", port.value);
+    if (m_simDevice != null) {
+      m_simGyroAngleX = m_simDevice.createDouble("gyro_angle_x", SimDevice.Direction.kInput, 0.0);
+      m_simGyroAngleY = m_simDevice.createDouble("gyro_angle_y", SimDevice.Direction.kInput, 0.0);
+      m_simGyroAngleZ = m_simDevice.createDouble("gyro_angle_z", SimDevice.Direction.kInput, 0.0);
+      m_simGyroRateX = m_simDevice.createDouble("gyro_rate_x", SimDevice.Direction.kInput, 0.0);
+      m_simGyroRateY = m_simDevice.createDouble("gyro_rate_y", SimDevice.Direction.kInput, 0.0);
+      m_simGyroRateZ = m_simDevice.createDouble("gyro_rate_z", SimDevice.Direction.kInput, 0.0);
+      m_simAccelX = m_simDevice.createDouble("accel_x", SimDevice.Direction.kInput, 0.0);
+      m_simAccelY = m_simDevice.createDouble("accel_y", SimDevice.Direction.kInput, 0.0);
+      m_simAccelZ = m_simDevice.createDouble("accel_z", SimDevice.Direction.kInput, 0.0);
     }
 
-    // Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1) =
-    // 400Hz)
-    writeRegister(DEC_RATE, 4);
+    if (m_simDevice == null) {
+      // Force the IMU reset pin to toggle on startup (doesn't require DS enable)
+      // Relies on the RIO hardware by default configuring an output as low
+      // and configuring an input as high Z. The 10k pull-up resistor internal to the
+      // IMU then forces the reset line high for normal operation.
+      m_reset_out = new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low
+      Timer.delay(0.01); // Wait 10ms
+      m_reset_out.close();
+      m_reset_in = new DigitalInput(27); // Set SPI CS2 (IMU RST) high
+      Timer.delay(0.25); // Wait 250ms for reset to complete
 
-    // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation and
-    // PoP
-    writeRegister(MSC_CTRL, 1);
+      if (!switchToStandardSPI()) {
+        return;
+      }
 
-    // Configure IMU internal Bartlett filter
-    writeRegister(FILT_CTRL, 0);
+      // Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1) =
+      // 400Hz)
+      writeRegister(DEC_RATE, 4);
 
-    // Configure continuous bias calibration time based on user setting
-    writeRegister(NULL_CNFG, (m_calibration_time | 0x0700));
+      // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation and
+      // PoP
+      writeRegister(MSC_CTRL, 1);
 
-    // Notify DS that IMU calibration delay is active
-    DriverStation.reportWarning(
-        "ADIS16470 IMU Detected. Starting initial calibration delay.", false);
+      // Configure IMU internal Bartlett filter
+      writeRegister(FILT_CTRL, 0);
 
-    // Wait for samples to accumulate internal to the IMU (110% of user-defined
-    // time)
-    try {
-      Thread.sleep((long) (Math.pow(2.0, m_calibration_time) / 2000 * 64 * 1.1 * 1000));
-    } catch (InterruptedException e) {
+      // Configure continuous bias calibration time based on user setting
+      writeRegister(NULL_CNFG, (m_calibration_time | 0x0700));
+
+      // Notify DS that IMU calibration delay is active
+      DriverStation.reportWarning(
+          "ADIS16470 IMU Detected. Starting initial calibration delay.", false);
+
+      // Wait for samples to accumulate internal to the IMU (110% of user-defined
+      // time)
+      try {
+        Thread.sleep((long) (Math.pow(2.0, m_calibration_time) / 2000 * 64 * 1.1 * 1000));
+      } catch (InterruptedException e) {
+      }
+
+      // Write offset calibration command to IMU
+      writeRegister(GLOB_CMD, 0x0001);
+
+      // Configure and enable auto SPI
+      if (!switchToAutoSPI()) {
+        return;
+      }
+
+      // Let the user know the IMU was initiallized successfully
+      DriverStation.reportWarning("ADIS16470 IMU Successfully Initialized!", false);
+
+      // Drive "Ready" LED low
+      m_status_led = new DigitalOutput(28); // Set SPI CS3 (IMU Ready LED) low
     }
 
-    // Write offset calibration command to IMU
-    writeRegister(GLOB_CMD, 0x0001);
-
-    // Configure and enable auto SPI
-    if (!switchToAutoSPI()) {
-      return;
-    }
-
-    // Let the user know the IMU was initiallized successfully
-    DriverStation.reportWarning("ADIS16470 IMU Successfully Initialized!", false);
-
-    // Drive "Ready" LED low
-    m_status_led = new DigitalOutput(28); // Set SPI CS3 (IMU Ready LED) low
-
     // Report usage and post data to DS
     HAL.report(tResourceType.kResourceType_ADIS16470, 0);
   }
@@ -890,6 +918,23 @@ public class ADIS16470_IMU implements Gyro, NTSendable {
 
   /** {@inheritDoc} */
   public synchronized double getAngle() {
+    switch (m_yaw_axis) {
+      case kX:
+        if (m_simGyroAngleX != null) {
+          return m_simGyroAngleX.get();
+        }
+        break;
+      case kY:
+        if (m_simGyroAngleY != null) {
+          return m_simGyroAngleY.get();
+        }
+        break;
+      case kZ:
+        if (m_simGyroAngleZ != null) {
+          return m_simGyroAngleZ.get();
+        }
+        break;
+    }
     return m_integ_angle;
   }
 
@@ -897,11 +942,11 @@ public class ADIS16470_IMU implements Gyro, NTSendable {
   public synchronized double getRate() {
     switch (m_yaw_axis) {
       case kX:
-        return m_gyro_x;
+        return getGyroRateX();
       case kY:
-        return m_gyro_y;
+        return getGyroRateY();
       case kZ:
-        return m_gyro_z;
+        return getGyroRateZ();
     }
     return 0.0;
   }
@@ -912,17 +957,26 @@ public class ADIS16470_IMU implements Gyro, NTSendable {
   }
 
   /** @return current gyro angle in the X direction */
-  public synchronized double getGyroInstantX() {
+  public synchronized double getGyroRateX() {
+    if (m_simGyroRateX != null) {
+      return m_simGyroRateX.get();
+    }
     return m_gyro_x;
   }
 
   /** @return current gyro angle in the Y axis */
-  public synchronized double getGyroInstantY() {
+  public synchronized double getGyroRateY() {
+    if (m_simGyroRateY != null) {
+      return m_simGyroRateY.get();
+    }
     return m_gyro_y;
   }
 
   /** @return current gyro angle in the Z axis */
-  public synchronized double getGyroInstantZ() {
+  public synchronized double getGyroRateZ() {
+    if (m_simGyroRateZ != null) {
+      return m_simGyroRateZ.get();
+    }
     return m_gyro_z;
   }
 
@@ -961,6 +1015,15 @@ public class ADIS16470_IMU implements Gyro, NTSendable {
     return m_accelAngleY;
   }
 
+  /**
+   * Get the SPI port number.
+   *
+   * @return The SPI port number.
+   */
+  public int getPort() {
+    return m_spi_port.value;
+  }
+
   @Override
   public void initSendable(NTSendableBuilder builder) {
     builder.setSmartDashboardType("Gyro");

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16448_IMUSim.java

@@ -0,0 +1,121 @@
+// 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 edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.ADIS16448_IMU;
+
+/** Class to control a simulated ADIS16448 gyroscope. */
+@SuppressWarnings({"TypeName", "AbbreviationAsWordInName"})
+public class ADIS16448_IMUSim {
+  private final SimDouble m_simGyroAngleX;
+  private final SimDouble m_simGyroAngleY;
+  private final SimDouble m_simGyroAngleZ;
+  private final SimDouble m_simGyroRateX;
+  private final SimDouble m_simGyroRateY;
+  private final SimDouble m_simGyroRateZ;
+  private final SimDouble m_simAccelX;
+  private final SimDouble m_simAccelY;
+  private final SimDouble m_simAccelZ;
+
+  /**
+   * Constructs from an ADIS16448_IMU object.
+   *
+   * @param gyro ADIS16448_IMU to simulate
+   */
+  public ADIS16448_IMUSim(ADIS16448_IMU gyro) {
+    SimDeviceSim wrappedSimDevice = new SimDeviceSim("Gyro:ADIS16448" + "[" + gyro.getPort() + "]");
+    m_simGyroAngleX = wrappedSimDevice.getDouble("gyro_angle_x");
+    m_simGyroAngleY = wrappedSimDevice.getDouble("gyro_angle_y");
+    m_simGyroAngleZ = wrappedSimDevice.getDouble("gyro_angle_z");
+    m_simGyroRateX = wrappedSimDevice.getDouble("gyro_rate_x");
+    m_simGyroRateY = wrappedSimDevice.getDouble("gyro_rate_y");
+    m_simGyroRateZ = wrappedSimDevice.getDouble("gyro_rate_z");
+    m_simAccelX = wrappedSimDevice.getDouble("accel_x");
+    m_simAccelY = wrappedSimDevice.getDouble("accel_y");
+    m_simAccelZ = wrappedSimDevice.getDouble("accel_z");
+  }
+
+  /**
+   * Sets the X axis angle (CCW positive).
+   *
+   * @param angleDegrees The angle.
+   */
+  public void setGyroAngleX(double angleDegrees) {
+    m_simGyroAngleX.set(angleDegrees);
+  }
+
+  /**
+   * Sets the Y axis angle (CCW positive).
+   *
+   * @param angleDegrees The angle.
+   */
+  public void setGyroAngleY(double angleDegrees) {
+    m_simGyroAngleY.set(angleDegrees);
+  }
+
+  /**
+   * Sets the Z axis angle (CCW positive).
+   *
+   * @param angleDegrees The angle.
+   */
+  public void setGyroAngleZ(double angleDegrees) {
+    m_simGyroAngleZ.set(angleDegrees);
+  }
+
+  /**
+   * Sets the X axis angular rate (CCW positive).
+   *
+   * @param rateDegreesPerSecond The angular rate.
+   */
+  public void setGyroRateX(double rateDegreesPerSecond) {
+    m_simGyroRateX.set(rateDegreesPerSecond);
+  }
+
+  /**
+   * Sets the Y axis angular rate (CCW positive).
+   *
+   * @param rateDegreesPerSecond The angular rate.
+   */
+  public void setGyroRateY(double rateDegreesPerSecond) {
+    m_simGyroRateY.set(rateDegreesPerSecond);
+  }
+
+  /**
+   * Sets the Z axis angular rate (CCW positive).
+   *
+   * @param rateDegreesPerSecond The angular rate.
+   */
+  public void setGyroRateZ(double rateDegreesPerSecond) {
+    m_simGyroRateZ.set(rateDegreesPerSecond);
+  }
+
+  /**
+   * Sets the X axis acceleration.
+   *
+   * @param accelMetersPerSecond The acceleration.
+   */
+  public void setAccelX(double accelMetersPerSecond) {
+    m_simAccelX.set(accelMetersPerSecond / 9.81);
+  }
+
+  /**
+   * Sets the Y axis acceleration.
+   *
+   * @param accelMetersPerSecond The acceleration.
+   */
+  public void setAccelY(double accelMetersPerSecond) {
+    m_simAccelY.set(accelMetersPerSecond / 9.81);
+  }
+
+  /**
+   * Sets the Z axis acceleration.
+   *
+   * @param accelMetersPerSecond The acceleration.
+   */
+  public void setAccelZ(double accelMetersPerSecond) {
+    m_simAccelZ.set(accelMetersPerSecond / 9.81);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16470_IMUSim.java

@@ -0,0 +1,121 @@
+// 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 edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.ADIS16470_IMU;
+
+/** Class to control a simulated ADIS16470 gyroscope. */
+@SuppressWarnings({"TypeName", "AbbreviationAsWordInName"})
+public class ADIS16470_IMUSim {
+  private final SimDouble m_simGyroAngleX;
+  private final SimDouble m_simGyroAngleY;
+  private final SimDouble m_simGyroAngleZ;
+  private final SimDouble m_simGyroRateX;
+  private final SimDouble m_simGyroRateY;
+  private final SimDouble m_simGyroRateZ;
+  private final SimDouble m_simAccelX;
+  private final SimDouble m_simAccelY;
+  private final SimDouble m_simAccelZ;
+
+  /**
+   * Constructs from an ADIS16470_IMU object.
+   *
+   * @param gyro ADIS16470_IMU to simulate
+   */
+  public ADIS16470_IMUSim(ADIS16470_IMU gyro) {
+    SimDeviceSim wrappedSimDevice = new SimDeviceSim("Gyro:ADIS16470" + "[" + gyro.getPort() + "]");
+    m_simGyroAngleX = wrappedSimDevice.getDouble("gyro_angle_x");
+    m_simGyroAngleY = wrappedSimDevice.getDouble("gyro_angle_y");
+    m_simGyroAngleZ = wrappedSimDevice.getDouble("gyro_angle_z");
+    m_simGyroRateX = wrappedSimDevice.getDouble("gyro_rate_x");
+    m_simGyroRateY = wrappedSimDevice.getDouble("gyro_rate_y");
+    m_simGyroRateZ = wrappedSimDevice.getDouble("gyro_rate_z");
+    m_simAccelX = wrappedSimDevice.getDouble("accel_x");
+    m_simAccelY = wrappedSimDevice.getDouble("accel_y");
+    m_simAccelZ = wrappedSimDevice.getDouble("accel_z");
+  }
+
+  /**
+   * Sets the X axis angle (CCW positive).
+   *
+   * @param angleDegrees The angle.
+   */
+  public void setGyroAngleX(double angleDegrees) {
+    m_simGyroAngleX.set(angleDegrees);
+  }
+
+  /**
+   * Sets the Y axis angle (CCW positive).
+   *
+   * @param angleDegrees The angle.
+   */
+  public void setGyroAngleY(double angleDegrees) {
+    m_simGyroAngleY.set(angleDegrees);
+  }
+
+  /**
+   * Sets the Z axis angle (CCW positive).
+   *
+   * @param angleDegrees The angle.
+   */
+  public void setGyroAngleZ(double angleDegrees) {
+    m_simGyroAngleZ.set(angleDegrees);
+  }
+
+  /**
+   * Sets the X axis angular rate (CCW positive).
+   *
+   * @param rateDegreesPerSecond The angular rate.
+   */
+  public void setGyroRateX(double rateDegreesPerSecond) {
+    m_simGyroRateX.set(rateDegreesPerSecond);
+  }
+
+  /**
+   * Sets the Y axis angular rate (CCW positive).
+   *
+   * @param rateDegreesPerSecond The angular rate.
+   */
+  public void setGyroRateY(double rateDegreesPerSecond) {
+    m_simGyroRateY.set(rateDegreesPerSecond);
+  }
+
+  /**
+   * Sets the Z axis angular rate (CCW positive).
+   *
+   * @param rateDegreesPerSecond The angular rate.
+   */
+  public void setGyroRateZ(double rateDegreesPerSecond) {
+    m_simGyroRateZ.set(rateDegreesPerSecond);
+  }
+
+  /**
+   * Sets the X axis acceleration.
+   *
+   * @param accelMetersPerSecond The acceleration.
+   */
+  public void setAccelX(double accelMetersPerSecond) {
+    m_simAccelX.set(accelMetersPerSecond / 9.81);
+  }
+
+  /**
+   * Sets the Y axis acceleration.
+   *
+   * @param accelMetersPerSecond The acceleration.
+   */
+  public void setAccelY(double accelMetersPerSecond) {
+    m_simAccelY.set(accelMetersPerSecond / 9.81);
+  }
+
+  /**
+   * Sets the Z axis acceleration.
+   *
+   * @param accelMetersPerSecond The acceleration.
+   */
+  public void setAccelZ(double accelMetersPerSecond) {
+    m_simAccelZ.set(accelMetersPerSecond / 9.81);
+  }
+}