Is more accurate now, finds the goal quicker. I calibrated the computation of the heading angle and the necessary turn angle to get to the goal