From b9409a4bcf85705a343c88028cd5697915a5e3c3 Mon Sep 17 00:00:00 2001
From: Jade <spacey-sooty@proton.me>
Date: Fri, 13 Sep 2024 13:16:00 +0800
Subject: [PATCH] [xrp] Add GetRotation2d to Gyro (#7034)

---
 .../main/java/edu/wpi/first/wpilibj/xrp/XRPGyro.java   | 10 ++++++++++
 xrpVendordep/src/main/native/cpp/xrp/XRPGyro.cpp       |  6 ++++++
 xrpVendordep/src/main/native/include/frc/xrp/XRPGyro.h |  9 +++++++++
 3 files changed, 25 insertions(+)

diff --git a/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPGyro.java b/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPGyro.java
index b1b8199e62..1c6403edfe 100644
--- a/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPGyro.java
+++ b/xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPGyro.java
@@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj.xrp;
 import edu.wpi.first.hal.SimDevice;
 import edu.wpi.first.hal.SimDevice.Direction;
 import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.math.geometry.Rotation2d;
 
 /**
  * Use a rate gyro to return the robots heading relative to a starting position.
@@ -154,6 +155,15 @@ public class XRPGyro {
     return getAngleZ();
   }
 
+  /**
+   * Gets the angle the robot is facing.
+   *
+   * @return A Rotation2d with the current heading.
+   */
+  public Rotation2d getRotation2d() {
+    return Rotation2d.fromDegrees(getAngle());
+  }
+
   /**
    * Return the rate of rotation of the gyro
    *
diff --git a/xrpVendordep/src/main/native/cpp/xrp/XRPGyro.cpp b/xrpVendordep/src/main/native/cpp/xrp/XRPGyro.cpp
index 46bf6180f5..50127e5551 100644
--- a/xrpVendordep/src/main/native/cpp/xrp/XRPGyro.cpp
+++ b/xrpVendordep/src/main/native/cpp/xrp/XRPGyro.cpp
@@ -4,6 +4,8 @@
 
 #include "frc/xrp/XRPGyro.h"
 
+#include <units/angle.h>
+
 using namespace frc;
 
 XRPGyro::XRPGyro() : m_simDevice("Gyro:XRPGyro") {
@@ -28,6 +30,10 @@ double XRPGyro::GetAngle() const {
   return GetAngleZ();
 }
 
+frc::Rotation2d XRPGyro::GetRotation2d() const {
+  return frc::Rotation2d{units::degree_t{GetAngle()}};
+}
+
 double XRPGyro::GetRate() const {
   return GetRateZ();
 }
diff --git a/xrpVendordep/src/main/native/include/frc/xrp/XRPGyro.h b/xrpVendordep/src/main/native/include/frc/xrp/XRPGyro.h
index 1cf49a7710..e32bd753fd 100644
--- a/xrpVendordep/src/main/native/include/frc/xrp/XRPGyro.h
+++ b/xrpVendordep/src/main/native/include/frc/xrp/XRPGyro.h
@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <frc/geometry/Rotation2d.h>
+
 #include <hal/SimDevice.h>
 
 namespace frc {
@@ -40,6 +42,13 @@ class XRPGyro {
    */
   double GetAngle() const;
 
+  /**
+   * Gets the angle the robot is facing.
+   *
+   * @return A Rotation2d with the current heading.
+   */
+  frc::Rotation2d GetRotation2d() const;
+
   /**
    * Return the rate of rotation of the gyro
    *