Fixed aStar waypoint reset. Changed aStar to run as fast as possible.