PID control for autonomous mode