[wpilib] Move RomiGyro into main wpilibc/j (#3143)

wpilibc/src/main/native/cpp/romi/RomiGyro.cpp

@@ -2,7 +2,9 @@
 // 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"
+#include "frc/romi/RomiGyro.h"
+
+using namespace frc;
 
 RomiGyro::RomiGyro() : m_simDevice("Gyro:RomiGyro") {
   if (m_simDevice) {
@@ -22,7 +24,15 @@ RomiGyro::RomiGyro() : m_simDevice("Gyro:RomiGyro") {
   }
 }
 
-double RomiGyro::GetRateX() {
+double RomiGyro::GetAngle() const {
+  return GetAngleZ();
+}
+
+double RomiGyro::GetRate() const {
+  return GetRateZ();
+}
+
+double RomiGyro::GetRateX() const {
   if (m_simRateX) {
     return m_simRateX.Get();
   }
@@ -30,7 +40,7 @@ double RomiGyro::GetRateX() {
   return 0.0;
 }
 
-double RomiGyro::GetRateY() {
+double RomiGyro::GetRateY() const {
   if (m_simRateY) {
     return m_simRateY.Get();
   }
@@ -38,7 +48,7 @@ double RomiGyro::GetRateY() {
   return 0.0;
 }
 
-double RomiGyro::GetRateZ() {
+double RomiGyro::GetRateZ() const {
   if (m_simRateZ) {
     return m_simRateZ.Get();
   }
@@ -46,7 +56,7 @@ double RomiGyro::GetRateZ() {
   return 0.0;
 }
 
-double RomiGyro::GetAngleX() {
+double RomiGyro::GetAngleX() const {
   if (m_simAngleX) {
     return m_simAngleX.Get() - m_angleXOffset;
   }
@@ -54,7 +64,7 @@ double RomiGyro::GetAngleX() {
   return 0.0;
 }
 
-double RomiGyro::GetAngleY() {
+double RomiGyro::GetAngleY() const {
   if (m_simAngleY) {
     return m_simAngleY.Get() - m_angleYOffset;
   }
@@ -62,7 +72,7 @@ double RomiGyro::GetAngleY() {
   return 0.0;
 }
 
-double RomiGyro::GetAngleZ() {
+double RomiGyro::GetAngleZ() const {
   if (m_simAngleZ) {
     return m_simAngleZ.Get() - m_angleZOffset;
   }

wpilibc/src/main/native/include/frc/romi/RomiGyro.h

@@ -0,0 +1,101 @@
+// 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 "frc/GyroBase.h"
+
+namespace frc {
+
+/**
+ * Use a rate gyro to return the robots heading relative to a starting position.
+ *
+ * This class is for the Romi onboard gyro, and will only work in
+ * simulation/Romi mode. Only one instance of a RomiGyro is supported.
+ */
+class RomiGyro : public GyroBase {
+ public:
+  RomiGyro();
+
+  /**
+   * Return the actual angle in degrees that the robot is currently facing.
+   *
+   * The angle is based on integration of the returned rate form the gyro.
+   * The angle is continuous, that is, it will continue from 360->361 degrees.
+   * This allows algorithms that wouldn't want to see a discontinuity in the
+   * gyro output as it sweeps from 360 to 0 on the second time around.
+   *
+   * @return the current heading of the robot in degrees.
+   */
+  double GetAngle() const override;
+
+  /**
+   * Return the rate of rotation of the gyro
+   *
+   * The rate is based on the most recent reading of the gyro.
+   *
+   * @return the current rate in degrees per second
+   */
+  double GetRate() const override;
+
+  /**
+   * Initialize the gyro.
+   *
+   * NOTE: This function is a no-op. The Romi gyro should be calibrated via
+   * the web UI.
+   */
+  void Calibrate() final {}
+
+  /**
+   * Gets the rate of turn in degrees-per-second around the X-axis
+   */
+  double GetRateX() const;
+
+  /**
+   * Gets the rate of turn in degrees-per-second around the Y-axis
+   */
+  double GetRateY() const;
+
+  /**
+   * Gets the rate of turn in degrees-per-second around the Z-axis
+   */
+  double GetRateZ() const;
+
+  /**
+   * Gets the currently reported angle around the X-axis
+   */
+  double GetAngleX() const;
+
+  /**
+   * Gets the currently reported angle around the X-axis
+   */
+  double GetAngleY() const;
+
+  /**
+   * Gets the currently reported angle around the X-axis
+   */
+  double GetAngleZ() const;
+
+  /**
+   * Resets the gyro
+   */
+  void Reset() override;
+
+ 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;
+};
+
+}  // namespace frc

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

@@ -1,60 +0,0 @@
-// 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,11 +8,10 @@
 #include <frc/Encoder.h>
 #include <frc/Spark.h>
 #include <frc/drive/DifferentialDrive.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;
@@ -117,6 +116,6 @@ class Drivetrain : public frc2::SubsystemBase {
 
   frc::DifferentialDrive m_drive{m_leftMotor, m_rightMotor};
 
-  RomiGyro m_gyro;
+  frc::RomiGyro m_gyro;
   frc::BuiltInAccelerometer m_accelerometer;
 };

wpilibj/src/main/java/edu/wpi/first/wpilibj/romi/RomiGyro.java

@@ -2,13 +2,15 @@
 // 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.examples.romireference.sensors;
+package edu.wpi.first.wpilibj.romi;
 
 import edu.wpi.first.hal.SimDevice;
 import edu.wpi.first.hal.SimDevice.Direction;
 import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.GyroBase;
 
-public class RomiGyro {
+public class RomiGyro extends GyroBase {
+  private final SimDevice m_simDevice;
   private SimDouble m_simRateX;
   private SimDouble m_simRateY;
   private SimDouble m_simRateZ;
@@ -22,16 +24,16 @@ public class RomiGyro {
 
   /** Create a new RomiGyro. */
   public RomiGyro() {
-    SimDevice gyroSimDevice = SimDevice.create("Gyro:RomiGyro");
-    if (gyroSimDevice != null) {
-      gyroSimDevice.createBoolean("init", Direction.kOutput, true);
-      m_simRateX = gyroSimDevice.createDouble("rate_x", Direction.kInput, 0.0);
-      m_simRateY = gyroSimDevice.createDouble("rate_y", Direction.kInput, 0.0);
-      m_simRateZ = gyroSimDevice.createDouble("rate_z", Direction.kInput, 0.0);
+    m_simDevice = SimDevice.create("Gyro:RomiGyro");
+    if (m_simDevice != null) {
+      m_simDevice.createBoolean("init", Direction.kOutput, true);
+      m_simRateX = m_simDevice.createDouble("rate_x", Direction.kInput, 0.0);
+      m_simRateY = m_simDevice.createDouble("rate_y", Direction.kInput, 0.0);
+      m_simRateZ = m_simDevice.createDouble("rate_z", Direction.kInput, 0.0);
 
-      m_simAngleX = gyroSimDevice.createDouble("angle_x", Direction.kInput, 0.0);
-      m_simAngleY = gyroSimDevice.createDouble("angle_y", Direction.kInput, 0.0);
-      m_simAngleZ = gyroSimDevice.createDouble("angle_z", Direction.kInput, 0.0);
+      m_simAngleX = m_simDevice.createDouble("angle_x", Direction.kInput, 0.0);
+      m_simAngleY = m_simDevice.createDouble("angle_y", Direction.kInput, 0.0);
+      m_simAngleZ = m_simDevice.createDouble("angle_z", Direction.kInput, 0.0);
     }
   }
 
@@ -114,6 +116,7 @@ public class RomiGyro {
   }
 
   /** Reset the gyro angles to 0. */
+  @Override
   public void reset() {
     if (m_simAngleX != null) {
       m_angleXOffset = m_simAngleX.get();
@@ -121,4 +124,26 @@ public class RomiGyro {
       m_angleZOffset = m_simAngleZ.get();
     }
   }
+
+  @Override
+  public void calibrate() {
+    // no-op - Romi Gyro calibration is done via web UI
+  }
+
+  @Override
+  public double getAngle() {
+    return getAngleZ();
+  }
+
+  @Override
+  public double getRate() {
+    return getRateZ();
+  }
+
+  @Override
+  public void close() throws Exception {
+    if (m_simDevice != null) {
+      m_simDevice.close();
+    }
+  }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java

@@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj.BuiltInAccelerometer;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Spark;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.examples.romireference.sensors.RomiGyro;
+import edu.wpi.first.wpilibj.romi.RomiGyro;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Drivetrain extends SubsystemBase {