AStar working
going to merge
robot working code - comp heat: 3
Final code for competition