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] Add cosineScale method to SwerveModuleState and instance optimize (#7114)

Browse Source
This commit is contained in:
Nicholas Armstrong
2024-09-30 15:23:30 -04:00
committed by GitHub
parent fde264b041
commit fe80d72fba
13 changed files with 311 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
View File

@@ -83,6 +83,21 @@ public class SwerveModuleState
"SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond, angle);
}
/**
* Minimize the change in heading this swerve module state would require by potentially reversing
* the direction the wheel spins. If this is used with the PIDController class's continuous input
* functionality, the furthest a wheel will ever rotate is 90 degrees.
*
* @param currentAngle The current module angle.
*/
public void optimize(Rotation2d currentAngle) {
var delta = angle.minus(currentAngle);
if (Math.abs(delta.getDegrees()) > 90.0) {
speedMetersPerSecond *= -1;
angle = angle.rotateBy(Rotation2d.kPi);
}
}
/**
* Minimize the change in heading the desired swerve module state would require by potentially
* reversing the direction the wheel spins. If this is used with the PIDController class's
@@ -91,7 +106,9 @@ public class SwerveModuleState
* @param desiredState The desired state.
* @param currentAngle The current module angle.
* @return Optimized swerve module state.
* @deprecated Use the instance method instead.
*/
@Deprecated
public static SwerveModuleState optimize(
SwerveModuleState desiredState, Rotation2d currentAngle) {
var delta = desiredState.angle.minus(currentAngle);
@@ -102,4 +119,15 @@ public class SwerveModuleState
return new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
}
}
/**
* Scales speed by cosine of angle error. This scales down movement perpendicular to the desired
* direction of travel that can occur when modules change directions. This results in smoother
* driving.
*
* @param currentAngle The current module angle.
*/
public void cosineScale(Rotation2d currentAngle) {
speedMetersPerSecond *= angle.minus(currentAngle).getCos();
}
}