[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