Updated combined code and astar plan to destination