Recoded the initiation of path planning. Old way wont work properly.