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

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

@@ -11,7 +11,6 @@ import edu.wpi.first.math.kinematics.struct.MecanumDriveWheelSpeedsStruct;
import edu.wpi.first.units.Distance;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Velocity;
import java.util.stream.DoubleStream;
public class MecanumDriveWheelSpeeds {
/** Speed of the front left wheel. */
@@ -83,13 +82,9 @@ public class MecanumDriveWheelSpeeds {
*/
public void desaturate(double attainableMaxSpeedMetersPerSecond) {
double realMaxSpeed =
DoubleStream.of(
frontLeftMetersPerSecond,
frontRightMetersPerSecond,
rearLeftMetersPerSecond,
rearRightMetersPerSecond)
.max()
.getAsDouble();
Math.max(Math.abs(frontLeftMetersPerSecond), Math.abs(frontRightMetersPerSecond));
realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearLeftMetersPerSecond));
realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearRightMetersPerSecond));
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
frontLeftMetersPerSecond =

4
wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
View File

@@ -16,10 +16,10 @@ void MecanumDriveWheelSpeeds::Desaturate(
units::meters_per_second_t attainableMaxSpeed) {
std::array<units::meters_per_second_t, 4> wheelSpeeds{frontLeft, frontRight,
rearLeft, rearRight};
units::meters_per_second_t realMaxSpeed = *std::max_element(
units::meters_per_second_t realMaxSpeed = units::math::abs(*std::max_element(
wheelSpeeds.begin(), wheelSpeeds.end(), [](const auto& a, const auto& b) {
return units::math::abs(a) < units::math::abs(b);
});
}));
if (realMaxSpeed > attainableMaxSpeed) {
for (int i = 0; i < 4; ++i) {

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

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