Miscellaneous code cleanup.
Added initial version of PID controller.
Fixed up some miscellaneous formatting.