[Swerve Setpoint Generator] Limit max speed when voltage sags (#931)

Co-authored-by: Michael Jansen <[email protected]>
mjansen4857 · Jan 5, 2025 · 5299889 · 5299889
1 parent b6d6207
commit 5299889
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 7 deletions.
diff --git a/pathplannerlib-python/pathplannerlib/util/swerve.py b/pathplannerlib-python/pathplannerlib/util/swerve.py
@@ -74,10 +74,11 @@ def generateSetpoint(self, prev_setpoint: SwerveSetpoint, desired_state_robot_re
         else:
             input_voltage = max(input_voltage, self._brownoutVoltage)
 
+        maxSpeed = self._config.moduleConfig.maxDriveVelocityMPS * min(1, input_voltage / 12)
+
         desired_module_states = self._config.toSwerveModuleStates(desired_state_robot_relative)
         # Make sure desired_state respects velocity limits.
-        SwerveDrive4Kinematics.desaturateWheelSpeeds(desired_module_states,
-                                                     self._config.moduleConfig.maxDriveVelocityMPS)
+        SwerveDrive4Kinematics.desaturateWheelSpeeds(desired_module_states, maxSpeed)
         desired_state_robot_relative = self._config.toChassisSpeeds(desired_module_states)
 
         # Special case: desired_state is a complete stop. In this case, module angle is arbitrary, so
@@ -223,7 +224,9 @@ def generateSetpoint(self, prev_setpoint: SwerveSetpoint, desired_state_robot_re
             reverse_current_draw = abs(
                 self._config.moduleConfig.driveMotor.getCurrent(abs(last_vel_rad_per_sec), -input_voltage))
             current_draw = min(current_draw, self._config.moduleConfig.driveCurrentLimit)
+            current_draw = max(current_draw, 0.0)
             reverse_current_draw = min(reverse_current_draw, self._config.moduleConfig.driveCurrentLimit)
+            reverse_current_draw = max(reverse_current_draw, 0.0)
             forward_module_torque = self._config.moduleConfig.driveMotor.torque(current_draw)
             reverse_module_torque = self._config.moduleConfig.driveMotor.torque(reverse_current_draw)
 

diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java b/pathplannerlib/src/main/java/com/pathplanner/lib/util/swerve/SwerveSetpointGenerator.java
@@ -80,12 +80,12 @@ public SwerveSetpoint generateSetpoint(
     } else {
       inputVoltage = Math.max(inputVoltage, brownoutVoltage);
     }
+    double maxSpeed = config.moduleConfig.maxDriveVelocityMPS * Math.min(1, inputVoltage / 12);
 
     SwerveModuleState[] desiredModuleStates =
         config.toSwerveModuleStates(desiredStateRobotRelative);
     // Make sure desiredState respects velocity limits.
-    SwerveDriveKinematics.desaturateWheelSpeeds(
-        desiredModuleStates, config.moduleConfig.maxDriveVelocityMPS);
+    SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleStates, maxSpeed);
     desiredStateRobotRelative = config.toChassisSpeeds(desiredModuleStates);
 
     // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so
@@ -254,7 +254,9 @@ public SwerveSetpoint generateSetpoint(
           Math.abs(
               config.moduleConfig.driveMotor.getCurrent(Math.abs(lastVelRadPerSec), -inputVoltage));
       currentDraw = Math.min(currentDraw, config.moduleConfig.driveCurrentLimit);
+      currentDraw = Math.max(currentDraw, 0);
       reverseCurrentDraw = Math.min(reverseCurrentDraw, config.moduleConfig.driveCurrentLimit);
+      reverseCurrentDraw = Math.max(reverseCurrentDraw, 0);
       double forwardModuleTorque = config.moduleConfig.driveMotor.getTorque(currentDraw);
       double reverseModuleTorque = config.moduleConfig.driveMotor.getTorque(reverseCurrentDraw);
 

diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/util/swerve/SwerveSetpointGenerator.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/util/swerve/SwerveSetpointGenerator.cpp
@@ -1,4 +1,5 @@
 #include "pathplanner/lib/util/swerve/SwerveSetpointGenerator.h"
+#include <algorithm>
 
 SwerveSetpointGenerator::SwerveSetpointGenerator() : maxSteerVelocity(
 		0_rad_per_s) {
@@ -20,13 +21,15 @@ SwerveSetpoint SwerveSetpointGenerator::generateSetpoint(
 	} else {
 		inputVoltage = units::math::max(inputVoltage, brownoutVoltage);
 	}
+	units::meters_per_second_t maxSpeed =
+			m_robotConfig.moduleConfig.maxDriveVelocityMPS
+					* std::min(1.0, inputVoltage() / 12.0);
 
 	std::vector < frc::SwerveModuleState > desiredModuleStates =
 			m_robotConfig.toSwerveModuleStates(desiredStateRobotRelative);
 	// Make sure desiredState respects velocity limits.
 	desiredModuleStates = m_robotConfig.desaturateWheelSpeeds(
-			desiredModuleStates,
-			m_robotConfig.moduleConfig.maxDriveVelocityMPS);
+			desiredModuleStates, maxSpeed);
 	desiredStateRobotRelative = m_robotConfig.toChassisSpeeds(
 			desiredModuleStates);
 
@@ -191,10 +194,12 @@ SwerveSetpoint SwerveSetpointGenerator::generateSetpoint(
 		units::ampere_t reverseCurrentDraw = units::math::abs(
 				m_robotConfig.moduleConfig.driveMotor.Current(
 						units::math::abs(lastVelRadPerSec), -inputVoltage));
-		currentDraw = std::min(currentDraw,
+		currentDraw = units::math::min(currentDraw,
 				m_robotConfig.moduleConfig.driveCurrentLimit);
+		currentDraw = units::math::max(currentDraw, 0_A);
 		reverseCurrentDraw = units::math::min(reverseCurrentDraw,
 				m_robotConfig.moduleConfig.driveCurrentLimit);
+		reverseCurrentDraw = units::math::max(reverseCurrentDraw, 0_A);
 		units::newton_meter_t forwardModuleTorque =
 				m_robotConfig.moduleConfig.driveMotor.Torque(currentDraw);
 		units::newton_meter_t reverseModuleTorque =