Getting from Point A to Point B
So, as explained in the prior sections, we now have a way to make our car steer toward a particular direction and, by reading coordinates from a GPS module, figure out where our car is at at point in time. So, let's call where the car is at any given moment, Point A. Further, let's pick a different point a short distance away as where we want the car to drive to next and call this point B. So, how do we program the Arduino so it knows how to steer the car from Point A to Point B? Actually, it's not that hard. If we were to plot both points on a graph, it might look something like this:
This means the trick is simply to compute the heading angle relative to North and then use the compass to steer in that direction. As the car moves, it may not move in exactly the computed direction but, each time the car gets a new GPS location fix, it will do this calculation again, and again, causing the car to converge on the goal point, even if the car is knocked off course by some obstacle. But, how to compute the angle? Hmm, remember that class in High School where you learned all the stuff you knew you'd never need to know out in the real world... Yup, it's trigonometry time.
DX, DY and ATAN2()
Actually, your trig class may not have introduced you to the secret sauce that makes this work, the atan2() function, as it was first introduced for use in computer languages, such as FORTRAN. But, it's included in the Arduino's Math library, and it's just what we need to perform this calculation. For the first step, we compute values for DX and DY, like this:
DX = PointB.x - PointA.x
DY = PointB.y - PointA.y
Where PointA.x and PointB.x are longitude values, and PointA.y and PointB.y are latitude values. Then, we feed these values into the atan2() function, like this:
angle in radians = atan2(DY, DX)
Then, since the code driving the car is expecting angles in degrees with North as our zero (0) value, and increasing in a clockwise direction, we convert this value, ilke this:
angle in degrees = radians * (180 / Pi)
However, because this results in negative values for angles > 180 degrees, we need to make an adjustment to this value, like this:
angle = angle < 0 ? angle + 360 : angle
or:
angle = fmod(angle + 360, 360)
With this final correction, we can compute a value that varies from 0 - 359 degrees in a full clockwise rotation, with North equal to 0 degree, East equal to 90 degrees, etc. And, with a few modifications to the code in "Steering to a Heading", our car can now find its way back to a given GPS coordinate as long as driving a straight line will get it there. Naturally, there are still many more details to handle before the car is ready to drive on it's own. But, I'll cover those in future posts.
Equivalent TinyGPS Function
The astute reader will perhaps know that TinyGPS includes a function called course_to() that also does an equivalent calculation to what I used atan2() to do in the above code. I chose to write own code to compute the heading angle rather than use the one in TinyGPS. One reason I did this is because the code in course_to() performs a more complex calculation based on the Great Circle path that I felt was an unnecessary complication. Also, for reason that will start to become clear in the next section, I soon learned about a problem I needed to address that would require some additional changes to the code I described above.