From dd163b62ae6969707d2180b67bced80884c8b8d2 Mon Sep 17 00:00:00 2001
From: Jonah Snider <jonah@jonahsnider.com>
Date: Wed, 13 Apr 2022 22:31:43 -0700
Subject: [PATCH] [wpimath] Rotation2d: Add factory method that uses rotations
 (#4166)

Rotation2d.fromRotations(1).equals(new Rotation2d(2 * Math.PI)); // true

Also adds a member method to get the value of the Rotation2d in rotations.
---
 .../wpi/first/math/geometry/Rotation2d.java   | 40 ++++++++++++++-----
 1 file changed, 30 insertions(+), 10 deletions(-)

diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
index 25f7b93985..b4ad39dcb4 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
@@ -10,6 +10,7 @@ import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.util.Units;
 import java.util.Objects;
 
 /** A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine). */
@@ -67,6 +68,16 @@ public class Rotation2d implements Interpolatable<Rotation2d> {
     return new Rotation2d(Math.toRadians(degrees));
   }
 
+  /**
+   * Constructs and returns a Rotation2d with the given number of rotations.
+   *
+   * @param rotations The value of the angle in rotations.
+   * @return The rotation object with the desired angle value.
+   */
+  public static Rotation2d fromRotations(double rotations) {
+    return new Rotation2d(Units.rotationsToRadians(rotations));
+  }
+
   /**
    * Adds two rotations together, with the result being bounded between -pi and pi.
    *
@@ -133,9 +144,9 @@ public class Rotation2d implements Interpolatable<Rotation2d> {
   }
 
   /**
-   * Returns the radian value of the rotation.
+   * Returns the radian value of the Rotation2d.
    *
-   * @return The radian value of the rotation.
+   * @return The radian value of the Rotation2d.
    */
   @JsonProperty
   public double getRadians() {
@@ -143,36 +154,45 @@ public class Rotation2d implements Interpolatable<Rotation2d> {
   }
 
   /**
-   * Returns the degree value of the rotation.
+   * Returns the degree value of the Rotation2d.
    *
-   * @return The degree value of the rotation.
+   * @return The degree value of the Rotation2d.
    */
   public double getDegrees() {
     return Math.toDegrees(m_value);
   }
 
   /**
-   * Returns the cosine of the rotation.
+   * Returns the number of rotations of the Rotation2d.
    *
-   * @return The cosine of the rotation.
+   * @return The number of rotations of the Rotation2d.
+   */
+  public double getRotations() {
+    return Units.radiansToRotations(m_value);
+  }
+
+  /**
+   * Returns the cosine of the Rotation2d.
+   *
+   * @return The cosine of the Rotation2d.
    */
   public double getCos() {
     return m_cos;
   }
 
   /**
-   * Returns the sine of the rotation.
+   * Returns the sine of the Rotation2d.
    *
-   * @return The sine of the rotation.
+   * @return The sine of the Rotation2d.
    */
   public double getSin() {
     return m_sin;
   }
 
   /**
-   * Returns the tangent of the rotation.
+   * Returns the tangent of the Rotation2d.
    *
-   * @return The tangent of the rotation.
+   * @return The tangent of the Rotation2d.
    */
   public double getTan() {
     return m_sin / m_cos;