[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.