We have found that a simple piece wise function is accurate enough to use for rotating to angles. We have a tolerance of two degrees. It works very well. We call this little set of functions Tigerdrive.
Here is a link: it is probably more simple to implement.
https://github.com/team2053tigertron...src/TigerDrive