[sim] Add sim wrappers for sensors that use SimDevice (#3517)

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

@@ -110,6 +110,14 @@ public class ADXL345_I2C implements Accelerometer, NTSendable, AutoCloseable {
     SendableRegistry.addLW(this, "ADXL345_I2C", port.value);
   }
 
+  public int getPort() {
+    return m_i2c.getPort();
+  }
+
+  public int getDeviceAddress() {
+    return m_i2c.getDeviceAddress();
+  }
+
   @Override
   public void close() {
     SendableRegistry.remove(this);

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

@@ -94,6 +94,10 @@ public class ADXL345_SPI implements Accelerometer, NTSendable, AutoCloseable {
     SendableRegistry.addLW(this, "ADXL345_SPI", port.value);
   }
 
+  public int getPort() {
+    return m_spi.getPort();
+  }
+
   @Override
   public void close() {
     SendableRegistry.remove(this);

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

@@ -137,6 +137,10 @@ public class ADXL362 implements Accelerometer, NTSendable, AutoCloseable {
     SendableRegistry.addLW(this, "ADXL362", port.value);
   }
 
+  public int getPort() {
+    return m_spi.getPort();
+  }
+
   @Override
   public void close() {
     SendableRegistry.remove(this);

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

@@ -43,7 +43,6 @@ public class ADXRS450_Gyro implements Gyro, Sendable {
   private static final int kSNLowRegister = 0x10;
 
   private SPI m_spi;
-  private SPI.Port m_port;
 
   private SimDevice m_simDevice;
   private SimBoolean m_simConnected;
@@ -62,7 +61,6 @@ public class ADXRS450_Gyro implements Gyro, Sendable {
    */
   public ADXRS450_Gyro(SPI.Port port) {
     m_spi = new SPI(port);
-    m_port = port;
 
     // simulation
     m_simDevice = SimDevice.create("Gyro:ADXRS450", port.value);
@@ -132,7 +130,7 @@ public class ADXRS450_Gyro implements Gyro, Sendable {
    * @return The SPI port number.
    */
   public int getPort() {
-    return m_port.value;
+    return m_spi.getPort();
   }
 
   private boolean calcParity(int value) {

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

@@ -48,6 +48,14 @@ public class I2C implements AutoCloseable {
     HAL.report(tResourceType.kResourceType_I2C, deviceAddress);
   }
 
+  public int getPort() {
+    return m_port;
+  }
+
+  public int getDeviceAddress() {
+    return m_deviceAddress;
+  }
+
   @Override
   public void close() {
     I2CJNI.i2CClose(m_port);

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

@@ -46,6 +46,10 @@ public class SPI implements AutoCloseable {
     HAL.report(tResourceType.kResourceType_SPI, port.value + 1);
   }
 
+  public int getPort() {
+    return m_port;
+  }
+
   @Override
   public void close() {
     if (m_accum != null) {

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

@@ -111,6 +111,10 @@ public class Ultrasonic implements Sendable, AutoCloseable {
     SendableRegistry.addLW(this, "Ultrasonic", m_echoChannel.getChannel());
   }
 
+  public int getEchoChannel() {
+    return m_echoChannel.getChannel();
+  }
+
   /**
    * Create an instance of the Ultrasonic Sensor. This is designed to supchannel the Daventech SRF04
    * and Vex ultrasonic sensors.

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

@@ -0,0 +1,62 @@
+// 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.ADXL345_I2C;
+import edu.wpi.first.wpilibj.ADXL345_SPI;
+import java.util.Objects;
+
+public class ADXL345Sim {
+  protected SimDouble m_simX;
+  protected SimDouble m_simY;
+  protected SimDouble m_simZ;
+
+  /**
+   * Constructor.
+   *
+   * @param device The device to simulate
+   */
+  public ADXL345Sim(ADXL345_SPI device) {
+    SimDeviceSim simDevice = new SimDeviceSim("Accel:ADXL345_SPI" + "[" + device.getPort() + "]");
+    initSim(simDevice);
+  }
+
+  /**
+   * Constructor.
+   *
+   * @param device The device to simulate
+   */
+  public ADXL345Sim(ADXL345_I2C device) {
+    SimDeviceSim simDevice =
+        new SimDeviceSim(
+            "Accel:ADXL345_I2C" + "[" + device.getPort() + "," + device.getDeviceAddress() + "]");
+    initSim(simDevice);
+  }
+
+  private void initSim(SimDeviceSim simDevice) {
+    Objects.requireNonNull(simDevice);
+
+    m_simX = simDevice.getDouble("x");
+    m_simY = simDevice.getDouble("y");
+    m_simZ = simDevice.getDouble("z");
+
+    Objects.requireNonNull(m_simX);
+    Objects.requireNonNull(m_simY);
+    Objects.requireNonNull(m_simZ);
+  }
+
+  public void setX(double x) {
+    m_simX.set(x);
+  }
+
+  public void setY(double y) {
+    m_simY.set(y);
+  }
+
+  public void setZ(double z) {
+    m_simZ.set(z);
+  }
+}

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

@@ -0,0 +1,48 @@
+// 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.ADXL362;
+import java.util.Objects;
+
+public class ADXL362Sim {
+  protected SimDouble m_simX;
+  protected SimDouble m_simY;
+  protected SimDouble m_simZ;
+
+  /**
+   * Constructor.
+   *
+   * @param device The device to simulate
+   */
+  public ADXL362Sim(ADXL362 device) {
+    SimDeviceSim wrappedSimDevice =
+        new SimDeviceSim("Accel:ADXL362" + "[" + device.getPort() + "]");
+    initSim(wrappedSimDevice);
+  }
+
+  private void initSim(SimDeviceSim wrappedSimDevice) {
+    m_simX = wrappedSimDevice.getDouble("x");
+    m_simY = wrappedSimDevice.getDouble("y");
+    m_simZ = wrappedSimDevice.getDouble("z");
+
+    Objects.requireNonNull(m_simX);
+    Objects.requireNonNull(m_simY);
+    Objects.requireNonNull(m_simZ);
+  }
+
+  public void setX(double x) {
+    m_simX.set(x);
+  }
+
+  public void setY(double y) {
+    m_simY.set(y);
+  }
+
+  public void setZ(double z) {
+    m_simZ.set(z);
+  }
+}

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

@@ -0,0 +1,38 @@
+// 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.SimBoolean;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Ultrasonic;
+
+public class UltrasonicSim {
+  private final SimBoolean m_simRangeValid;
+  private final SimDouble m_simRange;
+
+  /**
+   * Constructor.
+   *
+   * @param ultrasonic The real ultrasonic to simulate
+   */
+  public UltrasonicSim(Ultrasonic ultrasonic) {
+    SimDeviceSim simDevice = new SimDeviceSim("Ultrasonic", ultrasonic.getEchoChannel());
+    m_simRangeValid = simDevice.getBoolean("Range Valid");
+    m_simRange = simDevice.getDouble("Range (in)");
+  }
+
+  public void setRangeValid(boolean valid) {
+    m_simRangeValid.set(valid);
+  }
+
+  public void setRangeInches(double inches) {
+    m_simRange.set(inches);
+  }
+
+  public void setRangeMeters(double meters) {
+    m_simRange.set(Units.metersToInches(meters));
+  }
+}