Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Merged
merged 8 commits into from
Jan 5, 2025
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 5 additions & 2 deletions pathplannerlib-python/pathplannerlib/util/swerve.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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);

Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
#include "pathplanner/lib/util/swerve/SwerveSetpointGenerator.h"
#include <algorithm>

SwerveSetpointGenerator::SwerveSetpointGenerator() : maxSteerVelocity(
0_rad_per_s) {
Expand All @@ -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);

Expand Down Expand Up @@ -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 =
Expand Down
Loading