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

11
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
View File

@@ -10,7 +10,6 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Twist2d;
import java.util.Arrays;
import java.util.Collections;
import org.ejml.simple.SimpleMatrix;
/**
@@ -233,7 +232,10 @@ public class SwerveDriveKinematics {
*/
public static void desaturateWheelSpeeds(
SwerveModuleState[] moduleStates, double attainableMaxSpeedMetersPerSecond) {
double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
double realMaxSpeed = 0;
for (SwerveModuleState moduleState : moduleStates) {
realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speedMetersPerSecond));
}
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
for (SwerveModuleState moduleState : moduleStates) {
moduleState.speedMetersPerSecond =
@@ -266,7 +268,10 @@ public class SwerveDriveKinematics {
double attainableMaxModuleSpeedMetersPerSecond,
double attainableMaxTranslationalSpeedMetersPerSecond,
double attainableMaxRotationalVelocityRadiansPerSecond) {
double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
double realMaxSpeed = 0;
for (SwerveModuleState moduleState : moduleStates) {
realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speedMetersPerSecond));
}
if (attainableMaxTranslationalSpeedMetersPerSecond == 0
|| attainableMaxRotationalVelocityRadiansPerSecond == 0

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) {

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));
}
}

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);
}