Some Code
Hi, I'm the builder of Deathpod3000, which was referenced in a previous post. I'm in the process of writing some new articles on my build blog with code samples and thought I'd share them here as well. OP didn't ask for distance, but for navigation, this is pretty useful. I use the haversine formula as other formulas don't work well for 8 bit micros and small distances. You also could use simpler trig functions and ignore the curvature of the earth for small distances. I used an Atmel AVR so this is in C, but should translate easily.
Code:
float calcDistance(float lat1, float lon1, float lat2, float lon2)
{
float dlon, dlat, a, c;
float dist = 0.0;
lon1 = lon1 * -1.0; //make west = positive
lon2 = lon2 * -1.0;
dlon = lon2 - lon1;
dlat = lat2 - lat1;
a = pow(sin(dlat/2),2) + cos(lat1) * cos(lat2) * pow(sin(dlon/2),2);
c = 2 * atan2(sqrt(a), sqrt(1-a));
dist = 20925656.2 * c; //radius of the earth (6378140 meters) in feet 20925656.2
return(dist);
}
Lat and lon in the above function must be in radians. I wrote 2 small functions to convert between degrees for display and radians for calculations.
Code:
float dtor(float degrees)
{
return(degrees * PI / 180);
}
float rtod(float radians)
{
return(radians * 180.0 / PI);
}
You can read more about the haversine formula and other method of calculating distance and bearing here:
http://www.movable-type.co.uk/scripts/latlong.html
The code I use to calculate bearing (again lat and lon must be in radians) is:
Code:
float calcBearing(float lat1, float lon1, float lat2, float lon2)
{
float bearing = 0.0;
//determine angle
bearing = atan2(sin(lon2-lon1)*cos(lat2), (cos(lat1)*sin(lat2))-(sin(lat1)*cos(lat2)*cos(lon2-lon1)));
//convert to degrees
bearing = rtod(bearing);
//use mod to turn -90 = 270
bearing = fmod((bearing + 360.0), 360);
return bearing;
}
Be careful with the order of the parameters for atan2 as the avr gcc libs and Excel take the parameters in opposite order and this messed me up at first. This function takes parameters in radians and returns bearing in degrees. To convert negative values to correct positive 0-360 degree values, 360 is added to the bearing and then mod 360 (remainder when divided by 360).
I also created a relative bearing function so after computing bearing and reading my digital compass I could figure out which direction to turn and how many degrees.
Code:
float CalcRelBearing(float bearing, float heading)
{
float relBearing;
relBearing = bearing - heading;
if(relBearing > 180.0)
{
relBearing = bearing - (heading + 360);
}
if(relBearing < -180.0)
{
relBearing = bearing - (heading - 360);
}
return(relBearing);
}
Bearing is where you want to go and heading is where you are going now. These are in degrees. The function returns a value of -180 - +180 for left 0-180 degrees or right 0-180 degrees. I then used a proportional function to send appropriate servo pulses to the steering servo to turn the car.
Lookup tables would work for rough values of some of the trig functions if they aren't available. I used that method on a previous rover where the floating point stuff took too long. I thought about using fixed point and lookup tables, but decided to try the float stuff and see how it performed. On an Atmega32 at 8mhz, using the free gcc C compiler, I was getting 5-10 times through my main navigation loop per second, which seemed plenty fast so I just stuck with the floating point math as the trig fuctions were build into the compiler library so it was easy to implement.
Deathpod3000 was one of only 2 vehicles, and the only ground vehicle, to complete the course at the recent Sparkfun Autonomous Vehicle Competition. The other vehicle was an airplane. Airplane > car in a speed race so we got second place, but also got the Engineer's Choice award. I have many details about the construction and programming of the car and pics and videos from the competition on my build blog here:
http://deathpod3000.wordpress.com
Check it out if you're interested.
Eric
!