diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java index a74806e..0957211 100644 --- a/swervelib/SwerveModule.java +++ b/swervelib/SwerveModule.java @@ -248,6 +248,14 @@ public class SwerveModule 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 class SwerveModule 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)