Fixed stepping issues with a threshold value; now movement is smooth even when you interrupt the current path.