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] SwerveDrivePoseEstimator: Fix stationary module emitting error when calculating angle in ToSwerveModuleStates (#7175)

Browse Source
This commit is contained in:
Bryce Roethel
2024-10-11 01:05:49 -04:00
committed by GitHub
parent 2085ab3d47
commit d97a749d84
2 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
View File

@@ -167,7 +167,7 @@ public class SwerveDriveKinematics
double y = moduleStatesMatrix.get(i * 2 + 1, 0);
double speed = Math.hypot(x, y);
Rotation2d angle = new Rotation2d(x, y);
Rotation2d angle = speed > 1e-6 ? new Rotation2d(x, y) : m_moduleHeadings[i];
moduleStates[i] = new SwerveModuleState(speed, angle);
m_moduleHeadings[i] = angle;