diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java index d97c5f2..51be3cc 100644 --- a/swervelib/SwerveModule.java +++ b/swervelib/SwerveModule.java @@ -47,6 +47,10 @@ public class SwerveModule * Last angle set for the swerve module. */ public double lastAngle; + /** + * Last velocity set for the swerve module. + */ + public double lastVelocity; /** * Simulated swerve module. */ @@ -161,7 +165,11 @@ public class SwerveModule } else { double velocity = desiredState.speedMetersPerSecond; - driveMotor.setReference(velocity, feedforward.calculate(velocity)); + if (velocity != lastVelocity) + { + driveMotor.setReference(velocity, feedforward.calculate(velocity)); + } + lastVelocity = velocity; } // Prevents module rotation if speed is less than 1% @@ -169,8 +177,11 @@ public class SwerveModule (Math.abs(desiredState.speedMetersPerSecond) <= (configuration.maxSpeed * 0.01) ? lastAngle : desiredState.angle.getDegrees()); - angleMotor.setReference( - angle, Math.toDegrees(desiredState.omegaRadPerSecond) * configuration.angleKV); + if (angle != lastAngle) + { + angleMotor.setReference( + angle, Math.toDegrees(desiredState.omegaRadPerSecond) * configuration.angleKV); + } lastAngle = angle; if (SwerveDriveTelemetry.isSimulation)