Merge branch 'main' of https://github.com/Galaxia5987/common

Galaxia5987 · Feb 9, 2024 · c5614fb · c5614fb
2 parents be83eaa + 5fda7f8
commit c5614fb
Showing 1 changed file with 12 additions and 4 deletions.
diff --git a/src/main/java/lib/Utils.java b/src/main/java/lib/Utils.java
@@ -2,6 +2,10 @@
 
 import edu.wpi.first.math.geometry.*;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.units.Angle;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Units;
+
 import java.util.Comparator;
 import java.util.List;
 
@@ -38,17 +42,21 @@ public static double averageAmbiguity(List<Double> ambiguities) {
         return 1.0 / ambiguities.stream().map((num) -> 1.0 / num).reduce(0.0, Double::sum);
     }
 
-    public static double normalize(double angle) {
-        while (angle < 0) {
-            angle += 2 * Math.PI;
+    public static double normalize(double angleRadians) {
+        while (angleRadians < 0) {
+            angleRadians += 2 * Math.PI;
         }
-        return angle % (2 * Math.PI);
+        return angleRadians % (2 * Math.PI);
     }
 
     public static Rotation2d normalize(Rotation2d angle) {
         return Rotation2d.fromRadians(normalize(angle.getRadians()));
     }
 
+    public static Measure<Angle> normalize(Measure<Angle> angle) {
+        return Units.Radians.of(normalize(angle.in(Units.Radians)));
+    }
+
     public static double getDistanceFromPoint(Translation2d point, Pose2d robotPose) {
         return robotPose.getTranslation().getDistance(point);
     }