From 1bebe8a5fd8844b53ea4c46b8a218817ce74dab0 Mon Sep 17 00:00:00 2001
From: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>
Date: Mon, 6 Mar 2023 20:55:49 -0600
Subject: [PATCH] Added redundancy to prevent sending CAN messages too often,
 might interfere with PWM motors

---
 swervelib/SwerveModule.java | 17 ++++++++++++++---
 1 file changed, 14 insertions(+), 3 deletions(-)

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)