Bugfixes and adding more status display to queue

-Fixed bug where the error in heading wasn't calculated correctly.
-Rewrote the PID controller to be better suited for wheels (removes
wheel jitters).
-Tweaked the brake function to account for velocity, throttling down
breaking earlyer if driving fast and smoother overall.