PID control algorithm