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

26
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
View File

@@ -134,12 +134,13 @@ void SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds(
wpi::array<SwerveModuleState, NumModules>* moduleStates,
units::meters_per_second_t attainableMaxSpeed) {
auto& states = *moduleStates;
auto realMaxSpeed = std::max_element(states.begin(), states.end(),
[](const auto& a, const auto& b) {
return units::math::abs(a.speed) <
units::math::abs(b.speed);
})
->speed;
auto realMaxSpeed =
units::math::abs(std::max_element(states.begin(), states.end(),
[](const auto& a, const auto& b) {
return units::math::abs(a.speed) <
units::math::abs(b.speed);
})
->speed);
if (realMaxSpeed > attainableMaxSpeed) {
for (auto& module : states) {
@@ -157,12 +158,13 @@ void SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds(
units::radians_per_second_t attainableMaxRobotRotationSpeed) {
auto& states = *moduleStates;
auto realMaxSpeed = std::max_element(states.begin(), states.end(),
[](const auto& a, const auto& b) {
return units::math::abs(a.speed) <
units::math::abs(b.speed);
})
->speed;
auto realMaxSpeed =
units::math::abs(std::max_element(states.begin(), states.end(),
[](const auto& a, const auto& b) {
return units::math::abs(a.speed) <
units::math::abs(b.speed);
})
->speed);
if (attainableMaxRobotTranslationSpeed == 0_mps ||
attainableMaxRobotRotationSpeed == 0_rad_per_s || realMaxSpeed == 0_mps) {