Implement the PID algorithm