Change PID algorithm