Add PID algorithm