Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +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

17
wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
View File

@@ -371,4 +371,21 @@ class SwerveDriveKinematicsTest {
() -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon),
() -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon));
}
@Test
void testDesaturateNegativeSpeed() {
SwerveModuleState fl = new SwerveModuleState(1, new Rotation2d());
SwerveModuleState fr = new SwerveModuleState(1, new Rotation2d());
SwerveModuleState bl = new SwerveModuleState(-2, new Rotation2d());
SwerveModuleState br = new SwerveModuleState(-2, new Rotation2d());
SwerveModuleState[] arr = {fl, fr, bl, br};
SwerveDriveKinematics.desaturateWheelSpeeds(arr, 1);
assertAll(
() -> assertEquals(0.5, arr[0].speedMetersPerSecond, kEpsilon),
() -> assertEquals(0.5, arr[1].speedMetersPerSecond, kEpsilon),
() -> assertEquals(-1.0, arr[2].speedMetersPerSecond, kEpsilon),
() -> assertEquals(-1.0, arr[3].speedMetersPerSecond, kEpsilon));
}
}