From f57f69395535f66bd089db5076f09980eec45a17 Mon Sep 17 00:00:00 2001
From: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>
Date: Mon, 22 Jan 2024 17:40:04 -0600
Subject: [PATCH] Added back wheel speed desaturation

---
 swervelib/SwerveDrive.java | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java
index 9ee2352..0244c57 100644
--- a/swervelib/SwerveDrive.java
+++ b/swervelib/SwerveDrive.java
@@ -542,6 +542,9 @@ public class SwerveDrive
                                                   maxSpeedMPS,
                                                   attainableMaxTranslationalSpeedMetersPerSecond,
                                                   attainableMaxRotationalVelocityRadiansPerSecond);
+    } else
+    {
+      SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, maxSpeedMPS);
     }
 
     // Sets states