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] 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/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