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

[wpimath] MecanumDriveWheelSpeeds: Fix desaturate() (#6040)

Browse Source
This commit is contained in:
Gold856
2023-12-14 23:52:45 -05:00
committed by GitHub
parent 8798700cec
commit f87c64af8a
4 changed files with 31 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
View File

@@ -238,4 +238,18 @@ class MecanumDriveKinematicsTest {
() -> assertEquals(4.0 * factor, wheelSpeeds.rearLeftMetersPerSecond, kEpsilon),
() -> assertEquals(7.0 * factor, wheelSpeeds.rearRightMetersPerSecond, kEpsilon));
}
@Test
void testDesaturateNegativeSpeeds() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(-5, 6, 4, -7);
wheelSpeeds.desaturate(5.5);
final double kFactor = 5.5 / 7.0;
assertAll(
() -> assertEquals(-5.0 * kFactor, wheelSpeeds.frontLeftMetersPerSecond, kEpsilon),
() -> assertEquals(6.0 * kFactor, wheelSpeeds.frontRightMetersPerSecond, kEpsilon),
() -> assertEquals(4.0 * kFactor, wheelSpeeds.rearLeftMetersPerSecond, kEpsilon),
() -> assertEquals(-7.0 * kFactor, wheelSpeeds.rearRightMetersPerSecond, kEpsilon));
}
}