Fixed the step function of PathPlanning