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)