We’re currently testing autonomous. We’ve been noticing that the wheels have been slightly off while testing a basic, straight auto path. We use a NavX and we have a .getPose function that we provide to PathPlanner’s AutoBuilder instance.
public Pose2d getPose() {
return odometry.getPoseMeters();
}
However, we’ve noticed that .getPoseMeters from WPI’s SwerveDriveOdometry class returns a value from -180 to 180. We would like to pass in a value into our swerve drive method within the range [0, 360] instead. WPI doesn’t seem to provide a way to change this and we’d like to know how.
If you could give us any suggestions, that would be great.
Also, if you think something else might be wrong, please let us know. If you need additional info, please reach out.
You don’t quite explain the reasoning for wanting to use [0, 360]. Mind elaborating on that? I suspect there may be a better solution than translating the Pose2d object
/**
* Normalize an angle to be within 0 to 360.
*
* @param angle Angle in degrees.
* @return Normalized angle in degrees.
*/
public static double normalizeAngle(double angle)
{
Rotation2d angleRotation = Rotation2d.fromDegrees(angle);
return new Rotation2d(angleRotation.getCos(), angleRotation.getSin()).getDegrees();
}