GPS Navigation! part 2

This is the second part of the GPS navigation code.

Thanks again everyone!

By the way, the GPS module is the popular EM 406A, available from SparkFun, and the compass, in this code, is a not tilt compensated compass from SparkFun as well. The obstacle avoidance is from MaxSonar.

//-------------------------------------------------------------------------------------turn right
if(turn==8){
rightturn();
}
//------------------------------------------------------------------------------------------turn left
if(turn==5){
leftturn();
}

Serial.println("distance"); 
Serial.println(dist_calc);
if(dist_calc<4){             //this goes after the serial print of the distance in the function "distance". refer to my code. 4=radius
if(waycont==waypoints){   //this has to do with switching waypoints
  done();     //go to the function to stop motors
}
waycont+=1;
}

}


void avoidObstacle()
{
int l1, l2, r1, r2; //variables to store distance output.

for (int i =0; i <180; i+30){ //turn servo to degree and take reading, assign properly.
  servo.write(i);
  delay(700);
  pinMode(pwPin, INPUT);
  pulse = pulseIn(pwPin, HIGH);
  cm = (pulse/147) * 2.54; 
  if (i==30)
  l1=cm;
  else if (i==60)
  l2=cm;
  else if (i==120)
  r1=cm;
  else {r2=cm;}}
 
 servo.write(90); //at the end, set the servo back to looking straight.
 
 
int maxl, maxr; //max distance on right and left sides.

maxl= max(l1,l2);
maxr=max(r1, r2);
if (l1==maxl) //turn angles.
  turnl=30;
  else if (l2==maxl)
  turnl=60;
  else if (r1==maxr)
  turnr= 30;
  else {turnr=150;}
  
  previousMillis=millis(); //current time so that the robot can then go straight for one second after it turns.
  
  if (maxl>maxr) //turn left or right, whichever has more distance to an obstalce.
  obstacleTurnl();
  else if (maxl=maxr)
  obstacleTurnr();
  else {obstacleTurnr();}
}

void obstacleTurnl()
{
  unsigned long currentmillis=millis();
int turnAngle=0;
turnAngle=turnl+headingcompass;
if (turnl+headingcompass>360)
  turnAngle=turnl-(360 -headingcompass);
  
  if(headingcompass+2>=turnl){ //Accuracy to within two degrees
  if(headingcompass-2<=turnl){
  while(currentmillis - (previousMillis-interval)<interval+1000) {//previous millis should not change, but current millies should be updated each time until one second is reached.
  currentmillis=millis(); 
   digitalWrite(mo1, HIGH); //go straight
    digitalWrite(mo2, LOW);
      digitalWrite(mo3, HIGH);
        digitalWrite(mo4, LOW);
  }
    return;
  }
}
   
  digitalWrite(mo1, LOW); //turn left
  digitalWrite(mo2, HIGH);
  digitalWrite(mo3, HIGH);
  digitalWrite(mo4, LOW);
  
  Wire.beginTransmission(slaveAddress);        //the wire stuff is for the compass module
  Wire.send("A");              
  Wire.endTransmission();
  delay(10);                  
  Wire.requestFrom(slaveAddress, 2);       
  i = 0;
  while(Wire.available() && i < 2)
  { 
    headingData[i] = Wire.receive();
    i++;
  }
  headingValue = headingData[0]*256 + headingData[1];  
headingcompass = headingValue / 10; 
obstacleTurnl();
}

void obstacleTurnr()
{
  unsigned long currentmillisR=millis();
 int turnAngleR;
 turnAngleR=headingcompass-turnr;
if (headingcompass-turnr<0)
turnAngleR = 360-(turnr-headingcompass);

if (headingcompass +3>turnAngleR){ //again, to within two degrees
  if (headingcompass-3<turnAngleR){
    while(currentmillisR - (previousMillis-interval)<interval+1000) {
  currentmillisR=millis(); 
     digitalWrite(mo1, HIGH);
    digitalWrite(mo2, LOW);
      digitalWrite(mo3, HIGH);
        digitalWrite(mo4, LOW);
    }
    return;
  }
}


 digitalWrite(mo1, HIGH); //turn right
  digitalWrite(mo2, LOW);
  digitalWrite(mo3, LOW);
  digitalWrite(mo4, HIGH);


  Wire.beginTransmission(slaveAddress);        //the wire stuff is for the compass module
  Wire.send("A");              
  Wire.endTransmission();
  delay(10);                  
  Wire.requestFrom(slaveAddress, 2);       
  i = 0;
  while(Wire.available() && i < 2)
  { 
    headingData[i] = Wire.receive();
    i++;
  }
  headingValue = headingData[0]*256 + headingData[1];  
headingcompass = headingValue / 10; 
obstacleTurnr();
}


void done(){ //stop once the last waypoint is reached. 
digitalWrite(mo1, LOW);
digitalWrite(mo2, LOW);
digitalWrite(mo3, LOW);
digitalWrite(mo4, LOW);
      done();
}



void rightturn(){ //turn right from GPS and compass info
if(headingcompass+2>=heading){
  if(headingcompass-2<=heading){
   digitalWrite(mo1, HIGH);
    digitalWrite(mo2, LOW);
      digitalWrite(mo3, HIGH);
        digitalWrite(mo4, LOW);
    return;
  }
}
      x4=headingcompass-heading;  
if(x4<-180){
return;                           //check to see if we have turned too much and now need to go left

}
if(x4>=0){
  if(x4<180){
  return;                //check to see if we have turned too much and now need to go left
  }
}
 
  digitalWrite(mo1, HIGH);           //motor turning code
    digitalWrite(mo2, LOW );   
      digitalWrite(mo3, LOW );
        digitalWrite(mo4, HIGH); 
         Wire.beginTransmission(slaveAddress);        //the wire stuff is for the compass module
  Wire.send("A");              
  Wire.endTransmission();
  delay(10);                  
  Wire.requestFrom(slaveAddress, 2);       
  i = 0;
  while(Wire.available() && i < 2)
  { 
    headingData[i] = Wire.receive();
    i++;
  }
  headingValue = headingData[0]*256 + headingData[1];  
headingcompass = headingValue / 10;      // this is the heading of the compass
rightturn();
}


void leftturn(){ //turn left from gps and compass info. 
  if(headingcompass+2>=heading){
  if(headingcompass-2<=heading){
   digitalWrite(mo1, HIGH);
    digitalWrite(mo2, LOW);
      digitalWrite(mo3, HIGH);
        digitalWrite(mo4, LOW);
    return;
  }
}
      x4=headingcompass-heading;  
if(x4>=-180){
  if(x4<=0){
     return;         
  }
}

if(x4>=180){     
  return;
}

  digitalWrite(mo1, LOW);
    digitalWrite(mo2, HIGH);
      digitalWrite(mo3,HIGH);
        digitalWrite(mo4,LOW ); 
         Wire.beginTransmission(slaveAddress);        //the wire stuff is for the compass module
  Wire.send("A");              
  Wire.endTransmission();
  delay(10);                  
  Wire.requestFrom(slaveAddress, 2);       
  i = 0;
  while(Wire.available() && i < 2)
  { 
    headingData[i] = Wire.receive();
    i++;
  }
  headingValue = headingData[0]*256 + headingData[1];  
headingcompass = headingValue / 10;      // this is the heading of the compass
leftturn();
}