From 529988901ce72135933b937be5c74b416981e2a4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E3=83=8F=E3=82=A4=E3=83=89=E3=83=A9=E3=83=B3=E3=83=88?= Date: Sat, 4 Jan 2025 19:44:34 -0600 Subject: [PATCH] [Swerve Setpoint Generator] Limit max speed when voltage sags (#931) Co-authored-by: Michael Jansen --- pathplannerlib-python/pathplannerlib/util/swerve.py | 7 +++++-- .../lib/util/swerve/SwerveSetpointGenerator.java | 6 ++++-- .../lib/util/swerve/SwerveSetpointGenerator.cpp | 11 ++++++++--- 3 files changed, 17 insertions(+), 7 deletions(-) diff --git a/pathplannerlib-python/pathplannerlib/util/swerve.py b/pathplannerlib-python/pathplannerlib/util/swerve.py index da3962c6..b3b6dc48 100644 --- 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 index 857693e3..7df07a4a 100644 --- 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 index 4c9ba6df..bf72e5a0 100644 --- 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 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 =