GPS Navigation!

I am working on a project to autonomously navigate the desert, and I have added a section to some navigation code which deals with obstacle avoidance.

What I want this code to do, is to detect an obstacle using an ultrasonic sensor. Then once the obstacle is detected, I want a servo to run to four angle positions (30, 60, 120, 150) and at each position store the readout form the ultrasonic sensor to a separate variable. From this, the robot should determine the bet way to turn, and turn that way. Once it has turned to the angle at which the most distance to an obstacle was seen, then it will go straight for one second, and then go back to the lop for further navigation.

On a previous note I am sorry about the post with only a part of my sketch. This one will have all of the sketch posted.

The obstacle avoidance code was written by me, however the navigation code was taken from Credit goes to Patrick McCabe for developing it.

If you could please evaluate the code on what it does I would really appreciate it. Worst thing is when the code compiles but then does not do what you want it to do. I think that I have checked pretty thoroughly though.

The code will be in two posts because it does not fit other wise.

//Because I did not write the nav code, nor do I know enough about writing it,  cannot make any additional comments to what was provided.
#include <Servo.h> 
#include <NewSoftSerial.h>
#include <TinyGPS.h>
#include <Wire.h>
int HMC6352Address = 0x42;
int slaveAddress;
byte headingData[2];
int i, headingValue;
float flat, flon, x2lat, x2lon;
float heading=0;

int headingcompass;
TinyGPS gps;
NewSoftSerial nss(2, 3);
Servo servo;
int turnl, turnr; //turn agnles for obstacle avoidance
long previousMillis = 0; //time to go straight after obstacle avoidance turn
long interval=1500;   //how long to go forward

#define mo1 4 //pins for motor controller. The motor driver will be hooked up to these 4 pins
#define mo2 5
#define mo3 6
#define mo4 7
#define enablePin_a 8 /*Enable the motors on one side or the other side. I am going to connect 
                        two motors to each channel. motors 1 and 2 are left side, 3 and 4 are right side.*/
#define enablePin_b 10
int x4=0; 
#define waypoints 3

int waycont=1;
float flat2=35.217779357910175;   //waypoint 1    //28.324603,-82.267419 home testing
float flon2=-111.60044610500336;
float flat3=35.21742217953794;        //waypoint 2
float flon3=-111.60140633583069;
float flat4=35.21679547012954;       //waypoint 3
float flon4=-111.60106301307678;
/*float flat5=28.3414192;     // waypoint   4
float flon5=-82.2741470;
float flat6 = 28.3413600;
float flon6 =-82.2739028;*/

const int pwPin = 11; //MaxSonar input pin
long pulse, cm; //MaxSonar variables

void gpsdump(TinyGPS &gps);
bool feedgps();
void printFloat(double f, int digits = 2);

void setup()
  slaveAddress = HMC6352Address >> 1;  

pinMode(mo1, OUTPUT);
    pinMode(mo2, OUTPUT);
        pinMode(mo4, OUTPUT);
          pinMode(enablePin_a, OUTPUT);
          digitalWrite(enablePin_a, HIGH);
          pinMode(enablePin_b, OUTPUT);
          digitalWrite(enablePin_b, HIGH);

void loop()
   pinMode(pwPin, INPUT);
  pulse = pulseIn(pwPin, HIGH);
  cm = (pulse/147) * 2.54;
  bool newdata = false;
  unsigned long start = millis();

  // Every fourth of a second we print an update
  while (millis() - start < 250)
    if (feedgps())
      newdata = true;
  if (newdata)
    Serial.println("Acquired Data");

void printFloat(double number, int digits)
  // Handle negative numbers
  if (number < 0.0)
     number = -number;

  // Round correctly so that print(1.999, 2) prints as "2.00"
  double rounding = 0.5;
  for (uint8_t i=0; i<digits; ++i)
    rounding /= 10.0;
  number += rounding;

  // Extract the integer part of the number and print it
  unsigned long int_part = (unsigned long)number;
  double remainder = number - (double)int_part;

  // Print the decimal point, but only if there are digits beyond
  if (digits > 0)

  // Extract digits from the remainder one at a time
  while (digits-- > 0)
    remainder *= 10.0;
    int toPrint = int(remainder);
    remainder -= toPrint; 

void gpsdump(TinyGPS &gps)
  long lat, lon;
  float flat, flon;
  unsigned long age, date, time, chars;
  unsigned short sentences, failed;

  gps.get_position(&lat, &lon, &age);
  Serial.print("Lat/Long(10^-5 deg): "); Serial.print(lat); Serial.print(", "); Serial.print(lon); 
  Serial.print(" Fix age: "); Serial.print(age); Serial.println("ms.");
  feedgps(); // If we don't feed the gps during this long routine, we may drop characters and get checksum errors

  gps.f_get_position(&flat, &flon, &age);
  Serial.print("Lat/Long(float): "); printFloat(flat, 5); Serial.print(", "); printFloat(flon, 5);
  Serial.print(" Fix age: "); Serial.print(age); Serial.println("ms.");


  gps.stats(&chars, &sentences, &failed);
  Serial.print("Stats: characters: "); Serial.print(chars); Serial.print(" sentences: "); Serial.print(sentences); Serial.print(" failed checksum: "); Serial.println(failed);

/*********************************///Object detection, if more than 25cm then go to the obstacle avoid function to determine what to do. 
 if (cm>25){ //25cm was chosen based on the minimum turn angle, 30 degrees, and maximum size of obstalces.
 else {
bool feedgps()
  while (nss.available())
    if (gps.encode(
      return true;
  return false;

void distance()
{      //this function will do all the calculations

 x2lat = flat2;      // setting x2lat and x2lon equal to our first waypoint
 x2lon = flon2;   
    x2lat = flat3;
    x2lon = flon3;
    x2lat = flat4;
    x2lon = flon4;
  }/*  //not using waypoints 4 and 5 now.
  x2lat = flat5;
  x2lon = flon5;
  x2lat = flat6;
  x2lon = flon6;

float flat1=flat;     // flat1 = our current latitude. flat is from the gps data. 
float flon1=flon;  // flon1 = our current longitude. flon is from the fps data.

float dist_calc=0;
float dist_calc2=0;
float diflat=0;
float diflon=0;

//------------------------------------------ distance formula below. Calculates distance from current location to waypoint
diflat=radians(x2lat-flat1);  //notice it must be done in radians
flat1=radians(flat1);    //convert current latitude to radians
x2lat=radians(x2lat);  //convert waypoint latitude to radians
diflon=radians((x2lon)-(flon1));   //subtract and convert longitudes to radians
dist_calc = (sin(diflat/2.0)*sin(diflat/2.0));
dist_calc2= cos(flat1);
dist_calc +=dist_calc2;
dist_calc*=6371000.0; //Converting to meters
Serial.println(dist_calc);    //print the distance in meters

flon1 = radians(flon1);  //also must be done in radians
 x2lon = radians(x2lon);  //radians duh.
heading = atan2(sin(x2lon-flon1)*cos(x2lat),cos(flat1)*sin(x2lat)-sin(flat1)*cos(x2lat)*cos(x2lon-flon1)),2*3.1415926535;
heading = heading*180/3.1415926535;  // convert from radians to degrees
int head =heading; //make it a integer now
  heading+=360;   //if the heading is negative then add 360 to make it positive
Serial.println(heading);   // print the heading.

Wire.beginTransmission(slaveAddress);        //the wire stuff is for the compass module
  Wire.requestFrom(slaveAddress, 2);       
  i = 0;
  while(Wire.available() && i < 2)
    headingData[i] = Wire.receive();
  headingValue = headingData[0]*256 + headingData[1];
 int pracheading = headingValue / 10;      // this is the heading of the compass
Serial.println("current heading:");

x4=headingcompass-heading;   //getting the difference of our current heading to our needed bearing
int turn;

//-------------------------------------- below tells us which way we need to turn

    turn=8;    //set turn =8 which means "right"         
    turn=5;      //set turn = 5 which means "left"
    turn=5;   //set turn = 5 which means "left"
if(x4>=180){     //set turn =8 which means "right"
int hd = headingcompass;
    turn=3;   //then set turn = 3 meaning go "straight"

digitalWrite(mo1, HIGH);               //go straight 
    digitalWrite(mo2, LOW);
      digitalWrite(mo3, HIGH);
        digitalWrite(mo4, LOW);

More distance function in next post.

I'm working on a similar project but using different libraries and a different layout in the code. Here are a few good projects to reference-

google "my robot makey" and find the "follower.pde" example. This one has some servo and ping interaction without the servo library.

next google "ttjcrew" and find their "GPSCar" sketch. They use the NMEA library which I think is easier.

I'll post my sketch when it's working exactly as intended. I'm close but I'm currently using a prety fast RC car chassis that has no speed control. It works but it just leaps forward in 1 meter increments.