Fixed issue when robot returned to staring position and got confused