Fixed inconsistency.

Signed-off-by: thenetworkgrinch <[email protected]>
GeraldTM · Apr 12, 2023 · 7e647ad · 7e647ad
1 parent e5ac514
commit 7e647ad
Showing 1 changed file with 5 additions and 0 deletions.
diff --git a/src/main/java/swervelib/math/SwerveModuleState2.java b/src/main/java/swervelib/math/SwerveModuleState2.java
@@ -69,6 +69,11 @@ public static SwerveModuleState2 optimize(SwerveModuleState2 desiredState, Rotat
     {
       optimized.omegaRadPerSecond = 0;
     }
+    if (desiredState.angle.equals(optimized.angle.rotateBy(Rotation2d.fromDegrees(180))))
+    {
+      desiredState.omegaRadPerSecond = 0;
+      return desiredState;
+    }
     return optimized;
     /*
     // NEVER optimize if it's the same angle, it just doesn't make sense...