Attempt to fix angle error calculations

[AJahueyM]

63

We have been using your library for our robot! We have really liked it. We are using a tank chassis and found out an error with the Ramsete Follower, it only appeared when we started from 0,0 and went into some target with negative Y coordinate.

Making a custom Ramsete follower that does the following when caluclating the error solved the issue:

    var angleError = targetPose.heading - currentPose.heading

    if(angleError.absoluteValue > PI){
        angleError -= 2 * PI * angleError.sign
    }

    val error = Pose2d(targetPose.x - currentPose.x, targetPose.y - currentPose.y, angleError)

I looked into the implementation of the minus operation for the Pose2d and saw that it simply substracts one angle from the other. Maybe implementing something like we showed could fix it?

[rbrott]

Thanks for the issue and fix. I decided to a different route with the fix that preserves the current pose subtraction semantics. I made the early misstep of co-opting Pose2d for pose velocities, accelerations, differences, and other quantities which means standard operations treat it as a 3-vector.