Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +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();
}
}

28
wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
View File

@@ -34,6 +34,22 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
*/
bool operator==(const SwerveModuleState& other) const;
/**
* Minimize the change in the 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.
*/
void Optimize(const Rotation2d& currentAngle) {
auto delta = angle - currentAngle;
if (units::math::abs(delta.Degrees()) > 90_deg) {
speed *= -1;
angle = angle + Rotation2d{180_deg};
}
}
/**
* Minimize the change in heading the desired swerve module state would
* require by potentially reversing the direction the wheel spins. If this is
@@ -43,8 +59,20 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
* @param desiredState The desired state.
* @param currentAngle The current module angle.
*/
[[deprecated("Use instance method instead.")]]
static SwerveModuleState Optimize(const SwerveModuleState& desiredState,
const Rotation2d& currentAngle);
/**
* 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.
*/
void CosineScale(const Rotation2d& currentAngle) {
speed *= (angle - currentAngle).Cos();
}
};
} // namespace frc