diff --git a/romiVendordep/src/main/native/cpp/romi/RomiGyro.cpp b/romiVendordep/src/main/native/cpp/romi/RomiGyro.cpp index c44cff9d822..4c67f77abe9 100644 --- a/romiVendordep/src/main/native/cpp/romi/RomiGyro.cpp +++ b/romiVendordep/src/main/native/cpp/romi/RomiGyro.cpp @@ -24,60 +24,60 @@ RomiGyro::RomiGyro() : m_simDevice("Gyro:RomiGyro") { } } -double RomiGyro::GetAngle() const { +units::radian_t RomiGyro::GetAngle() const { return GetAngleZ(); } -double RomiGyro::GetRate() const { +units::radians_per_second_t RomiGyro::GetRate() const { return GetRateZ(); } -double RomiGyro::GetRateX() const { +units::radians_per_second_t RomiGyro::GetRateX() const { if (m_simRateX) { - return m_simRateX.Get(); + return units::degrees_per_second_t{m_simRateX.Get()}; } - return 0.0; + return 0.0_rad_per_s; } -double RomiGyro::GetRateY() const { +units::radians_per_second_t RomiGyro::GetRateY() const { if (m_simRateY) { - return m_simRateY.Get(); + return units::degrees_per_second_t{m_simRateY.Get()}; } - return 0.0; + return 0.0_rad_per_s; } -double RomiGyro::GetRateZ() const { +units::radians_per_second_t RomiGyro::GetRateZ() const { if (m_simRateZ) { - return m_simRateZ.Get(); + return units::degrees_per_second_t{m_simRateZ.Get()}; } - return 0.0; + return 0.0_rad_per_s; } -double RomiGyro::GetAngleX() const { +units::radian_t RomiGyro::GetAngleX() const { if (m_simAngleX) { - return m_simAngleX.Get() - m_angleXOffset; + return units::degree_t{m_simAngleX.Get() - m_angleXOffset}; } - return 0.0; + return 0.0_rad; } -double RomiGyro::GetAngleY() const { +units::radian_t RomiGyro::GetAngleY() const { if (m_simAngleY) { - return m_simAngleY.Get() - m_angleYOffset; + return units::degree_t{m_simAngleY.Get() - m_angleYOffset}; } - return 0.0; + return 0.0_rad; } -double RomiGyro::GetAngleZ() const { +units::radian_t RomiGyro::GetAngleZ() const { if (m_simAngleZ) { - return m_simAngleZ.Get() - m_angleZOffset; + return units::degree_t{m_simAngleZ.Get() - m_angleZOffset}; } - return 0.0; + return 0.0_rad; } void RomiGyro::Reset() { diff --git a/romiVendordep/src/main/native/include/frc/romi/RomiGyro.h b/romiVendordep/src/main/native/include/frc/romi/RomiGyro.h index 5ef50456608..b64ee60ec4b 100644 --- a/romiVendordep/src/main/native/include/frc/romi/RomiGyro.h +++ b/romiVendordep/src/main/native/include/frc/romi/RomiGyro.h @@ -5,6 +5,8 @@ #pragma once #include +#include +#include namespace frc { @@ -24,55 +26,67 @@ class RomiGyro { RomiGyro(); /** - * Return the actual angle in degrees that the robot is currently facing. + * Return the actual angle in radians 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. + * The angle is continuous, that is, it will continue from 2π->3π radians. * 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. + * gyro output as it sweeps from 2π to 0 on the second time around. * - * @return the current heading of the robot in degrees. + * @return The current heading of the robot. */ - double GetAngle() const; + units::radian_t GetAngle() const; /** * 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 + * @return The current rate. */ - double GetRate() const; + units::radians_per_second_t GetRate() const; /** - * Gets the rate of turn in degrees-per-second around the X-axis + * Get the rate of turn in around the X-axis. + * + * @return Rate of turn. */ - double GetRateX() const; + units::radians_per_second_t GetRateX() const; /** - * Gets the rate of turn in degrees-per-second around the Y-axis + * Get the rate of turn in around the Y-axis. + * + * @return Rate of turn. */ - double GetRateY() const; + units::radians_per_second_t GetRateY() const; /** - * Gets the rate of turn in degrees-per-second around the Z-axis + * Get the rate of turn around the Z-axis. + * + * @return Rate of turn. */ - double GetRateZ() const; + units::radians_per_second_t GetRateZ() const; /** - * Gets the currently reported angle around the X-axis + * Get the currently reported angle around the X-axis. + * + * @return Current angle around X-axis. */ - double GetAngleX() const; + units::radian_t GetAngleX() const; /** - * Gets the currently reported angle around the X-axis + * Get the currently reported angle around the Y-axis. + * + * @return Current angle around Y-axis. */ - double GetAngleY() const; + units::radian_t GetAngleY() const; /** - * Gets the currently reported angle around the X-axis + * Get the currently reported angle around the Z-axis. + * + * @return Current angle around Z-axis. */ - double GetAngleZ() const; + units::radian_t GetAngleZ() const; /** * Resets the gyro 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 1d212d5c5d8..7afc2d77df1 100644 --- a/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp @@ -63,27 +63,27 @@ units::meter_t Drivetrain::GetAverageDistance() { return (GetLeftDistance() + GetRightDistance()) / 2.0; } -double Drivetrain::GetAccelX() { - return m_accelerometer.GetX(); +units::meters_per_second_squared_t Drivetrain::GetAccelX() { + return units::meters_per_second_squared_t{m_accelerometer.GetX()}; } -double Drivetrain::GetAccelY() { - return m_accelerometer.GetY(); +units::meters_per_second_squared_t Drivetrain::GetAccelY() { + return units::meters_per_second_squared_t{m_accelerometer.GetY()}; } -double Drivetrain::GetAccelZ() { - return m_accelerometer.GetZ(); +units::meters_per_second_squared_t Drivetrain::GetAccelZ() { + return units::meters_per_second_squared_t{m_accelerometer.GetZ()}; } -double Drivetrain::GetGyroAngleX() { +units::radian_t Drivetrain::GetGyroAngleX() { return m_gyro.GetAngleX(); } -double Drivetrain::GetGyroAngleY() { +units::radian_t Drivetrain::GetGyroAngleY() { return m_gyro.GetAngleY(); } -double Drivetrain::GetGyroAngleZ() { +units::radian_t Drivetrain::GetGyroAngleZ() { return m_gyro.GetAngleZ(); } 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 d679178fd04..3b55c9e8347 100644 --- a/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h @@ -10,6 +10,8 @@ #include #include #include +#include +#include #include class Drivetrain : public frc2::SubsystemBase { @@ -73,34 +75,46 @@ class Drivetrain : public frc2::SubsystemBase { units::meter_t GetAverageDistance(); /** - * Returns the acceleration along the X-axis, in Gs. + * The acceleration in the X-axis. + * + * @return The acceleration of the Romi along the X-axis. */ - double GetAccelX(); + units::meters_per_second_squared_t GetAccelX(); /** - * Returns the acceleration along the Y-axis, in Gs. + * The acceleration in the Y-axis. + * + * @return The acceleration of the Romi along the Y-axis. */ - double GetAccelY(); + units::meters_per_second_squared_t GetAccelY(); /** - * Returns the acceleration along the Z-axis, in Gs. + * The acceleration in the Z-axis. + * + * @return The acceleration of the Romi along the Z-axis. */ - double GetAccelZ(); + units::meters_per_second_squared_t GetAccelZ(); /** - * Returns the current angle of the Romi around the X-axis, in degrees. + * Current angle of the Romi around the X-axis. + * + * @return The current angle of the Romi. */ - double GetGyroAngleX(); + units::radian_t GetGyroAngleX(); /** - * Returns the current angle of the Romi around the Y-axis, in degrees. + * Current angle of the Romi around the Y-axis. + * + * @return The current angle of the Romi. */ - double GetGyroAngleY(); + units::radian_t GetGyroAngleY(); /** - * Returns the current angle of the Romi around the Z-axis, in degrees. + * Current angle of the Romi around the Z-axis. + * + * @return The current angle of the Romi. */ - double GetGyroAngleZ(); + units::radian_t GetGyroAngleZ(); /** * Reset the gyro.