hello ,

The target is to make an autonomous self navigated robot.The user will mark a path in Google Earth and save it as *.kml file. The absolute data(Latitude , Longitude) of each point will be sent to the vehicle via Serial Communication using an interface written in visual c++.After that the robot will enter a whole loop moving around, avoiding any obstacle in the path, until it will reach the final point.

the basic idea of the main loop with 1 point in the path is :

**(1):get coordinates of the point**

**loop**

**{**

**(2):get GPS data,**

**(3):find direction to move,**

**(4):move,**

**(5):if you are at the target point stop();**

**}**

The first thing that i thought I needed to do was to test every single hardware(GPS module, compass, motors) alone.

I did it and everything worked ok :

(1):I read GPS data in the Serial monitor with a ±5m accuracy.

(2):I read the current Angle in the Serial monitor.

(3):I made any available moves of the robot with the motors (forward, backwards, turn left, turn right) .

well, I as I need to put all together and make them work i started to make some tests trying to reach a point.I made the circuits and put them all to my ArduinoBT board and after a lot of tests I realized that most of times the robot doesn’t take the correct direction to move.

I thought that the precision of the GPS is the problem as the heading (in which direction i need to go?) is being calculated with the current GPS data (lat , lon) and the point to reach data (lat, lon).Same thing happens in the distance calculation function where the returned value maybe will be always useless cause the GPS precision.

Note : sometimes the whole thing works fine (taking the correct direction, moving and finally it stops nearby to the reach point).that’s why i am suspicious about the GPS precision.

The functions I am using to calculate the heading, the distance from the the point to reach and the whole navigation are the following :

```
int calc_heading(float flat1, float flon1, float flat2, float flon2)
{
float a;
float b;
float heading;
float diflon;
flat1 = radians(flat1);
flat2 = radians(flat2);
diflon = radians((flon2)-(flon1));
a = sin(diflon)*cos(flat2);
b = cos(flat1)*sin(flat2)-sin(flat1)*cos(flat2)*cos(diflon);
a = atan2(a,b);
heading = degrees(a);
if(heading < 0){heading=360+head_calc; }
return heading;
}
float calc_distance(float flat1, float flon1, float flat2, float flon2)
{
float diflon;
float dist;
float a;
float b;
int R = 6371000;
flat1 = radians(flat1);
flat2 = radians(flat2);
diflon = radians((flon2)-(flon1));
a = sin(flat1)*sin(flat2);
b = cos(flat1)*cos(flat2)*cos(diflon);
dist = acos(a+b)*R;
return dist;
}
void get_direction(float lat1, float lon1, float lat2, float lon2)
{
int currentAngle = get_compass_angle();
int heading = calc_heading(lat1,lon1,lat2,lon2);
while(abs(currentAngle-heading) > 8)
{
if((currentAngle - heading < 0 && abs(currentAngle - heading) < 180) || (currentAngle - heading > 0 && abs(currentAngle - heading) > 180))
{
stopmoving();
turnRight();
}
else
{
stopmoving();
turnLeft();
}
currentAngle = get_compass_angle();
heading = calc_heading(lat1,lon1,lat2,lon2);
Serial.print("Heading : ");Serial.print(heading);Serial.print(" , Current angle : ");Serial.print(currentAngle);
Serial.print(" Angle difference:"); Serial.println(abs(currentAngle-heading));
}
stopmoving();
}
void navigation(float flat1, float flon1, float flat2, float flon2)
{
if(calc_distance(flat1, flon1, flat2, flon2) < 2) stopmoving();
else
{
get_direction(flat1, flon1, flat2, flon2);
forward();
}
}
void loop()
{
Serial.println("loop Started !!");
bool newdata = getDataFromGPS();
if (newdata)
{
navigation(CurrentLat, CurrentLon, LatToReach, LonToReach);
}
}
```

I don’t post the whole code cause it’s so big … I just want to see if I have right in the previous thoughts about the GPS precision and if you can give me some ideas so I can find the solution in my problem !

Thank you in advance .