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