Anti-Jitter should go before cosine compensation. Fixes many issues.

Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>

src/main/java/swervelib/SwerveModule.java

@@ -248,6 +248,14 @@ public void queueSynchronizeEncoders()
   public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force)
   {
     desiredState = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(getAbsolutePosition()));
+
+    // If we are forcing the angle
+    if (!force)
+    {
+      // Prevents module rotation if speed is less than 1%
+      SwerveMath.antiJitter(desiredState, lastState, Math.min(maxSpeed, 4));
+    }
+
     // Cosine compensation.
     double velocity = configuration.useCosineCompensator
                       ? getCosineCompensatedVelocity(desiredState)
@@ -263,13 +271,6 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop,
       desiredState.speedMetersPerSecond = velocity;
     }
 
-    // If we are forcing the angle
-    if (!force)
-    {
-      // Prevents module rotation if speed is less than 1%
-      SwerveMath.antiJitter(desiredState, lastState, Math.min(maxSpeed, 4));
-    }
-
     // Prevent module rotation if angle is the same as the previous angle.
     // Synchronize encoders if queued and send in the current position as the value from the absolute encoder.
     if (absoluteEncoder != null && synchronizeEncoderQueued)