Arduino follow me with GPS

I'm making a project with the purpose to build a robot that follow me. In order to do this, I began from basics, so I made an app for android with four buttons to control the direction of the robot via WiFi using an esp8266-01 connected to arduino, which is also connected with two motors, and it worked.
Then I have added to this app the location listener and compass reading, placing one smartphone on the robot and another in my hand, which send latitude, longitude and compass angle to arduino, that use them to determinate how long it has to go and to which direction it has to turn. The calculation of the distance between the two works approximately well, whith an error of about 6 meters, but I have trouble with the direction. At the beginning I stupidly used this formula:

float y = latPhone - latArd;
  float x = lonPhone - lonArd;
  dirPhone = 2 * atan( y / ( sqrt(x * x + y * y) + x)); //value in Radians
  dirPhone = dirPhone * 180 / M_PI; //to degree

But then I have thought "Hey, but Lat and Lon are not catetes but angles!" So I searched and found this formula from an HackerHouse project, where there is a cooler that follow a person, that do exactly what I want:

  float y = sin(lonPhone-lonArd) * cos(latPhone);
  float x = cos(latArd)*sin(latPhone) - sin(latArd)*cos(latPhone)*cos(lonPhone-lonArd);
  difDir = atan2(y, x) * 180 / M_PI;

but it does not worked...! Has someone tryied to do something similar and can enlight me where I mistake? Thanks. (In attachment the Arduino sketch which I use)

pi_Argomenti.ino (8.02 KB)

but it does not worked...!

Post the actual input and output numbers.

Over short distances, use the Pythagoras theorem (the "equirectangular approximation") to calculate distances. On AVR-based Arduinos, double is the same as float. The error in single precision Arduino floating point calculations is too large to use spherical trigonometry.

Excellent reference on navigation formulas here.

Hi jremington, thanks for the answer!

Post the actual input and output numbers.

The error is that while I was testing it printed"turn left" and not "turn right" or the reverse.

Many thanks for the link, it's very helpful!! But I have a question. In the "Rhumb-line distance between two points" calculation, I wached the JavaScript code of the page and I have found this code:

LatLon.prototype.rhumbBearingTo = function(point) {
    if (!(point instanceof LatLon)) throw new TypeError('point is not LatLon object');

    var φ1 = this.lat.toRadians(), φ2 = point.lat.toRadians();
    var Δλ = (point.lon-this.lon).toRadians();
    // if dLon over 180° take shorter rhumb line across the anti-meridian:
    if (Δλ >  Math.PI) Δλ -= 2*Math.PI;
    if (Δλ < -Math.PI) Δλ += 2*Math.PI;

    var Δψ = Math.log(Math.tan(φ2/2+Math.PI/4)/Math.tan(φ1/2+Math.PI/4));

    var θ = Math.atan2(Δλ, Δψ);

    return (θ.toDegrees()+360) % 360;
};

But if I try to calculate the bearing with this formula, I obtain different result from the calculator on the page! Could you expain me this?

Could you expain me this?

Not without seeing the code and the results.

Sorry, I'm not a mind reader.

I have not copied the code because I have not tryed it with Arduino but with the calculator ! So, in the page with in input:

Point 1:	
50 21 59N ,  4 8 2W

Point 2:	
42 21 4N ,  71 2 27W

The bearing is 260° 07′ 38″ . Now, if you convert every angle in radians you'll obtain :

Lat 1 : 0.8791

Lon 1 : 0.07215

Lat 2 : 0.7392

Lon 2 : 1.2399

Now, if using Wolfram you do the following operations, you'll obtain:

lon2 - lon1 = 1.16775 , which the site call Δλ ;

log(tan(0.7392/2+PI/4)/tan(0.8791/2+PI/4)) = -0.203252 , the Δψ of the site;

arctan(Δλ, Δψ) = -0.172328 ( θ ) ;

in degree : -9.874 ;

(-9.874 +360) % 360 = 0.03 (where % is the division's remainder)

Maybe I have copied the wrong formula (?)

Edit: I was thinking, do you know if this formula is valid for short distances? I'll search it.

SOLVED! In the bottom of the page I have found an excel file with the formulas, and what it is shown in it:

ARCTAN.2(COS(lat1)*SEN(lat2)-SEN(lat1)*COS(lat2)*COS(lon2-lon1); SEN(lon2-lon1)*COS(lat2))

and it worked!!! Tomorrow I'll test it on Arduino! Many thanks again for the link!

(-9.874 +360) % 360 = 0.03 (where % is the division's remainder)

The % operator does not work with float variables.

Hie there.I'm also working on same project. I'm using neo6m gps and qmc5883l compass which is fake of hmc5883l. i can't solve the programing problem.please send me your codes if u done this project. Thanks