RoadRunner Support

For you roadrunner people!

As this library uses the Quadrant 1 and Unit Circle format, there are additional steps you must do in order to convert the roadrunner position, so it's compatible to this library. But not to worry, they are included!

Pose Estimate

The pose estimate returned from the library is in Quadrant 1 format, so you must use the asRoadRunnerCoords() method.

So when you call tsl.poseEstimate, you must call it as: asRoadRunnerCoords(tsl.poseEstimate).

Or if you are assigning it from the update() method, you can call it as: asRoadRunnerCoords(tsl.update()).

Unit Circle

Inside the update() method, when using roadrunner, you must use the asUnitCircle() method for the heading parameter. This transforms the roadrunner heading system to the unit circle heading as follows:

tsl.update(left, front, right, asUnitCircle(heading))

Doing these methods will allow you to seamlessly adapt the RoadRunner library to this library. The full usage would look similar to this:

tsl = ThreeSensorLocalization(
    Pose(), Pose(), Pose(), 45.0
)
Pose2d newPose = asPose2d(tsl.update(left, front, right, asUnitCircle(robotTheta)))