Changed to a struct style for all logging. Gives easier post processing.
Changed the heading controller to dump state all the time for checking.
Added watchdogs to everything.
mode.
Added a heading controller.
Changed the PID controller to work on error so the heading controller
can handle the wrap around.
Support deadbands in both directions.