Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11: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

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