Merge branch 'main' into 2022

wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/sensors/RomiGyro.cpp

@@ -0,0 +1,79 @@
+// 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 "sensors/RomiGyro.h"
+
+RomiGyro::RomiGyro() : m_simDevice("Gyro:RomiGyro") {
+  if (m_simDevice) {
+    m_simDevice.CreateBoolean("init", hal::SimDevice::kOutput, true);
+    m_simRateX =
+        m_simDevice.CreateDouble("rate_x", hal::SimDevice::kInput, 0.0);
+    m_simRateY =
+        m_simDevice.CreateDouble("rate_y", hal::SimDevice::kInput, 0.0);
+    m_simRateZ =
+        m_simDevice.CreateDouble("rate_z", hal::SimDevice::kInput, 0.0);
+    m_simAngleX =
+        m_simDevice.CreateDouble("angle_x", hal::SimDevice::kInput, 0.0);
+    m_simAngleY =
+        m_simDevice.CreateDouble("angle_y", hal::SimDevice::kInput, 0.0);
+    m_simAngleZ =
+        m_simDevice.CreateDouble("angle_z", hal::SimDevice::kInput, 0.0);
+  }
+}
+
+double RomiGyro::GetRateX() {
+  if (m_simRateX) {
+    return m_simRateX.Get();
+  }
+
+  return 0.0;
+}
+
+double RomiGyro::GetRateY() {
+  if (m_simRateY) {
+    return m_simRateY.Get();
+  }
+
+  return 0.0;
+}
+
+double RomiGyro::GetRateZ() {
+  if (m_simRateZ) {
+    return m_simRateZ.Get();
+  }
+
+  return 0.0;
+}
+
+double RomiGyro::GetAngleX() {
+  if (m_simAngleX) {
+    return m_simAngleX.Get() - m_angleXOffset;
+  }
+
+  return 0.0;
+}
+
+double RomiGyro::GetAngleY() {
+  if (m_simAngleY) {
+    return m_simAngleY.Get() - m_angleYOffset;
+  }
+
+  return 0.0;
+}
+
+double RomiGyro::GetAngleZ() {
+  if (m_simAngleZ) {
+    return m_simAngleZ.Get() - m_angleZOffset;
+  }
+
+  return 0.0;
+}
+
+void RomiGyro::Reset() {
+  if (m_simAngleX) {
+    m_angleXOffset = m_simAngleX.Get();
+    m_angleYOffset = m_simAngleY.Get();
+    m_angleZOffset = m_simAngleZ.Get();
+  }
+}

wpilibcExamples/src/main/cpp/examples/RomiReference/include/sensors/RomiGyro.h

@@ -0,0 +1,60 @@
+// 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>
+
+class RomiGyro {
+ public:
+  RomiGyro();
+
+  /**
+   * Gets the rate of turn in degrees-per-second around the X-axis
+   */
+  double GetRateX();
+
+  /**
+   * Gets the rate of turn in degrees-per-second around the Y-axis
+   */
+  double GetRateY();
+
+  /**
+   * Gets the rate of turn in degrees-per-second around the Z-axis
+   */
+  double GetRateZ();
+
+  /**
+   * Gets the currently reported angle around the X-axis
+   */
+  double GetAngleX();
+
+  /**
+   * Gets the currently reported angle around the X-axis
+   */
+  double GetAngleY();
+
+  /**
+   * Gets the currently reported angle around the X-axis
+   */
+  double GetAngleZ();
+
+  /**
+   * Resets the gyro
+   */
+  void Reset();
+
+ private:
+  hal::SimDevice m_simDevice;
+  hal::SimDouble m_simRateX;
+  hal::SimDouble m_simRateY;
+  hal::SimDouble m_simRateZ;
+  hal::SimDouble m_simAngleX;
+  hal::SimDouble m_simAngleY;
+  hal::SimDouble m_simAngleZ;
+
+  double m_angleXOffset = 0;
+  double m_angleYOffset = 0;
+  double m_angleZOffset = 0;
+};

wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h

@@ -8,10 +8,11 @@
 #include <frc/Encoder.h>
 #include <frc/drive/DifferentialDrive.h>
 #include <frc/motorcontrol/Spark.h>
-#include <frc/romi/RomiGyro.h>
 #include <frc2/command/SubsystemBase.h>
 #include <units/length.h>
 
+#include "sensors/RomiGyro.h"
+
 class Drivetrain : public frc2::SubsystemBase {
  public:
   static constexpr double kCountsPerRevolution = 1440.0;
@@ -116,6 +117,6 @@ class Drivetrain : public frc2::SubsystemBase {
 
   frc::DifferentialDrive m_drive{m_leftMotor, m_rightMotor};
 
-  frc::RomiGyro m_gyro;
+  RomiGyro m_gyro;
   frc::BuiltInAccelerometer m_accelerometer;
 };