Implement sim devices for ADXL345, ADXL362, ADXRS450, Ultrasonic

This makes the halsim_adx_gyro_accelerometer simulation plugin and
the accelerometer part of lowfi_simulation obsolete.

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

@@ -13,6 +13,9 @@ import java.nio.ByteOrder;
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 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.hal.SimEnum;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.wpilibj.interfaces.Accelerometer;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
@@ -64,6 +67,12 @@ public class ADXL345_I2C implements Accelerometer, Sendable, AutoCloseable {
 
   protected I2C m_i2c;
 
+  protected SimDevice m_simDevice;
+  protected SimEnum m_simRange;
+  protected SimDouble m_simX;
+  protected SimDouble m_simY;
+  protected SimDouble m_simZ;
+
   /**
    * Constructs the ADXL345 Accelerometer with I2C address 0x1D.
    *
@@ -84,6 +93,15 @@ public class ADXL345_I2C implements Accelerometer, Sendable, AutoCloseable {
   public ADXL345_I2C(I2C.Port port, Range range, int deviceAddress) {
     m_i2c = new I2C(port, deviceAddress);
 
+    // simulation
+    m_simDevice = SimDevice.create("ADXL345_I2C", port.value, deviceAddress);
+    if (m_simDevice != null) {
+      m_simRange = m_simDevice.createEnum("Range", true, new String[] {"2G", "4G", "8G", "16G"}, 0);
+      m_simX = m_simDevice.createDouble("X Accel", false, 0.0);
+      m_simX = m_simDevice.createDouble("Y Accel", false, 0.0);
+      m_simZ = m_simDevice.createDouble("Z Accel", false, 0.0);
+    }
+
     // Turn on the measurements
     m_i2c.write(kPowerCtlRegister, kPowerCtl_Measure);
 
@@ -95,7 +113,14 @@ public class ADXL345_I2C implements Accelerometer, Sendable, AutoCloseable {
 
   @Override
   public void close() {
-    m_i2c.close();
+    if (m_i2c != null) {
+      m_i2c.close();
+      m_i2c = null;
+    }
+    if (m_simDevice != null) {
+      m_simDevice.close();
+      m_simDevice = null;
+    }
   }
 
   @Override
@@ -121,6 +146,10 @@ public class ADXL345_I2C implements Accelerometer, Sendable, AutoCloseable {
 
     // Specify the data format to read
     m_i2c.write(kDataFormatRegister, kDataFormat_FullRes | value);
+
+    if (m_simRange != null) {
+      m_simRange.set(value);
+    }
   }
 
   @Override
@@ -145,6 +174,15 @@ public class ADXL345_I2C implements Accelerometer, Sendable, AutoCloseable {
    * @return Acceleration of the ADXL345 in Gs.
    */
   public double getAcceleration(Axes axis) {
+    if (axis == Axes.kX && m_simX != null) {
+      return m_simX.get();
+    }
+    if (axis == Axes.kY && m_simY != null) {
+      return m_simY.get();
+    }
+    if (axis == Axes.kZ && m_simZ != null) {
+      return m_simZ.get();
+    }
     ByteBuffer rawAccel = ByteBuffer.allocate(2);
     m_i2c.read(kDataRegister + axis.value, 2, rawAccel);
 
@@ -160,6 +198,12 @@ public class ADXL345_I2C implements Accelerometer, Sendable, AutoCloseable {
    */
   public AllAxes getAccelerations() {
     AllAxes data = new AllAxes();
+    if (m_simX != null && m_simY != null && m_simZ != null) {
+      data.XAxis = m_simX.get();
+      data.YAxis = m_simY.get();
+      data.ZAxis = m_simZ.get();
+      return data;
+    }
     ByteBuffer rawData = ByteBuffer.allocate(6);
     m_i2c.read(kDataRegister, 6, rawData);
 

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

@@ -13,6 +13,9 @@ import java.nio.ByteOrder;
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 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.hal.SimEnum;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.wpilibj.interfaces.Accelerometer;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
@@ -67,6 +70,12 @@ public class ADXL345_SPI implements Accelerometer, Sendable, AutoCloseable {
 
   protected SPI m_spi;
 
+  protected SimDevice m_simDevice;
+  protected SimEnum m_simRange;
+  protected SimDouble m_simX;
+  protected SimDouble m_simY;
+  protected SimDouble m_simZ;
+
   /**
    * Constructor.
    *
@@ -75,13 +84,28 @@ public class ADXL345_SPI implements Accelerometer, Sendable, AutoCloseable {
    */
   public ADXL345_SPI(SPI.Port port, Range range) {
     m_spi = new SPI(port);
+    // simulation
+    m_simDevice = SimDevice.create("ADXL345_SPI", port.value);
+    if (m_simDevice != null) {
+      m_simRange = m_simDevice.createEnum("Range", true, new String[] {"2G", "4G", "8G", "16G"}, 0);
+      m_simX = m_simDevice.createDouble("X Accel", false, 0.0);
+      m_simX = m_simDevice.createDouble("Y Accel", false, 0.0);
+      m_simZ = m_simDevice.createDouble("Z Accel", false, 0.0);
+    }
     init(range);
     SendableRegistry.addLW(this, "ADXL345_SPI", port.value);
   }
 
   @Override
   public void close() {
-    m_spi.close();
+    if (m_spi != null) {
+      m_spi.close();
+      m_spi = null;
+    }
+    if (m_simDevice != null) {
+      m_simDevice.close();
+      m_simDevice = null;
+    }
   }
 
   /**
@@ -131,6 +155,10 @@ public class ADXL345_SPI implements Accelerometer, Sendable, AutoCloseable {
     // Specify the data format to read
     byte[] commands = new byte[]{kDataFormatRegister, (byte) (kDataFormat_FullRes | value)};
     m_spi.write(commands, commands.length);
+
+    if (m_simRange != null) {
+      m_simRange.set(value);
+    }
   }
 
   @Override
@@ -155,6 +183,15 @@ public class ADXL345_SPI implements Accelerometer, Sendable, AutoCloseable {
    * @return Acceleration of the ADXL345 in Gs.
    */
   public double getAcceleration(ADXL345_SPI.Axes axis) {
+    if (axis == Axes.kX && m_simX != null) {
+      return m_simX.get();
+    }
+    if (axis == Axes.kY && m_simY != null) {
+      return m_simY.get();
+    }
+    if (axis == Axes.kZ && m_simZ != null) {
+      return m_simZ.get();
+    }
     ByteBuffer transferBuffer = ByteBuffer.allocate(3);
     transferBuffer.put(0,
         (byte) ((kAddress_Read | kAddress_MultiByte | kDataRegister) + axis.value));
@@ -172,6 +209,12 @@ public class ADXL345_SPI implements Accelerometer, Sendable, AutoCloseable {
    */
   public ADXL345_SPI.AllAxes getAccelerations() {
     ADXL345_SPI.AllAxes data = new ADXL345_SPI.AllAxes();
+    if (m_simX != null && m_simY != null && m_simZ != null) {
+      data.XAxis = m_simX.get();
+      data.YAxis = m_simY.get();
+      data.ZAxis = m_simZ.get();
+      return data;
+    }
     if (m_spi != null) {
       ByteBuffer dataBuffer = ByteBuffer.allocate(7);
       // Select the data address.

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

@@ -12,6 +12,9 @@ import java.nio.ByteOrder;
 
 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.hal.SimEnum;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.wpilibj.interfaces.Accelerometer;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
@@ -62,6 +65,13 @@ public class ADXL362 implements Accelerometer, Sendable, AutoCloseable {
   }
 
   private SPI m_spi;
+
+  private SimDevice m_simDevice;
+  private SimEnum m_simRange;
+  private SimDouble m_simX;
+  private SimDouble m_simY;
+  private SimDouble m_simZ;
+
   private double m_gsPerLSB;
 
   /**
@@ -82,22 +92,33 @@ public class ADXL362 implements Accelerometer, Sendable, AutoCloseable {
   public ADXL362(SPI.Port port, Range range) {
     m_spi = new SPI(port);
 
+    // simulation
+    m_simDevice = SimDevice.create("ADXL362", port.value);
+    if (m_simDevice != null) {
+      m_simRange = m_simDevice.createEnum("Range", true, new String[] {"2G", "4G", "8G", "16G"}, 0);
+      m_simX = m_simDevice.createDouble("X Accel", false, 0.0);
+      m_simX = m_simDevice.createDouble("Y Accel", false, 0.0);
+      m_simZ = m_simDevice.createDouble("Z Accel", false, 0.0);
+    }
+
     m_spi.setClockRate(3000000);
     m_spi.setMSBFirst();
     m_spi.setSampleDataOnTrailingEdge();
     m_spi.setClockActiveLow();
     m_spi.setChipSelectActiveLow();
 
-    // Validate the part ID
     ByteBuffer transferBuffer = ByteBuffer.allocate(3);
-    transferBuffer.put(0, kRegRead);
-    transferBuffer.put(1, kPartIdRegister);
-    m_spi.transaction(transferBuffer, transferBuffer, 3);
-    if (transferBuffer.get(2) != (byte) 0xF2) {
-      m_spi.close();
-      m_spi = null;
-      DriverStation.reportError("could not find ADXL362 on SPI port " + port.value, false);
-      return;
+    if (m_simDevice == null) {
+      // Validate the part ID
+      transferBuffer.put(0, kRegRead);
+      transferBuffer.put(1, kPartIdRegister);
+      m_spi.transaction(transferBuffer, transferBuffer, 3);
+      if (transferBuffer.get(2) != (byte) 0xF2) {
+        m_spi.close();
+        m_spi = null;
+        DriverStation.reportError("could not find ADXL362 on SPI port " + port.value, false);
+        return;
+      }
     }
 
     setRange(range);
@@ -118,6 +139,10 @@ public class ADXL362 implements Accelerometer, Sendable, AutoCloseable {
       m_spi.close();
       m_spi = null;
     }
+    if (m_simDevice != null) {
+      m_simDevice.close();
+      m_simDevice = null;
+    }
   }
 
   @Override
@@ -150,6 +175,10 @@ public class ADXL362 implements Accelerometer, Sendable, AutoCloseable {
     byte[] commands = new byte[]{kRegWrite, kFilterCtlRegister, (byte) (kFilterCtl_ODR_100Hz
         | value)};
     m_spi.write(commands, commands.length);
+
+    if (m_simRange != null) {
+      m_simRange.set(value);
+    }
   }
 
 
@@ -175,6 +204,15 @@ public class ADXL362 implements Accelerometer, Sendable, AutoCloseable {
    * @return Acceleration of the ADXL362 in Gs.
    */
   public double getAcceleration(ADXL362.Axes axis) {
+    if (axis == Axes.kX && m_simX != null) {
+      return m_simX.get();
+    }
+    if (axis == Axes.kY && m_simY != null) {
+      return m_simY.get();
+    }
+    if (axis == Axes.kZ && m_simZ != null) {
+      return m_simZ.get();
+    }
     if (m_spi == null) {
       return 0.0;
     }
@@ -195,6 +233,12 @@ public class ADXL362 implements Accelerometer, Sendable, AutoCloseable {
    */
   public ADXL362.AllAxes getAccelerations() {
     ADXL362.AllAxes data = new ADXL362.AllAxes();
+    if (m_simX != null && m_simY != null && m_simZ != null) {
+      data.XAxis = m_simX.get();
+      data.YAxis = m_simY.get();
+      data.ZAxis = m_simZ.get();
+      return data;
+    }
     if (m_spi != null) {
       ByteBuffer dataBuffer = ByteBuffer.allocate(8);
       // Select the data address.

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

@@ -12,6 +12,9 @@ import java.nio.ByteOrder;
 
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SimBoolean;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
 import edu.wpi.first.wpilibj.interfaces.Gyro;
 import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
 
@@ -42,6 +45,11 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable
 
   private SPI m_spi;
 
+  private SimDevice m_simDevice;
+  private SimBoolean m_simConnected;
+  private SimDouble m_simAngle;
+  private SimDouble m_simRate;
+
   /**
    * Constructor.  Uses the onboard CS0.
    */
@@ -57,31 +65,49 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable
   public ADXRS450_Gyro(SPI.Port port) {
     m_spi = new SPI(port);
 
+    // simulation
+    m_simDevice = SimDevice.create("ADXRS450_Gyro", port.value);
+    if (m_simDevice != null) {
+      m_simConnected = m_simDevice.createBoolean("Connected", false, true);
+      m_simAngle = m_simDevice.createDouble("Angle", false, 0.0);
+      m_simRate = m_simDevice.createDouble("Rate", false, 0.0);
+    }
+
     m_spi.setClockRate(3000000);
     m_spi.setMSBFirst();
     m_spi.setSampleDataOnLeadingEdge();
     m_spi.setClockActiveHigh();
     m_spi.setChipSelectActiveLow();
 
-    // Validate the part ID
-    if ((readRegister(kPIDRegister) & 0xff00) != 0x5200) {
-      m_spi.close();
-      m_spi = null;
-      DriverStation.reportError("could not find ADXRS450 gyro on SPI port " + port.value,
-          false);
-      return;
+    if (m_simDevice == null) {
+      // Validate the part ID
+      if ((readRegister(kPIDRegister) & 0xff00) != 0x5200) {
+        m_spi.close();
+        m_spi = null;
+        DriverStation.reportError("could not find ADXRS450 gyro on SPI port " + port.value,
+            false);
+        return;
+      }
+
+      m_spi.initAccumulator(kSamplePeriod, 0x20000000, 4, 0x0c00000e, 0x04000000, 10, 16,
+          true, true);
+
+      calibrate();
     }
 
-    m_spi.initAccumulator(kSamplePeriod, 0x20000000, 4, 0x0c00000e, 0x04000000, 10, 16,
-        true, true);
-
-    calibrate();
-
     HAL.report(tResourceType.kResourceType_ADXRS450, port.value);
     SendableRegistry.addLW(this, "ADXRS450_Gyro", port.value);
   }
 
+  /**
+   * Determine if the gyro is connected.
+   *
+   * @return true if it is connected, false otherwise.
+   */
   public boolean isConnected() {
+    if (m_simConnected != null) {
+      return m_simConnected.get();
+    }
     return m_spi != null;
   }
 
@@ -133,6 +159,12 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable
 
   @Override
   public void reset() {
+    if (m_simAngle != null) {
+      m_simAngle.set(0.0);
+    }
+    if (m_simRate != null) {
+      m_simRate.set(0.0);
+    }
     if (m_spi != null) {
       m_spi.resetAccumulator();
     }
@@ -147,10 +179,17 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable
       m_spi.close();
       m_spi = null;
     }
+    if (m_simDevice != null) {
+      m_simDevice.close();
+      m_simDevice = null;
+    }
   }
 
   @Override
   public double getAngle() {
+    if (m_simAngle != null) {
+      return m_simAngle.get();
+    }
     if (m_spi == null) {
       return 0.0;
     }
@@ -159,6 +198,9 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable
 
   @Override
   public double getRate() {
+    if (m_simRate != null) {
+      return m_simRate.get();
+    }
     if (m_spi == null) {
       return 0.0;
     }

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

@@ -12,6 +12,9 @@ import java.util.List;
 
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.SimBoolean;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDouble;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
 import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
 
@@ -59,6 +62,10 @@ public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
   private static int m_instances;
   protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
 
+  private SimDevice m_simDevice;
+  private SimBoolean m_simRangeValid;
+  private SimDouble m_simRange;
+
   /**
    * Background task that goes through the list of ultrasonic sensors and pings each one in turn.
    * The counter is configured to read the timing of the returned echo pulse.
@@ -94,6 +101,13 @@ public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
    * then automatic mode is restored.
    */
   private synchronized void initialize() {
+    m_simDevice = SimDevice.create("Ultrasonic", m_echoChannel.getChannel());
+    if (m_simDevice != null) {
+      m_simRangeValid = m_simDevice.createBoolean("Range Valid", false, true);
+      m_simRange = m_simDevice.createDouble("Range (in)", false, 0.0);
+      m_pingChannel.setSimDevice(m_simDevice);
+      m_echoChannel.setSimDevice(m_simDevice);
+    }
     if (m_task == null) {
       m_task = new UltrasonicChecker();
     }
@@ -216,6 +230,11 @@ public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
     if (!m_sensors.isEmpty() && wasAutomaticMode) {
       setAutomaticMode(true);
     }
+
+    if (m_simDevice != null) {
+      m_simDevice.close();
+      m_simDevice = null;
+    }
   }
 
   /**
@@ -285,6 +304,9 @@ public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
    * @return true if the range is valid
    */
   public boolean isRangeValid() {
+    if (m_simRangeValid != null) {
+      return m_simRangeValid.get();
+    }
     return m_counter.get() > 1;
   }
 
@@ -296,6 +318,9 @@ public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
    */
   public double getRangeInches() {
     if (isRangeValid()) {
+      if (m_simRange != null) {
+        return m_simRange.get();
+      }
       return m_counter.getPeriod() * kSpeedOfSoundInchesPerSec / 2.0;
     } else {
       return 0;