[examples] Add RomiGyro to the Romi Reference example (#3037)

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/cpp/subsystems/Drivetrain.cpp

@@ -54,3 +54,31 @@ units::meter_t Drivetrain::GetRightDistance() {
 units::meter_t Drivetrain::GetAverageDistance() {
   return (GetLeftDistance() + GetRightDistance()) / 2.0;
 }
+
+double Drivetrain::GetAccelX() {
+  return m_accelerometer.GetX();
+}
+
+double Drivetrain::GetAccelY() {
+  return m_accelerometer.GetY();
+}
+
+double Drivetrain::GetAccelZ() {
+  return m_accelerometer.GetZ();
+}
+
+double Drivetrain::GetGyroAngleX() {
+  return m_gyro.GetAngleX();
+}
+
+double Drivetrain::GetGyroAngleY() {
+  return m_gyro.GetAngleY();
+}
+
+double Drivetrain::GetGyroAngleZ() {
+  return m_gyro.GetAngleZ();
+}
+
+void Drivetrain::ResetGyro() {
+  m_gyro.Reset();
+}

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

@@ -4,12 +4,15 @@
 
 #pragma once
 
+#include <frc/BuiltInAccelerometer.h>
 #include <frc/Encoder.h>
 #include <frc/Spark.h>
 #include <frc/drive/DifferentialDrive.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;
@@ -70,6 +73,41 @@ class Drivetrain : public frc2::SubsystemBase {
    */
   units::meter_t GetAverageDistance();
 
+  /**
+   * Returns the acceleration along the X-axis, in Gs.
+   */
+  double GetAccelX();
+
+  /**
+   * Returns the acceleration along the Y-axis, in Gs.
+   */
+  double GetAccelY();
+
+  /**
+   * Returns the acceleration along the Z-axis, in Gs.
+   */
+  double GetAccelZ();
+
+  /**
+   * Returns the current angle of the Romi around the X-axis, in degrees.
+   */
+  double GetGyroAngleX();
+
+  /**
+   * Returns the current angle of the Romi around the Y-axis, in degrees.
+   */
+  double GetGyroAngleY();
+
+  /**
+   * Returns the current angle of the Romi around the Z-axis, in degrees.
+   */
+  double GetGyroAngleZ();
+
+  /**
+   * Reset the gyro.
+   */
+  void ResetGyro();
+
  private:
   frc::Spark m_leftMotor{0};
   frc::Spark m_rightMotor{1};
@@ -78,4 +116,7 @@ class Drivetrain : public frc2::SubsystemBase {
   frc::Encoder m_rightEncoder{6, 7};
 
   frc::DifferentialDrive m_drive{m_leftMotor, m_rightMotor};
+
+  RomiGyro m_gyro;
+  frc::BuiltInAccelerometer m_accelerometer;
 };

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/sensors/RomiGyro.java

@@ -0,0 +1,124 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.romireference.sensors;
+
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDevice.Direction;
+import edu.wpi.first.hal.SimDouble;
+
+public class RomiGyro {
+  private SimDouble m_simRateX;
+  private SimDouble m_simRateY;
+  private SimDouble m_simRateZ;
+  private SimDouble m_simAngleX;
+  private SimDouble m_simAngleY;
+  private SimDouble m_simAngleZ;
+
+  private double m_angleXOffset;
+  private double m_angleYOffset;
+  private double m_angleZOffset;
+
+  /** 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_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);
+    }
+  }
+
+  /**
+   * Get the rate of turn in degrees-per-second around the X-axis.
+   *
+   * @return rate of turn in degrees-per-second
+   */
+  public double getRateX() {
+    if (m_simRateX != null) {
+      return m_simRateX.get();
+    }
+
+    return 0.0;
+  }
+
+  /**
+   * Get the rate of turn in degrees-per-second around the Y-axis.
+   *
+   * @return rate of turn in degrees-per-second
+   */
+  public double getRateY() {
+    if (m_simRateY != null) {
+      return m_simRateY.get();
+    }
+
+    return 0.0;
+  }
+
+  /**
+   * Get the rate of turn in degrees-per-second around the Z-axis.
+   *
+   * @return rate of turn in degrees-per-second
+   */
+  public double getRateZ() {
+    if (m_simRateZ != null) {
+      return m_simRateZ.get();
+    }
+
+    return 0.0;
+  }
+
+  /**
+   * Get the currently reported angle around the X-axis.
+   *
+   * @return current angle around X-axis in degrees
+   */
+  public double getAngleX() {
+    if (m_simAngleX != null) {
+      return m_simAngleX.get() - m_angleXOffset;
+    }
+
+    return 0.0;
+  }
+
+  /**
+   * Get the currently reported angle around the X-axis.
+   *
+   * @return current angle around Y-axis in degrees
+   */
+  public double getAngleY() {
+    if (m_simAngleY != null) {
+      return m_simAngleY.get() - m_angleYOffset;
+    }
+
+    return 0.0;
+  }
+
+  /**
+   * Get the currently reported angle around the Z-axis.
+   *
+   * @return current angle around Z-axis in degrees
+   */
+  public double getAngleZ() {
+    if (m_simAngleZ != null) {
+      return m_simAngleZ.get() - m_angleZOffset;
+    }
+
+    return 0.0;
+  }
+
+  /** Reset the gyro angles to 0. */
+  public void reset() {
+    if (m_simAngleX != null) {
+      m_angleXOffset = m_simAngleX.get();
+      m_angleYOffset = m_simAngleY.get();
+      m_angleZOffset = m_simAngleZ.get();
+    }
+  }
+}

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

@@ -4,9 +4,11 @@
 
 package edu.wpi.first.wpilibj.examples.romireference.subsystems;
 
+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.wpilibj2.command.SubsystemBase;
 
 public class Drivetrain extends SubsystemBase {
@@ -26,6 +28,12 @@ public class Drivetrain extends SubsystemBase {
   // Set up the differential drive controller
   private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
 
+  // Set up the RomiGyro
+  private final RomiGyro m_gyro = new RomiGyro();
+
+  // Set up the BuiltInAccelerometer
+  private final BuiltInAccelerometer m_accelerometer = new BuiltInAccelerometer();
+
   /** Creates a new Drivetrain. */
   public Drivetrain() {
     // Use inches as unit for encoder distances
@@ -63,6 +71,65 @@ public class Drivetrain extends SubsystemBase {
     return (getLeftDistanceInch() + getRightDistanceInch()) / 2.0;
   }
 
+  /**
+   * The acceleration in the X-axis.
+   *
+   * @return The acceleration of the Romi along the X-axis in Gs
+   */
+  public double getAccelX() {
+    return m_accelerometer.getX();
+  }
+
+  /**
+   * The acceleration in the Y-axis.
+   *
+   * @return The acceleration of the Romi along the Y-axis in Gs
+   */
+  public double getAccelY() {
+    return m_accelerometer.getY();
+  }
+
+  /**
+   * The acceleration in the Z-axis.
+   *
+   * @return The acceleration of the Romi along the Z-axis in Gs
+   */
+  public double getAccelZ() {
+    return m_accelerometer.getZ();
+  }
+
+  /**
+   * Current angle of the Romi around the X-axis.
+   *
+   * @return The current angle of the Romi in degrees
+   */
+  public double getGyroAngleX() {
+    return m_gyro.getAngleX();
+  }
+
+  /**
+   * Current angle of the Romi around the Y-axis.
+   *
+   * @return The current angle of the Romi in degrees
+   */
+  public double getGyroAngleY() {
+    return m_gyro.getAngleY();
+  }
+
+  /**
+   * Current angle of the Romi around the Z-axis.
+   *
+   * @return The current angle of the Romi in degrees
+   */
+  public double getGyroAngleZ() {
+    return m_gyro.getAngleZ();
+  }
+
+  /** Reset the gyro. */
+  public void resetGyro() {
+    m_gyro.reset();
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run