diff --git a/wpilibcExamples/src/main/cpp/examples/XRPReference/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/XRPReference/cpp/RobotContainer.cpp index 2b6cfa242b9..fba484b07fc 100644 --- a/wpilibcExamples/src/main/cpp/examples/XRPReference/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/XRPReference/cpp/RobotContainer.cpp @@ -26,12 +26,12 @@ void RobotContainer::ConfigureButtonBindings() { .OnFalse(frc2::cmd::Print("USER Button Released")); frc2::JoystickButton(&m_controller, 1) - .OnTrue(frc2::cmd::RunOnce([this] { m_arm.SetAngle(45.0); }, {})) - .OnFalse(frc2::cmd::RunOnce([this] { m_arm.SetAngle(0.0); }, {})); + .OnTrue(frc2::cmd::RunOnce([this] { m_arm.SetAngle(45_deg); }, {})) + .OnFalse(frc2::cmd::RunOnce([this] { m_arm.SetAngle(0_deg); }, {})); frc2::JoystickButton(&m_controller, 2) - .OnTrue(frc2::cmd::RunOnce([this] { m_arm.SetAngle(90.0); }, {})) - .OnFalse(frc2::cmd::RunOnce([this] { m_arm.SetAngle(0.0); }, {})); + .OnTrue(frc2::cmd::RunOnce([this] { m_arm.SetAngle(90_deg); }, {})) + .OnFalse(frc2::cmd::RunOnce([this] { m_arm.SetAngle(0_deg); }, {})); // Setup SmartDashboard options. m_chooser.SetDefaultOption("Auto Routine Distance", &m_autoDistance); diff --git a/wpilibcExamples/src/main/cpp/examples/XRPReference/cpp/subsystems/Arm.cpp b/wpilibcExamples/src/main/cpp/examples/XRPReference/cpp/subsystems/Arm.cpp index cf75e5210db..ea70e10a8c4 100644 --- a/wpilibcExamples/src/main/cpp/examples/XRPReference/cpp/subsystems/Arm.cpp +++ b/wpilibcExamples/src/main/cpp/examples/XRPReference/cpp/subsystems/Arm.cpp @@ -8,6 +8,6 @@ void Arm::Periodic() { // This method will be called once per scheduler run. } -void Arm::SetAngle(double angleDeg) { - m_armServo.SetAngle(angleDeg); +void Arm::SetAngle(units::radian_t angle) { + m_armServo.SetAngle(angle); } diff --git a/wpilibcExamples/src/main/cpp/examples/XRPReference/cpp/subsystems/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/XRPReference/cpp/subsystems/Drivetrain.cpp index 25c520cef25..1a46f59b883 100644 --- a/wpilibcExamples/src/main/cpp/examples/XRPReference/cpp/subsystems/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/XRPReference/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/XRPReference/include/subsystems/Arm.h b/wpilibcExamples/src/main/cpp/examples/XRPReference/include/subsystems/Arm.h index aacb7c6f9f5..9081a5c55b7 100644 --- a/wpilibcExamples/src/main/cpp/examples/XRPReference/include/subsystems/Arm.h +++ b/wpilibcExamples/src/main/cpp/examples/XRPReference/include/subsystems/Arm.h @@ -6,6 +6,7 @@ #include #include +#include class Arm : public frc2::SubsystemBase { public: @@ -15,11 +16,11 @@ class Arm : public frc2::SubsystemBase { void Periodic() override; /** - * Set the current angle of the arm (0 - 180 degrees). + * Set the current angle of the arm (0° - 180°). * - * @param angleDeg the commanded angle + * @param angle the commanded angle */ - void SetAngle(double angleDeg); + void SetAngle(units::radian_t angle); private: frc::XRPServo m_armServo{4}; diff --git a/wpilibcExamples/src/main/cpp/examples/XRPReference/include/subsystems/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/XRPReference/include/subsystems/Drivetrain.h index c2a2ef319f1..050988e8821 100644 --- a/wpilibcExamples/src/main/cpp/examples/XRPReference/include/subsystems/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/XRPReference/include/subsystems/Drivetrain.h @@ -10,6 +10,8 @@ #include #include #include +#include +#include #include class Drivetrain : public frc2::SubsystemBase { @@ -77,34 +79,34 @@ class Drivetrain : public frc2::SubsystemBase { units::meter_t GetAverageDistance(); /** - * Returns the acceleration along the X-axis, in Gs. + * Returns the acceleration along the X-axis, in m/s². */ - double GetAccelX(); + units::meters_per_second_squared_t GetAccelX(); /** - * Returns the acceleration along the Y-axis, in Gs. + * Returns the acceleration along the Y-axis, in m/s². */ - double GetAccelY(); + units::meters_per_second_squared_t GetAccelY(); /** - * Returns the acceleration along the Z-axis, in Gs. + * Returns the acceleration along the Z-axis, in m/s². */ - double GetAccelZ(); + units::meters_per_second_squared_t GetAccelZ(); /** * Returns the current angle of the Romi around the X-axis, in degrees. */ - double GetGyroAngleX(); + units::radian_t GetGyroAngleX(); /** * Returns the current angle of the Romi around the Y-axis, in degrees. */ - double GetGyroAngleY(); + units::radian_t GetGyroAngleY(); /** * Returns the current angle of the Romi around the Z-axis, in degrees. */ - double GetGyroAngleZ(); + units::radian_t GetGyroAngleZ(); /** * Reset the gyro. diff --git a/xrpVendordep/src/main/native/cpp/xrp/XRPGyro.cpp b/xrpVendordep/src/main/native/cpp/xrp/XRPGyro.cpp index 50127e55514..a312ab4189c 100644 --- a/xrpVendordep/src/main/native/cpp/xrp/XRPGyro.cpp +++ b/xrpVendordep/src/main/native/cpp/xrp/XRPGyro.cpp @@ -5,6 +5,7 @@ #include "frc/xrp/XRPGyro.h" #include +#include using namespace frc; @@ -26,70 +27,70 @@ XRPGyro::XRPGyro() : m_simDevice("Gyro:XRPGyro") { } } -double XRPGyro::GetAngle() const { +units::radian_t XRPGyro::GetAngle() const { return GetAngleZ(); } frc::Rotation2d XRPGyro::GetRotation2d() const { - return frc::Rotation2d{units::degree_t{GetAngle()}}; + return frc::Rotation2d{GetAngle()}; } -double XRPGyro::GetRate() const { +units::radians_per_second_t XRPGyro::GetRate() const { return GetRateZ(); } -double XRPGyro::GetRateX() const { +units::radians_per_second_t XRPGyro::GetRateX() const { if (m_simRateX) { - return m_simRateX.Get(); + return units::degrees_per_second_t{m_simRateX.Get()}; } - return 0.0; + return 0_rad_per_s; } -double XRPGyro::GetRateY() const { +units::radians_per_second_t XRPGyro::GetRateY() const { if (m_simRateY) { - return m_simRateY.Get(); + return units::degrees_per_second_t{m_simRateY.Get()}; } - return 0.0; + return 0_rad_per_s; } -double XRPGyro::GetRateZ() const { +units::radians_per_second_t XRPGyro::GetRateZ() const { if (m_simRateZ) { - return m_simRateZ.Get(); + return units::degrees_per_second_t{m_simRateZ.Get()}; } - return 0.0; + return 0_rad_per_s; } -double XRPGyro::GetAngleX() const { +units::radian_t XRPGyro::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_rad; } -double XRPGyro::GetAngleY() const { +units::radian_t XRPGyro::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_rad; } -double XRPGyro::GetAngleZ() const { +units::radian_t XRPGyro::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_rad; } void XRPGyro::Reset() { if (m_simAngleX) { - m_angleXOffset = m_simAngleX.Get(); - m_angleYOffset = m_simAngleY.Get(); - m_angleZOffset = m_simAngleZ.Get(); + m_angleXOffset = units::degree_t{m_simAngleX.Get()}; + m_angleYOffset = units::degree_t{m_simAngleY.Get()}; + m_angleZOffset = units::degree_t{m_simAngleZ.Get()}; } } diff --git a/xrpVendordep/src/main/native/cpp/xrp/XRPServo.cpp b/xrpVendordep/src/main/native/cpp/xrp/XRPServo.cpp index f7233de22a7..6df18b1cb85 100644 --- a/xrpVendordep/src/main/native/cpp/xrp/XRPServo.cpp +++ b/xrpVendordep/src/main/native/cpp/xrp/XRPServo.cpp @@ -7,9 +7,12 @@ #include #include +#include #include #include +#include + using namespace frc; std::map XRPServo::s_simDeviceMap = {{4, "servo1"}, @@ -44,28 +47,21 @@ XRPServo::XRPServo(int deviceNum) { } } -void XRPServo::SetAngle(double angleDegrees) { - if (angleDegrees < 0.0) { - angleDegrees = 0.0; - } - - if (angleDegrees > 180.0) { - angleDegrees = 180.0; - } - - double pos = (angleDegrees / 180.0); +void XRPServo::SetAngle(units::radian_t angle) { + angle = std::clamp(angle, 0_deg, 180_deg); + double pos = angle.value() / std::numbers::pi; if (m_simPosition) { m_simPosition.Set(pos); } } -double XRPServo::GetAngle() const { +units::radian_t XRPServo::GetAngle() const { if (m_simPosition) { - return m_simPosition.Get() * 180.0; + return units::radian_t{m_simPosition.Get() * std::numbers::pi}; } - return 90.0; + return 90_deg; } void XRPServo::SetPosition(double pos) { diff --git a/xrpVendordep/src/main/native/include/frc/xrp/XRPGyro.h b/xrpVendordep/src/main/native/include/frc/xrp/XRPGyro.h index c2251090dc6..dbd2c1f1d7e 100644 --- a/xrpVendordep/src/main/native/include/frc/xrp/XRPGyro.h +++ b/xrpVendordep/src/main/native/include/frc/xrp/XRPGyro.h @@ -7,6 +7,8 @@ #include #include +#include +#include namespace frc { @@ -31,16 +33,16 @@ class XRPGyro { XRPGyro(); /** - * 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π->2.1π. * 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 radians on the second time around. * - * @return the current heading of the robot in degrees. + * @return the current heading of the robot in radians. */ - double GetAngle() const; + units::radian_t GetAngle() const; /** * Gets the angle the robot is facing. @@ -54,51 +56,51 @@ class XRPGyro { * * The rate is based on the most recent reading of the gyro. * - * @return the current rate in degrees per second + * @return the current rate in radians per second */ - double GetRate() const; + units::radians_per_second_t GetRate() const; /** - * Gets the rate of turn in degrees-per-second around the X-axis. + * Gets the rate of turn in radians-per-second around the X-axis. * - * @return rate of turn in degrees-per-second + * @return rate of turn in radians-per-second */ - double GetRateX() const; + units::radians_per_second_t GetRateX() const; /** - * Gets the rate of turn in degrees-per-second around the Y-axis. + * Gets the rate of turn in radians-per-second around the Y-axis. * - * @return rate of turn in degrees-per-second + * @return rate of turn in radians-per-second */ - double GetRateY() const; + units::radians_per_second_t GetRateY() const; /** - * Gets the rate of turn in degrees-per-second around the Z-axis. + * Gets the rate of turn in radians-per-second around the Z-axis. * - * @return rate of turn in degrees-per-second + * @return rate of turn in radians-per-second */ - double GetRateZ() const; + units::radians_per_second_t GetRateZ() const; /** * Gets the currently reported angle around the X-axis. * - * @return current angle around X-axis in degrees + * @return current angle around X-axis in radians */ - double GetAngleX() const; + units::radian_t GetAngleX() const; /** * Gets the currently reported angle around the Y-axis. * - * @return current angle around Y-axis in degrees + * @return current angle around Y-axis in radians */ - double GetAngleY() const; + units::radian_t GetAngleY() const; /** * Gets the currently reported angle around the Z-axis. * - * @return current angle around Z-axis in degrees + * @return current angle around Z-axis in radians */ - double GetAngleZ() const; + units::radian_t GetAngleZ() const; /** * Reset the gyro angles to 0. @@ -114,9 +116,9 @@ class XRPGyro { hal::SimDouble m_simAngleY; hal::SimDouble m_simAngleZ; - double m_angleXOffset = 0; - double m_angleYOffset = 0; - double m_angleZOffset = 0; + units::radian_t m_angleXOffset = 0_rad; + units::radian_t m_angleYOffset = 0_rad; + units::radian_t m_angleZOffset = 0_rad; }; /** @} */ diff --git a/xrpVendordep/src/main/native/include/frc/xrp/XRPServo.h b/xrpVendordep/src/main/native/include/frc/xrp/XRPServo.h index 744dc641a05..653a16a072f 100644 --- a/xrpVendordep/src/main/native/include/frc/xrp/XRPServo.h +++ b/xrpVendordep/src/main/native/include/frc/xrp/XRPServo.h @@ -9,6 +9,7 @@ #include #include +#include namespace frc { @@ -34,29 +35,33 @@ class XRPServo { /** * Set the servo angle. * - * @param angleDegrees Desired angle in degrees + * @param angle Desired angle in radians */ - void SetAngle(double angleDegrees); + void SetAngle(units::radian_t angle); /** * Get the servo angle. * - * @return Current servo angle + * @return Current servo angle in radians */ - double GetAngle() const; + units::radian_t GetAngle() const; /** * Set the servo position. * * @param position Desired position (Between 0.0 and 1.0) + * @deprecated Use SetAngle() instead */ + [[deprecated("Use SetAngle() instead")]] void SetPosition(double position); /** * Get the servo position. * * @return Current servo position + * @deprecated Use GetAngle() instead */ + [[deprecated("Use GetAngle() instead")]] double GetPosition() const; private: