Update PathPlanning.c