Add PID library and breakout sludge code

[Dane Gardner]

Break all of the loop() code out into a separate PID library that is easier to maintain. In the future we can look at auto-tuning PIDs and whatnot.