PID loop optimization