Go Down

### Topic: Arduino follow me with GPS (Read 2075 times)previous topic - next topic

#### Cricri

##### Dec 08, 2017, 11:49 am
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:

Code: [Select]
`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:
Code: [Select]
`  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)

#### jremington

#1
##### Dec 08, 2017, 05:34 pmLast Edit: Dec 08, 2017, 05:44 pm by jremington
Quote
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.

#### Cricri

#2
##### Dec 09, 2017, 04:39 pm
Hi jremington, thanks for the answer!

Quote
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:

Code: [Select]
`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?

#### jremington

#3
##### Dec 09, 2017, 05:09 pmLast Edit: Dec 09, 2017, 05:09 pm by jremington
Quote
Could you expain me this?
Not without seeing the code and the results.

Sorry, I'm not a mind reader.

#### Cricri

#4
##### Dec 09, 2017, 05:25 pmLast Edit: Dec 09, 2017, 06:44 pm by Cricri
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:

Code: [Select]
`Point 1: 50 21 59N ,  4 8 2WPoint 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.

#### Cricri

#5
##### Dec 09, 2017, 11:43 pm
SOLVED! In the bottom of the page I have found an excel file with the formulas, and what it is shown in it:
Code: [Select]
`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!

#### jremington

#6
##### Dec 10, 2017, 01:04 am
Code: [Select]
`(-9.874 +360) % 360 = 0.03 (where % is the division's remainder)`
The % operator does not work with float variables.