Merge pull request #232 from yapplejack/desaturate-fix

Suggestion for desaturateWheelSpeeds() to use desiredChassisSpeeds
BroncBotz3481 · Oct 5, 2024 · 8bc8f43 · 8bc8f43
2 parents ed3e23e + a2dbab9
commit 8bc8f43
Showing 1 changed file with 16 additions and 6 deletions.
diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java
@@ -518,7 +518,7 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent
     // Calculate required module states via kinematics
     SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(velocity, centerOfRotationMeters);
 
-    setRawModuleStates(swerveModuleStates, isOpenLoop);
+    setRawModuleStates(swerveModuleStates, velocity, isOpenLoop);
   }
 
   /**
@@ -568,14 +568,15 @@ public double getMaximumAngularVelocity()
    * Set the module states (azimuth and velocity) directly.
    *
    * @param desiredStates A list of SwerveModuleStates to send to the modules.
+   * @param desiredChassisSpeed The desired chassis speeds to set the robot to achieve.
    * @param isOpenLoop    Whether to use closed-loop velocity control. Set to true to disable closed-loop.
    */
-  private void setRawModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop)
+  private void setRawModuleStates(SwerveModuleState[] desiredStates, ChassisSpeeds desiredChassisSpeed, boolean isOpenLoop)
   {
     // Desaturates wheel speeds
     if (attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0)
     {
-      SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, getRobotVelocity(),
+      SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, desiredChassisSpeed,
                                                   maxSpeedMPS,
                                                   attainableMaxTranslationalSpeedMetersPerSecond,
                                                   attainableMaxRotationalVelocityRadiansPerSecond);
@@ -593,14 +594,23 @@ private void setRawModuleStates(SwerveModuleState[] desiredStates, boolean isOpe
 
   /**
    * Set the module states (azimuth and velocity) directly. Used primarily for auto paths.
+   * Does not allow for usage of desaturateWheelSpeeds(SwerveModuleState[] moduleStates, 
+   * ChassisSpeeds desiredChassisSpeed, double attainableMaxModuleSpeedMetersPerSecond, 
+   * double attainableMaxTranslationalSpeedMetersPerSecond, double attainableMaxRotationalVelocityRadiansPerSecond)
    *
    * @param desiredStates A list of SwerveModuleStates to send to the modules.
    * @param isOpenLoop    Whether to use closed-loop velocity control. Set to true to disable closed-loop.
    */
   public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop)
   {
-    setRawModuleStates(kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates)),
-                       isOpenLoop);
+    desiredStates = kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates));
+    SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, maxSpeedMPS);
+
+    // Sets states
+    for (SwerveModule module : swerveModules)
+    {
+      module.setDesiredState(desiredStates[module.moduleNumber], isOpenLoop, false);
+    }
   }
 
   /**
@@ -614,7 +624,7 @@ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds)
     SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond;
     SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond);
 
-    setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), false);
+    setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), chassisSpeeds, false);
   }
 
   /**