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