Updated PID controller to have K_p & K_d constants