Update WPILib, only check angle once when setting module angle

Author: democat3457

examples/mk3-minibot/build.gradle

@@ -1,7 +1,7 @@
 plugins {
     id 'application'
     id 'com.github.johnrengelman.shadow'
-    id 'edu.wpi.first.GradleRIO' version "2024.1.1"
+    id 'edu.wpi.first.GradleRIO' version "2024.2.1"
 }
 
 application {

examples/mk3-testchassis/build.gradle

@@ -1,7 +1,7 @@
 plugins {
     id 'application'
     id 'com.github.johnrengelman.shadow'
-    id 'edu.wpi.first.GradleRIO' version "2024.1.1"
+    id 'edu.wpi.first.GradleRIO' version "2024.2.1"
 }
 
 application {

gradle.properties

@@ -1,6 +1,6 @@
 org.gradle.warning.mode=all
 
 # dependency versions
-wpilib_version = 2024.1.1
+wpilib_version = 2024.2.1
 ctre_phoenix_version = 24.1.0
 revlib_version = 2024.2.0

src/main/java/com/swervedrivespecialties/swervelib/SwerveModuleFactory.java

@@ -127,14 +127,17 @@ public void set(double driveVoltage, double steerAngle) {
                 steerAngle += 2.0 * Math.PI;
             }
 
-            double difference = steerAngle - getSteerAngle();
-            // Change the target angle so the difference is in the range [-pi, pi) instead of [0, 2pi)
+            double currentAngle = getSteerAngle();
+
+            // [0, 2pi) - [0, 2pi) = (-2pi, 2pi)
+            double difference = steerAngle - currentAngle;
+            // Change the target angle so the difference is in the range [-pi, pi) instead of (-2pi, 2pi)
             if (difference >= Math.PI) {
                 steerAngle -= 2.0 * Math.PI;
             } else if (difference < -Math.PI) {
                 steerAngle += 2.0 * Math.PI;
             }
-            difference = steerAngle - getSteerAngle(); // Recalculate difference
+            difference = steerAngle - currentAngle; // Recalculate difference
 
             // If the difference is greater than 90 deg or less than -90 deg the drive can be inverted so the total
             // movement of the module is less than 90 deg