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

15
wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
View File

@@ -274,3 +274,18 @@ TEST_F(SwerveDriveKinematicsTest, DesaturateSmooth) {
EXPECT_NEAR(arr[2].speed.value(), 4.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[3].speed.value(), 7.0 * kFactor, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, DesaturateNegativeSpeed) {
SwerveModuleState state1{1.0_mps, 0_deg};
SwerveModuleState state2{1.0_mps, 0_deg};
SwerveModuleState state3{-2.0_mps, 0_deg};
SwerveModuleState state4{-2.0_mps, 0_deg};
wpi::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelSpeeds(&arr, 1.0_mps);
EXPECT_NEAR(arr[0].speed.value(), 0.5, kEpsilon);
EXPECT_NEAR(arr[1].speed.value(), 0.5, kEpsilon);
EXPECT_NEAR(arr[2].speed.value(), -1.0, kEpsilon);
EXPECT_NEAR(arr[3].speed.value(), -1.0, kEpsilon);
}