[examples] Update C++ XRP Code to use SI Units (#7366)

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);

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);
 }

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();
 }
 

wpilibcExamples/src/main/cpp/examples/XRPReference/include/subsystems/Arm.h

@@ -6,6 +6,7 @@
 
 #include <frc/xrp/XRPServo.h>
 #include <frc2/command/SubsystemBase.h>
+#include <units/angle.h>
 
 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};

wpilibcExamples/src/main/cpp/examples/XRPReference/include/subsystems/Drivetrain.h

@@ -10,6 +10,8 @@
 #include <frc/xrp/XRPGyro.h>
 #include <frc/xrp/XRPMotor.h>
 #include <frc2/command/SubsystemBase.h>
+#include <units/acceleration.h>
+#include <units/angle.h>
 #include <units/length.h>
 
 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.

xrpVendordep/src/main/native/cpp/xrp/XRPGyro.cpp

@@ -5,6 +5,7 @@
 #include "frc/xrp/XRPGyro.h"
 
 #include <units/angle.h>
+#include <units/angular_velocity.h>
 
 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()};
   }
 }

xrpVendordep/src/main/native/cpp/xrp/XRPServo.cpp

@@ -7,9 +7,12 @@
 #include <frc/Errors.h>
 
 #include <map>
+#include <numbers>
 #include <set>
 #include <string>
 
+#include <units/angle.h>
+
 using namespace frc;
 
 std::map<int, std::string> 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<units::radian_t>(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) {