cpp changes

mjansen4857 · Jan 5, 2025 · d91b3b0 · d91b3b0
1 parent fa20b64
commit d91b3b0
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 4 deletions.
diff --git a/pathplannerlib-python/pathplannerlib/util/swerve.py b/pathplannerlib-python/pathplannerlib/util/swerve.py
@@ -221,10 +221,10 @@ def generateSetpoint(self, prev_setpoint: SwerveSetpoint, desired_state_robot_re
             # Use the current battery voltage since we won't be able to supply 12v if the
             # battery is sagging down to 11v, which will affect the max torque output
             current_draw = self._config.moduleConfig.driveMotor.current(abs(last_vel_rad_per_sec), input_voltage)
-            current_draw = max(current_draw, 0.0)
             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)

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 =