用Arduino制作超声波避障机器人

功能说明：

自动机器人自动行走前进，当前方有障碍物时，能够自动判断一条可以安全前进的路线，使机器人不会碰到任何障碍物

#include 
Servo servo;
const int sig=2;
const int negR=7;
const int posR=8;
const int negL=12;
const int posL=13;
const int pwmR=5;
const int pwmL=6;
const int Rspeed=120;
const int Lspeed=130;
const int rotSpeed=150;
unsigned long Rdistance;
unsigned long Ldistance;
unsigned long Cdistance;
void setup()
{
  pinMode(posR,OUTPUT);
  pinMode(negR,OUTPUT);
  pinMode(posL,OUTPUT);
  pinMode(negL,OUTPUT); 
  servo.attach(3); 
  servo.write(90);
}

void loop() 
{
  servo.write(90);
  delay(500);
  Cdistance=ping(sig);  
  if(Cdistance<25)
  {
    pause(0,0);    
    servo.write(45);
    delay(500);
    Rdistance=ping(sig);  
    servo.write(135);
    delay(500);
    Ldistance=ping(sig);         
    servo.write(90); 
    if(Rdistance<25 && Ldistance<25)
    {
      back(Rspeed,Lspeed);
      delay(2000);
      right(rotSpeed,rotSpeed);
      delay(500);
      forward(Rspeed,Lspeed);
    }      
    else if(Rdistance>Ldistance)
    {
      right(rotSpeed,rotSpeed);
      delay(500);
    }
    else if(Ldistance>Rdistance)
    {
      left(rotSpeed,rotSpeed);
      delay(500);  
    }    
  } 
  else
    forward(Rspeed,Lspeed);  
}  
int ping(int sig)
{
   unsigned long cm,duration;
   pinMode(sig,OUTPUT);
   digitalWrite(sig,LOW);
   delayMicroseconds(2);
   digitalWrite(sig,HIGH);
   delayMicroseconds(5);
   digitalWrite(sig,LOW);
   pinMode(sig,INPUT);
   duration=pulseIn(sig,HIGH);
   cm=duration/29/2;
   return cm;
}  
void forward(byte RmotorSpeed, byte LmotorSpeed)
{
    analogWrite(pwmR,RmotorSpeed);
    analogWrite(pwmL,LmotorSpeed);
    digitalWrite(posR,HIGH);
    digitalWrite(negR,LOW);         
    digitalWrite(posL,LOW);
    digitalWrite(negL,HIGH);   
}  
void back(byte RmotorSpeed, byte LmotorSpeed)
{
    analogWrite(pwmR,RmotorSpeed);
    analogWrite(pwmL,LmotorSpeed);
    digitalWrite(posR,LOW);
    digitalWrite(negR,HIGH);         
    digitalWrite(posL,HIGH);
    digitalWrite(negL,LOW);   
} 
void pause(byte RmotorSpeed, byte LmotorSpeed)
{
    analogWrite(pwmR,RmotorSpeed);
    analogWrite(pwmL,LmotorSpeed);
    digitalWrite(posR,LOW);
    digitalWrite(negR,LOW);         
    digitalWrite(posL,LOW);
    digitalWrite(negL,LOW);   
} 
void right(byte RmotorSpeed, byte LmotorSpeed)
{
    analogWrite(pwmR,RmotorSpeed);
    analogWrite(pwmL,LmotorSpeed);
    digitalWrite(posR,LOW);
    digitalWrite(negR,LOW);         
    digitalWrite(posL,LOW);
    digitalWrite(negL,HIGH);        
} 
void left(byte RmotorSpeed, byte LmotorSpeed)
{
    analogWrite(pwmR,RmotorSpeed);
    analogWrite(pwmL,LmotorSpeed);
    digitalWrite(posR,HIGH);
    digitalWrite(negR,LOW);         
    digitalWrite(posL,LOW);
    digitalWrite(negL,LOW);        
}