Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix desaturateWheelSpeeds to account for negative speeds (#5269)

Browse Source
This commit is contained in:
Gold856
2023-04-28 23:53:20 -04:00
committed by GitHub
parent e099948a77
commit 24828afd11
4 changed files with 54 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
View File

@@ -10,7 +10,6 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Twist2d;
import java.util.Arrays;
import java.util.Collections;
import org.ejml.simple.SimpleMatrix;
/**
@@ -233,7 +232,10 @@ public class SwerveDriveKinematics {
*/
public static void desaturateWheelSpeeds(
SwerveModuleState[] moduleStates, double attainableMaxSpeedMetersPerSecond) {
double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
double realMaxSpeed = 0;
for (SwerveModuleState moduleState : moduleStates) {
realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speedMetersPerSecond));
}
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
for (SwerveModuleState moduleState : moduleStates) {
moduleState.speedMetersPerSecond =
@@ -266,7 +268,10 @@ public class SwerveDriveKinematics {
double attainableMaxModuleSpeedMetersPerSecond,
double attainableMaxTranslationalSpeedMetersPerSecond,
double attainableMaxRotationalVelocityRadiansPerSecond) {
double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
double realMaxSpeed = 0;
for (SwerveModuleState moduleState : moduleStates) {
realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speedMetersPerSecond));
}
if (attainableMaxTranslationalSpeedMetersPerSecond == 0
|| attainableMaxRotationalVelocityRadiansPerSecond == 0