diff --git a/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/sensors/RomiGyro.cpp b/wpilibc/src/main/native/cpp/romi/RomiGyro.cpp similarity index 79% rename from wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/sensors/RomiGyro.cpp rename to wpilibc/src/main/native/cpp/romi/RomiGyro.cpp index 08a537bca3..50260f9d37 100644 --- a/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/sensors/RomiGyro.cpp +++ b/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; } diff --git a/wpilibc/src/main/native/include/frc/romi/RomiGyro.h b/wpilibc/src/main/native/include/frc/romi/RomiGyro.h new file mode 100644 index 0000000000..391e26b700 --- /dev/null +++ b/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 + +#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 diff --git a/wpilibcExamples/src/main/cpp/examples/RomiReference/include/sensors/RomiGyro.h b/wpilibcExamples/src/main/cpp/examples/RomiReference/include/sensors/RomiGyro.h deleted file mode 100644 index 0e93d488ed..0000000000 --- a/wpilibcExamples/src/main/cpp/examples/RomiReference/include/sensors/RomiGyro.h +++ /dev/null @@ -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 - -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; -}; diff --git a/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h index 7bedcdee4b..0f96ff4f4d 100644 --- a/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h @@ -8,11 +8,10 @@ #include #include #include +#include #include #include -#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; }; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/sensors/RomiGyro.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/romi/RomiGyro.java similarity index 69% rename from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/sensors/RomiGyro.java rename to wpilibj/src/main/java/edu/wpi/first/wpilibj/romi/RomiGyro.java index c21f62317e..ea61795c4e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/sensors/RomiGyro.java +++ b/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(); + } + } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java index 40d2d2feb1..eb3ca22a1e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java +++ b/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 {