CS373: new PID controller
CS373: new twiddle algorithm

* Optimizes PID parameters