Updates on PID controller

The Trapezoidal Rule function was changed to accept variables to become
more universal; its is now being used to integrate/dead-reckon both the
angles and the error calculated by the PID loop
Functions were created to automatically create the PID constants once
the ultimate gain is found; see Ziegler-Nichols Tuning method
Added the integral component to the controller