diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index a74806e22..0957211e0 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/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)