Added a straight distance heuristic. Now need to fix the path-following PID controller. Is too jerky.