GPS Navigation! part 2

this second part of gps navigation code.

thanks again everyone!

by way, gps module popular em 406a, available sparkfun, , compass, in code, not tilt compensated compass sparkfun well. obstacle avoidance maxsonar.

code: [select]


//-------------------------------------------------------------------------------------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 serial print of distance in function "distance". refer code. 4=radius
if(waycont==waypoints){   //this has switching waypoints
 done();     //go function stop motors
}
waycont+=1;
}

}


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

for (int =0; <180; i+30){ //turn servo degree , 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 end, set servo looking straight.


int maxl, maxr; //max distance on right , 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 robot can go straight 1 second after turns.
 
 if (maxl>maxr) //turn left or right, whichever has more distance 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 within 2 degrees
 if(headingcompass-2<=turnl){
 while(currentmillis - (previousmillis-interval)<interval+1000) {//previous millis should not change, current millies should updated each time until 1 second 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 compass module
 wire.send("a");              
 wire.endtransmission();
 delay(10);                  
 wire.requestfrom(slaveaddress, 2);      
 i = 0;
 while(wire.available() && < 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, within 2 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 compass module
 wire.send("a");              
 wire.endtransmission();
 delay(10);                  
 wire.requestfrom(slaveaddress, 2);      
 i = 0;
 while(wire.available() && < 2)
 {
   headingdata[i] = wire.receive();
   i++;
 }
 headingvalue = headingdata[0]*256 + headingdata[1];  
headingcompass = headingvalue / 10;
obstacleturnr();
}


void done(){ //stop once last waypoint reached.
digitalwrite(mo1, low);
digitalwrite(mo2, low);
digitalwrite(mo3, low);
digitalwrite(mo4, low);
     done();
}



void rightturn(){ //turn right gps , 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 see if have turned , need go left

}
if(x4>=0){
 if(x4<180){
 return;                //check see if have turned , need go left
 }
}

 digitalwrite(mo1, high);           //motor turning code
   digitalwrite(mo2, low );  
     digitalwrite(mo3, low );
       digitalwrite(mo4, high);
        wire.begintransmission(slaveaddress);        //the wire stuff compass module
 wire.send("a");              
 wire.endtransmission();
 delay(10);                  
 wire.requestfrom(slaveaddress, 2);      
 i = 0;
 while(wire.available() && < 2)
 {
   headingdata[i] = wire.receive();
   i++;
 }
 headingvalue = headingdata[0]*256 + headingdata[1];  
headingcompass = headingvalue / 10;      // heading of compass
rightturn();
}


void leftturn(){ //turn left gps , 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 compass module
 wire.send("a");              
 wire.endtransmission();
 delay(10);                  
 wire.requestfrom(slaveaddress, 2);      
 i = 0;
 while(wire.available() && < 2)
 {
   headingdata[i] = wire.receive();
   i++;
 }
 headingvalue = headingdata[0]*256 + headingdata[1];  
headingcompass = headingvalue / 10;      // heading of compass
leftturn();
}

Arduino Forum > Forum 2005-2010 (read only) > Software > Syntax & Programs > GPS Navigation! part 2

arduino