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

wpilibc/src/main/native/cpp/ADXL345_I2C.cpp

@@ -32,6 +32,14 @@ ADXL345_I2C::ADXL345_I2C(I2C::Port port, Range range, int deviceAddress)
   wpi::SendableRegistry::AddLW(this, "ADXL345_I2C", port);
 }
 
+I2C::Port ADXL345_I2C::GetI2CPort() const {
+  return m_i2c.GetPort();
+}
+
+int ADXL345_I2C::GetI2CDeviceAddress() const {
+  return m_i2c.GetDeviceAddress();
+}
+
 void ADXL345_I2C::SetRange(Range range) {
   m_i2c.Write(kDataFormatRegister,
               kDataFormat_FullRes | static_cast<uint8_t>(range));

wpilibc/src/main/native/cpp/ADXL345_SPI.cpp

@@ -40,6 +40,10 @@ ADXL345_SPI::ADXL345_SPI(SPI::Port port, ADXL345_SPI::Range range)
   wpi::SendableRegistry::AddLW(this, "ADXL345_SPI", port);
 }
 
+SPI::Port ADXL345_SPI::GetSpiPort() const {
+  return m_spi.GetPort();
+}
+
 void ADXL345_SPI::SetRange(Range range) {
   uint8_t commands[2];
 

wpilibc/src/main/native/cpp/ADXL362.cpp

@@ -75,6 +75,10 @@ ADXL362::ADXL362(SPI::Port port, Range range)
   wpi::SendableRegistry::AddLW(this, "ADXL362", port);
 }
 
+SPI::Port ADXL362::GetSpiPort() const {
+  return m_spi.GetPort();
+}
+
 void ADXL362::SetRange(Range range) {
   if (m_gsPerLSB == 0.0) {
     return;

wpilibc/src/main/native/cpp/I2C.cpp

@@ -26,6 +26,14 @@ I2C::~I2C() {
   HAL_CloseI2C(m_port);
 }
 
+I2C::Port I2C::GetPort() const {
+  return static_cast<Port>(static_cast<int>(m_port));
+}
+
+int I2C::GetDeviceAddress() const {
+  return m_deviceAddress;
+}
+
 bool I2C::Transaction(uint8_t* dataToSend, int sendSize, uint8_t* dataReceived,
                       int receiveSize) {
   int32_t status = 0;

wpilibc/src/main/native/cpp/SPI.cpp

@@ -168,6 +168,10 @@ SPI::~SPI() {
   HAL_CloseSPI(m_port);
 }
 
+SPI::Port SPI::GetPort() const {
+  return static_cast<Port>(static_cast<int>(m_port));
+}
+
 void SPI::SetClockRate(int hz) {
   HAL_SetSPISpeed(m_port, hz);
 }

wpilibc/src/main/native/cpp/Ultrasonic.cpp

@@ -80,6 +80,10 @@ Ultrasonic::~Ultrasonic() {
   }
 }
 
+int Ultrasonic::GetEchoChannel() const {
+  return m_echoChannel->GetChannel();
+}
+
 void Ultrasonic::Ping() {
   if (m_automaticEnabled) {
     throw FRC_MakeError(err::IncompatibleMode, "{}",

wpilibc/src/main/native/cpp/simulation/ADXL345Sim.cpp

@@ -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.
+
+#include "frc/simulation/ADXL345Sim.h"
+
+#include "frc/ADXL345_I2C.h"
+#include "frc/ADXL345_SPI.h"
+#include "frc/simulation/SimDeviceSim.h"
+
+using namespace frc::sim;
+
+ADXL345Sim::ADXL345Sim(const frc::ADXL345_I2C& accel) {
+  frc::sim::SimDeviceSim deviceSim{"Accel:ADXL345_I2C", accel.GetI2CPort(),
+                                   accel.GetI2CDeviceAddress()};
+  m_simX = deviceSim.GetDouble("x");
+  m_simY = deviceSim.GetDouble("y");
+  m_simZ = deviceSim.GetDouble("z");
+}
+
+ADXL345Sim::ADXL345Sim(const frc::ADXL345_SPI& accel) {
+  frc::sim::SimDeviceSim deviceSim{"Accel:ADXL345_SPI", accel.GetSpiPort()};
+  m_simX = deviceSim.GetDouble("x");
+  m_simY = deviceSim.GetDouble("y");
+  m_simZ = deviceSim.GetDouble("z");
+}
+
+void ADXL345Sim::SetX(double accel) {
+  m_simX.Set(accel);
+}
+
+void ADXL345Sim::SetY(double accel) {
+  m_simY.Set(accel);
+}
+
+void ADXL345Sim::SetZ(double accel) {
+  m_simZ.Set(accel);
+}

wpilibc/src/main/native/cpp/simulation/ADXL362Sim.cpp

@@ -0,0 +1,29 @@
+// 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.
+
+#include "frc/simulation/ADXL362Sim.h"
+
+#include "frc/ADXL362.h"
+#include "frc/simulation/SimDeviceSim.h"
+
+using namespace frc::sim;
+
+ADXL362Sim::ADXL362Sim(const frc::ADXL362& accel) {
+  frc::sim::SimDeviceSim deviceSim{"Accel:ADXL362", accel.GetSpiPort()};
+  m_simX = deviceSim.GetDouble("x");
+  m_simY = deviceSim.GetDouble("y");
+  m_simZ = deviceSim.GetDouble("z");
+}
+
+void ADXL362Sim::SetX(double accel) {
+  m_simX.Set(accel);
+}
+
+void ADXL362Sim::SetY(double accel) {
+  m_simY.Set(accel);
+}
+
+void ADXL362Sim::SetZ(double accel) {
+  m_simZ.Set(accel);
+}

wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp

@@ -0,0 +1,24 @@
+// 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.
+
+#include "frc/simulation/UltrasonicSim.h"
+
+#include "frc/Ultrasonic.h"
+#include "frc/simulation/SimDeviceSim.h"
+
+using namespace frc::sim;
+
+UltrasonicSim::UltrasonicSim(const frc::Ultrasonic& ultrasonic) {
+  frc::sim::SimDeviceSim deviceSim{"Ultrasonic", ultrasonic.GetEchoChannel()};
+  m_simRangeValid = deviceSim.GetBoolean("Range Valid");
+  m_simRange = deviceSim.GetDouble("Range (in)");
+}
+
+void UltrasonicSim::SetRangeValid(bool isValid) {
+  m_simRangeValid.Set(isValid);
+}
+
+void UltrasonicSim::SetRange(units::meter_t range) {
+  m_simRange.Set(range.to<double>());
+}

wpilibc/src/main/native/include/frc/ADXL345_I2C.h

@@ -46,6 +46,9 @@ class ADXL345_I2C : public Accelerometer,
   ADXL345_I2C(ADXL345_I2C&&) = default;
   ADXL345_I2C& operator=(ADXL345_I2C&&) = default;
 
+  I2C::Port GetI2CPort() const;
+  int GetI2CDeviceAddress() const;
+
   // Accelerometer interface
   void SetRange(Range range) final;
   double GetX() override;

wpilibc/src/main/native/include/frc/ADXL345_SPI.h

@@ -44,6 +44,8 @@ class ADXL345_SPI : public Accelerometer,
   ADXL345_SPI(ADXL345_SPI&&) = default;
   ADXL345_SPI& operator=(ADXL345_SPI&&) = default;
 
+  SPI::Port GetSpiPort() const;
+
   // Accelerometer interface
   void SetRange(Range range) final;
   double GetX() override;

wpilibc/src/main/native/include/frc/ADXL362.h

@@ -50,6 +50,8 @@ class ADXL362 : public Accelerometer,
   ADXL362(ADXL362&&) = default;
   ADXL362& operator=(ADXL362&&) = default;
 
+  SPI::Port GetSpiPort() const;
+
   // Accelerometer interface
   void SetRange(Range range) final;
   double GetX() override;

wpilibc/src/main/native/include/frc/I2C.h

@@ -33,6 +33,9 @@ class I2C {
   I2C(I2C&&) = default;
   I2C& operator=(I2C&&) = default;
 
+  Port GetPort() const;
+  int GetDeviceAddress() const;
+
   /**
    * Generic transaction.
    *

wpilibc/src/main/native/include/frc/SPI.h

@@ -40,6 +40,8 @@ class SPI {
   SPI(SPI&&) = default;
   SPI& operator=(SPI&&) = default;
 
+  Port GetPort() const;
+
   /**
    * Configure the rate of the generated clock signal.
    *

wpilibc/src/main/native/include/frc/Ultrasonic.h

@@ -90,6 +90,8 @@ class Ultrasonic : public wpi::Sendable,
   Ultrasonic(Ultrasonic&&) = default;
   Ultrasonic& operator=(Ultrasonic&&) = default;
 
+  int GetEchoChannel() const;
+
   /**
    * Single ping to ultrasonic sensor.
    *

wpilibc/src/main/native/include/frc/simulation/ADXL345Sim.h

@@ -0,0 +1,57 @@
+// 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.
+
+#pragma once
+
+#include <hal/SimDevice.h>
+
+namespace frc {
+
+class ADXL345_SPI;
+class ADXL345_I2C;
+
+namespace sim {
+
+/**
+ * Class to control a simulated ADXRS450 gyroscope.
+ */
+class ADXL345Sim {
+ public:
+  /**
+   * Constructs from a ADXL345_I2C object.
+   *
+   * @param ADXL345 accel to simulate
+   */
+  explicit ADXL345Sim(const ADXL345_I2C& accel);
+
+  /**
+   * Constructs from a ADXL345_SPI object.
+   *
+   * @param ADXL345 accel to simulate
+   */
+  explicit ADXL345Sim(const ADXL345_SPI& accel);
+
+  /**
+   * Sets the X acceleration.
+   */
+  void SetX(double accel);
+
+  /**
+   * Sets the Y acceleration.
+   */
+  void SetY(double accel);
+
+  /**
+   * Sets the Z acceleration.
+   */
+  void SetZ(double accel);
+
+ private:
+  hal::SimDouble m_simX;
+  hal::SimDouble m_simY;
+  hal::SimDouble m_simZ;
+};
+
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/frc/simulation/ADXL362Sim.h

@@ -0,0 +1,49 @@
+// 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.
+
+#pragma once
+
+#include <hal/SimDevice.h>
+
+namespace frc {
+
+class ADXL362;
+
+namespace sim {
+
+/**
+ * Class to control a simulated ADXRS450 gyroscope.
+ */
+class ADXL362Sim {
+ public:
+  /**
+   * Constructs from a ADXL362 object.
+   *
+   * @param ADXL362 accel to simulate
+   */
+  explicit ADXL362Sim(const ADXL362& accel);
+
+  /**
+   * Sets the X acceleration.
+   */
+  void SetX(double accel);
+
+  /**
+   * Sets the Y acceleration.
+   */
+  void SetY(double accel);
+
+  /**
+   * Sets the Z acceleration.
+   */
+  void SetZ(double accel);
+
+ private:
+  hal::SimDouble m_simX;
+  hal::SimDouble m_simY;
+  hal::SimDouble m_simZ;
+};
+
+}  // namespace sim
+}  // namespace frc

wpilibc/src/main/native/include/frc/simulation/UltrasonicSim.h

@@ -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.
+
+#pragma once
+
+#include <hal/SimDevice.h>
+#include <units/length.h>
+
+namespace frc {
+
+class Ultrasonic;
+
+namespace sim {
+
+/**
+ * Class to control a simulated ADXRS450 gyroscope.
+ */
+class UltrasonicSim {
+ public:
+  /**
+   * Constructs from a ADXRS450_Gyro object.
+   *
+   * @param gyro ADXRS450_Gyro to simulate
+   */
+  explicit UltrasonicSim(const Ultrasonic& gyro);
+
+  /**
+   * Sets if the range measurement is valid.
+   *
+   * @param valid True if valid
+   */
+  void SetRangeValid(bool isValid);
+
+  /**
+   * Sets the range measurement
+   *
+   * @param rate The range
+   */
+  void SetRange(units::meter_t range);
+
+ private:
+  hal::SimBoolean m_simRangeValid;
+  hal::SimDouble m_simRange;
+};
+
+}  // namespace sim
+}  // namespace frc

wpilibc/src/test/native/cpp/UltrasonicTest.cpp

@@ -0,0 +1,26 @@
+// 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.
+
+#include "frc/Ultrasonic.h"
+#include "frc/simulation/UltrasonicSim.h"
+#include "gtest/gtest.h"
+
+namespace frc {
+
+TEST(UltrasonicTest, SimDevices) {
+  Ultrasonic ultrasonic{0, 1};
+  sim::UltrasonicSim sim{ultrasonic};
+
+  EXPECT_EQ(0, ultrasonic.GetRange().to<double>());
+  EXPECT_TRUE(ultrasonic.IsRangeValid());
+
+  sim.SetRange(units::meter_t{35.04});
+  EXPECT_EQ(35.04, ultrasonic.GetRange().to<double>());
+
+  sim.SetRangeValid(false);
+  EXPECT_FALSE(ultrasonic.IsRangeValid());
+  EXPECT_EQ(0, ultrasonic.GetRange().to<double>());
+}
+
+}  // namespace frc

wpilibc/src/test/native/cpp/simulation/ADXL345SimTest.cpp

@@ -0,0 +1,63 @@
+// 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.
+
+#include "frc/simulation/ADXL345Sim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+
+#include "frc/ADXL345_I2C.h"
+#include "frc/ADXL345_SPI.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+TEST(ADXL345SimTests, SetSpiAttributes) {
+  HAL_Initialize(500, 0);
+
+  ADXL345_SPI accel(SPI::kMXP, Accelerometer::kRange_2G);
+  ADXL345Sim sim(accel);
+
+  EXPECT_EQ(0, accel.GetX());
+  EXPECT_EQ(0, accel.GetY());
+  EXPECT_EQ(0, accel.GetZ());
+
+  sim.SetX(1.91);
+  sim.SetY(-3.405);
+  sim.SetZ(2.29);
+
+  EXPECT_EQ(1.91, accel.GetX());
+  EXPECT_EQ(-3.405, accel.GetY());
+  EXPECT_EQ(2.29, accel.GetZ());
+
+  ADXL345_SPI::AllAxes allAccel = accel.GetAccelerations();
+  EXPECT_EQ(1.91, allAccel.XAxis);
+  EXPECT_EQ(-3.405, allAccel.YAxis);
+  EXPECT_EQ(2.29, allAccel.ZAxis);
+}
+
+TEST(ADXL345SimTests, SetI2CAttribute) {
+  HAL_Initialize(500, 0);
+
+  ADXL345_I2C accel(I2C::kMXP);
+  ADXL345Sim sim(accel);
+
+  EXPECT_EQ(0, accel.GetX());
+  EXPECT_EQ(0, accel.GetY());
+  EXPECT_EQ(0, accel.GetZ());
+
+  sim.SetX(1.91);
+  sim.SetY(-3.405);
+  sim.SetZ(2.29);
+
+  EXPECT_EQ(1.91, accel.GetX());
+  EXPECT_EQ(-3.405, accel.GetY());
+  EXPECT_EQ(2.29, accel.GetZ());
+
+  ADXL345_I2C::AllAxes allAccel = accel.GetAccelerations();
+  EXPECT_EQ(1.91, allAccel.XAxis);
+  EXPECT_EQ(-3.405, allAccel.YAxis);
+  EXPECT_EQ(2.29, allAccel.ZAxis);
+}
+
+}  // namespace frc::sim

wpilibc/src/test/native/cpp/simulation/ADXL362SimTest.cpp

@@ -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.
+
+#include "frc/simulation/ADXL362Sim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+
+#include "frc/ADXL362.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+TEST(ADXL362SimTests, SetAttributes) {
+  HAL_Initialize(500, 0);
+
+  ADXL362 accel(SPI::kMXP, Accelerometer::kRange_2G);
+  ADXL362Sim sim(accel);
+
+  EXPECT_EQ(0, accel.GetX());
+  EXPECT_EQ(0, accel.GetY());
+  EXPECT_EQ(0, accel.GetZ());
+
+  sim.SetX(1.91);
+  sim.SetY(-3.405);
+  sim.SetZ(2.29);
+
+  EXPECT_EQ(1.91, accel.GetX());
+  EXPECT_EQ(-3.405, accel.GetY());
+  EXPECT_EQ(2.29, accel.GetZ());
+
+  ADXL362::AllAxes allAccel = accel.GetAccelerations();
+  EXPECT_EQ(1.91, allAccel.XAxis);
+  EXPECT_EQ(-3.405, allAccel.YAxis);
+  EXPECT_EQ(2.29, allAccel.ZAxis);
+}
+
+}  // namespace frc::sim

wpilibc/src/test/native/cpp/simulation/ADXRS450_GyroSimTest.cpp

@@ -0,0 +1,35 @@
+// 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.
+
+#include "frc/simulation/ADXRS450_GyroSim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+
+#include "frc/ADXRS450_Gyro.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+TEST(ADXRS450GyroSimTest, SetAttributes) {
+  HAL_Initialize(500, 0);
+
+  ADXRS450_Gyro gyro;
+  ADXRS450_GyroSim sim{gyro};
+
+  EXPECT_EQ(0, gyro.GetAngle());
+  EXPECT_EQ(0, gyro.GetRate());
+
+  constexpr units::degree_t TEST_ANGLE{123.456};
+  constexpr units::degrees_per_second_t TEST_RATE{229.3504};
+  sim.SetAngle(TEST_ANGLE);
+  sim.SetRate(TEST_RATE);
+
+  EXPECT_EQ(TEST_ANGLE.to<double>(), gyro.GetAngle());
+  EXPECT_EQ(TEST_RATE.to<double>(), gyro.GetRate());
+
+  gyro.Reset();
+  EXPECT_EQ(0, gyro.GetAngle());
+}
+
+}  // namespace frc::sim

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));
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/UltrasonicTest.java

@@ -0,0 +1,31 @@
+// 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;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.wpilibj.simulation.UltrasonicSim;
+import org.junit.jupiter.api.Test;
+
+public class UltrasonicTest {
+  @Test
+  public void testUltrasonic() {
+    try (Ultrasonic ultrasonic = new Ultrasonic(0, 1)) {
+      UltrasonicSim sim = new UltrasonicSim(ultrasonic);
+
+      assertEquals(0, ultrasonic.getRangeInches());
+      assertTrue(ultrasonic.isRangeValid());
+
+      sim.setRangeInches(35.04);
+      assertEquals(35.04, ultrasonic.getRangeInches());
+
+      sim.setRangeValid(false);
+      assertFalse(ultrasonic.isRangeValid());
+      assertEquals(0, ultrasonic.getRangeInches());
+    }
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXL345SimTest.java

@@ -0,0 +1,64 @@
+// 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 edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.ADXL345_I2C;
+import edu.wpi.first.wpilibj.ADXL345_SPI;
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.SPI;
+import edu.wpi.first.wpilibj.interfaces.Accelerometer;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.EnumSource;
+
+public class ADXL345SimTest {
+  @ParameterizedTest
+  @EnumSource(Accelerometer.Range.class)
+  void testInitI2C(Accelerometer.Range range) {
+    HAL.initialize(500, 0);
+
+    try (ADXL345_I2C accel = new ADXL345_I2C(I2C.Port.kMXP, range)) {
+      ADXL345Sim sim = new ADXL345Sim(accel);
+
+      sim.setX(1.91);
+      sim.setY(-3.405);
+      sim.setZ(2.29);
+
+      assertEquals(1.91, accel.getX());
+      assertEquals(-3.405, accel.getY());
+      assertEquals(2.29, accel.getZ());
+
+      ADXL345_I2C.AllAxes allAccel = accel.getAccelerations();
+      assertEquals(1.91, allAccel.XAxis);
+      assertEquals(-3.405, allAccel.YAxis);
+      assertEquals(2.29, allAccel.ZAxis);
+    }
+  }
+
+  @ParameterizedTest
+  @EnumSource(Accelerometer.Range.class)
+  void testInitSPi(Accelerometer.Range range) {
+    HAL.initialize(500, 0);
+
+    try (ADXL345_SPI accel = new ADXL345_SPI(SPI.Port.kMXP, range)) {
+      ADXL345Sim sim = new ADXL345Sim(accel);
+
+      sim.setX(1.91);
+      sim.setY(-3.405);
+      sim.setZ(2.29);
+
+      assertEquals(1.91, accel.getX());
+      assertEquals(-3.405, accel.getY());
+      assertEquals(2.29, accel.getZ());
+
+      ADXL345_SPI.AllAxes allAccel = accel.getAccelerations();
+      assertEquals(1.91, allAccel.XAxis);
+      assertEquals(-3.405, allAccel.YAxis);
+      assertEquals(2.29, allAccel.ZAxis);
+    }
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXL362SimTest.java

@@ -0,0 +1,42 @@
+// 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 edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.ADXL362;
+import edu.wpi.first.wpilibj.SPI;
+import edu.wpi.first.wpilibj.interfaces.Accelerometer;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.EnumSource;
+
+public class ADXL362SimTest {
+  @ParameterizedTest
+  @EnumSource(Accelerometer.Range.class)
+  void testAccel(Accelerometer.Range range) {
+    HAL.initialize(500, 0);
+
+    try (ADXL362 accel = new ADXL362(SPI.Port.kMXP, range)) {
+      assertEquals(0, accel.getX());
+      assertEquals(0, accel.getY());
+      assertEquals(0, accel.getZ());
+
+      ADXL362Sim sim = new ADXL362Sim(accel);
+      sim.setX(1.91);
+      sim.setY(-3.405);
+      sim.setZ(2.29);
+
+      assertEquals(1.91, accel.getX());
+      assertEquals(-3.405, accel.getY());
+      assertEquals(2.29, accel.getZ());
+
+      ADXL362.AllAxes allAccel = accel.getAccelerations();
+      assertEquals(1.91, allAccel.XAxis);
+      assertEquals(-3.405, allAccel.YAxis);
+      assertEquals(2.29, allAccel.ZAxis);
+    }
+  }
+}