Help with SDS MK3 and WPILib's Odometry

Situation:
We are using MK3’s and the SDS’s swerve-lib. We are trying to get it to work with the WPILib’s Swerve Odometry class.

Issue:
The odometry works well when all the swerve modules are facing the correct way. For example, driving forward when all the modules’ steering angles are 0 and the velocities are positive causes the x value to increase (as expected). However whenever the modules try to optimize the rotation by steering to 180 degrees and reversing the velocity, the x value in the odometry will decrease when driving forward instead.

Code:

Has anyone had any luck with swerve-lib and WPILib’s odometry? We’d love to see some examples or get some suggestion. Thanks.

Our team has mk3s and below is our swerve template code. I’m not too confident with sds libraries, but my initial read says that you have to override the getSteerAngle code with something that makes sense with your setup. This value (at bottom of your awervedrivesunsystem.java) informs odometry whether or not to rotate your velocity around.

Print that method and see if ur getting any values other than zero.

We can print out the values of module.getSteerAngle() and module.getDriveVelocity(). They are what we expect. When driving forward we’ll either have 0 degrees and positive velocity or 180 degrees and negative velocity. This is what we are passing into the odometry update method. Is this correct?

Sounds fine. Just to be explicitly clear, are your velocities showing negatives when your module angles are 180?

Yes, that’s correct.

We discovered the issue.

Issue:
Swerve-Lib’s getSteerAngle() returns a radian value rather than a degree value.

Fix:
Use new Rotation2d(module.getSteerAngle()) rather than Rotation2d.fromDegrees(module.getSteerAngle()).

Explanation for unexpected behavior:
We were treating the module’s steer angle as degrees while it was actually in radians. Pi radians was treated as degrees would be 3.14 degrees (pretty close to zero), but the velocity was negative when moving forward. This is opposite of how modules typically behave, as 0 steer angle would have a positive velocity when moving forward. When half our modules are reversed, this cancels out in the odometry class.

3 Likes

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.