Speed to angle PID tuned pretty well. Tried adding Serial to println debug from inside the library, but it made the robot start to oscillate a lot, so println in the library is commented out.