Setting a constant heading for use with M1, since I don't have a path datastructure yet.
Created rough outline of updateNav function.
Wrote the correction finding algorithm. Correctness dubious at best.
Added basic PID controller support. Still needs support for controller state.
Added correction factor.

See comment.
Fixed call to findCorrection
Added derivative support to PID Controller
Merge pull request #9 from soderstroff/develop

Milestone 1 Commits