From e45a0f6ce2f68e579703f349f2592848357fcac5 Mon Sep 17 00:00:00 2001 From: Zhiquan Yeo Date: Sun, 3 Jan 2021 00:59:57 -0500 Subject: [PATCH] [examples] Add RomiGyro to the Romi Reference example (#3037) --- .../RomiReference/cpp/sensors/RomiGyro.cpp | 79 +++++++++++ .../cpp/subsystems/Drivetrain.cpp | 28 ++++ .../RomiReference/include/sensors/RomiGyro.h | 60 +++++++++ .../include/subsystems/Drivetrain.h | 41 ++++++ .../romireference/sensors/RomiGyro.java | 124 ++++++++++++++++++ .../romireference/subsystems/Drivetrain.java | 67 ++++++++++ 6 files changed, 399 insertions(+) create mode 100644 wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/sensors/RomiGyro.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/RomiReference/include/sensors/RomiGyro.h create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/sensors/RomiGyro.java diff --git a/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/sensors/RomiGyro.cpp b/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/sensors/RomiGyro.cpp new file mode 100644 index 0000000000..08a537bca3 --- /dev/null +++ b/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(); + } +} diff --git a/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp index b272006926..4f43eee18d 100644 --- a/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp +++ b/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(); +} diff --git a/wpilibcExamples/src/main/cpp/examples/RomiReference/include/sensors/RomiGyro.h b/wpilibcExamples/src/main/cpp/examples/RomiReference/include/sensors/RomiGyro.h new file mode 100644 index 0000000000..0e93d488ed --- /dev/null +++ b/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 + +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 c3edc7c7e3..7bedcdee4b 100644 --- a/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h @@ -4,12 +4,15 @@ #pragma once +#include #include #include #include #include #include +#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; }; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/sensors/RomiGyro.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/sensors/RomiGyro.java new file mode 100644 index 0000000000..c21f62317e --- /dev/null +++ b/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(); + } + } +} 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 46d7e6c778..40d2d2feb1 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 @@ -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