Implement PID control

Will need to test extensively to find appropriate values of Kp, Ki and Kd