PID: Add basic PID algorithm, initializations, and control functions