[wpilib] Add ADXRS450_GyroSim (#2800)

Closes #2760.

wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -32,7 +32,7 @@ static constexpr int kPIDRegister = 0x0C;
 ADXRS450_Gyro::ADXRS450_Gyro() : ADXRS450_Gyro(SPI::kOnboardCS0) {}
 
 ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port)
-    : m_spi(port), m_simDevice("ADXRS450_Gyro", port) {
+    : m_spi(port), m_port(port), m_simDevice("ADXRS450_Gyro", port) {
   if (m_simDevice) {
     m_simAngle = m_simDevice.CreateDouble("Angle", false, 0.0);
     m_simRate = m_simDevice.CreateDouble("Rate", false, 0.0);
@@ -123,3 +123,5 @@ void ADXRS450_Gyro::Calibrate() {
   m_spi.SetAccumulatorIntegratedCenter(m_spi.GetAccumulatorIntegratedAverage());
   m_spi.ResetAccumulator();
 }
+
+int ADXRS450_Gyro::GetPort() const { return m_port; }

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

@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/simulation/ADXRS450_GyroSim.h"
+
+#include <wpi/SmallString.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/ADXRS450_Gyro.h"
+#include "frc/simulation/SimDeviceSim.h"
+
+using namespace frc::sim;
+
+ADXRS450_GyroSim::ADXRS450_GyroSim(const frc::ADXRS450_Gyro& gyro) {
+  wpi::SmallString<128> fullname;
+  wpi::raw_svector_ostream os(fullname);
+  os << "ADXRS450_Gyro" << '[' << gyro.GetPort() << ']';
+  frc::sim::SimDeviceSim deviceSim{fullname.c_str()};
+  m_simAngle = deviceSim.GetDouble("Angle");
+  m_simRate = deviceSim.GetDouble("Rate");
+}
+
+void ADXRS450_GyroSim::SetAngle(units::degree_t angle) {
+  m_simAngle.Set(angle.to<double>());
+}
+
+void ADXRS450_GyroSim::SetRate(units::degrees_per_second_t rate) {
+  m_simRate.Set(rate.to<double>());
+}

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

@@ -92,8 +92,16 @@ class ADXRS450_Gyro : public GyroBase {
    */
   void Calibrate() override;
 
+  /**
+   * Get the SPI port number.
+   *
+   * @return The SPI port number.
+   */
+  int GetPort() const;
+
  private:
   SPI m_spi;
+  SPI::Port m_port;
 
   hal::SimDevice m_simDevice;
   hal::SimDouble m_simAngle;

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

@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <hal/SimDevice.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+
+#include "frc/geometry/Rotation2d.h"
+
+namespace frc {
+
+class ADXRS450_Gyro;
+
+namespace sim {
+
+/**
+ * Class to control a simulated ADXRS450 gyroscope.
+ */
+class ADXRS450_GyroSim {
+ public:
+  /**
+   * Constructs from a ADXRS450_Gyro object.
+   *
+   * @param gyro ADXRS450_Gyro to simulate
+   */
+  explicit ADXRS450_GyroSim(const ADXRS450_Gyro& gyro);
+
+  /**
+   * Sets the angle.
+   *
+   * @param angle The angle (clockwise positive).
+   */
+  void SetAngle(units::degree_t angle);
+
+  /**
+   * Sets the angular rate (clockwise positive).
+   *
+   * @param rate The angular rate.
+   */
+  void SetRate(units::degrees_per_second_t rate);
+
+ private:
+  hal::SimDouble m_simAngle;
+  hal::SimDouble m_simRate;
+};
+
+}  // namespace sim
+}  // namespace frc

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

@@ -45,6 +45,7 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable
   private static final int kSNLowRegister = 0x10;
 
   private SPI m_spi;
+  private SPI.Port m_port;
 
   private SimDevice m_simDevice;
   private SimBoolean m_simConnected;
@@ -65,6 +66,7 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable
    */
   public ADXRS450_Gyro(SPI.Port port) {
     m_spi = new SPI(port);
+    m_port = port;
 
     // simulation
     m_simDevice = SimDevice.create("ADXRS450_Gyro", port.value);
@@ -129,6 +131,15 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable
     m_spi.resetAccumulator();
   }
 
+  /**
+   * Get the SPI port number.
+   *
+   * @return The SPI port number.
+   */
+  public int getPort() {
+    return m_port.value;
+  }
+
   private boolean calcParity(int value) {
     boolean parity = false;
     while (value != 0) {

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

@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
+
+/**
+ * Class to control a simulated ADXRS450 gyroscope.
+ */
+@SuppressWarnings("TypeName")
+public class ADXRS450_GyroSim {
+  private final SimDouble m_simAngle;
+  private final SimDouble m_simRate;
+
+  /**
+   * Constructs from an ADXRS450_Gyro object.
+   *
+   * @param gyro ADXRS450_Gyro to simulate
+   */
+  public ADXRS450_GyroSim(ADXRS450_Gyro gyro) {
+    SimDeviceSim wrappedSimDevice = new SimDeviceSim("ADXRS450_Gyro" + "[" + gyro.getPort() + "]");
+    m_simAngle = wrappedSimDevice.getDouble("Angle");
+    m_simRate = wrappedSimDevice.getDouble("Rate");
+  }
+
+  /**
+   * Sets the angle in degrees (clockwise positive).
+   *
+   * @param angleDegrees The angle.
+   */
+  public void setAngle(double angleDegrees) {
+    m_simAngle.set(angleDegrees);
+  }
+
+  /**
+   * Sets the angular rate in degrees per second (clockwise positive).
+   *
+   * @param rateDegreesPerSecond The angular rate.
+   */
+  public void setRate(double rateDegreesPerSecond) {
+    m_simRate.set(rateDegreesPerSecond);
+  }
+}