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

12
wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
View File

@@ -215,3 +215,15 @@ TEST_F(MecanumDriveKinematicsTest, Desaturate) {
EXPECT_NEAR(wheelSpeeds.rearLeft.value(), 4.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.rearRight.value(), 7.0 * kFactor, 1E-9);
}
TEST_F(MecanumDriveKinematicsTest, DesaturateNegativeSpeeds) {
MecanumDriveWheelSpeeds wheelSpeeds{-5_mps, 6_mps, 4_mps, -7_mps};
wheelSpeeds.Desaturate(5.5_mps);
constexpr double kFactor = 5.5 / 7.0;
EXPECT_NEAR(wheelSpeeds.frontLeft.value(), -5.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.frontRight.value(), 6.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.rearLeft.value(), 4.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.rearRight.value(), -7.0 * kFactor, 1E-9);
}