新手求助

sam21408

我看著書的解說，設計一個超音波避障車，結果車子只會前進幾秒，後退幾秒，右後轉，一直重複，請問我哪裡打錯，我是新手海請各位高手幫幫忙。
#include <Servo.h>
Servo servo;
const int ECHOPIN=2;
const int TRIGPIN=4;
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=150;
const int Lspeed=150;
const int rotSpeed=180;
unsigned long Rdistance;
unsigned long Ldistance;
unsigned long Cdistance;
void setup()
{
  pinMode(posR,OUTPUT);
  pinMode(negR,OUTPUT);
  pinMode(posL,OUTPUT);
  pinMode(negL,OUTPUT);
  pinMode(TRIGPIN,OUTPUT);
  pinMode(ECHOPIN, INPUT);

  servo.attach(3);
  servo.write(90);
}

void loop()
{
  servo.write(90);
  delay(500);
  Cdistance=ping(TRIGPIN );  
  if(Cdistance<8)
  {
    pause(0,0);   

    servo.write(45);
    delay(500);
    Rdistance=ping(TRIGPIN);  
    servo.write(135);
    delay(500);
    Ldistance=ping(TRIGPIN);         
    servo.write(90);
    if(Rdistance<8 && Ldistance<8)
    {
      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)
{
   unsigned long cm,duration;

   digitalWrite(TRIGPIN,HIGH);
   delayMicroseconds(2);
   digitalWrite(TRIGPIN,HIGH);
   delayMicroseconds(10);
   digitalWrite(TRIGPIN,LOW);
   duration=pulseIn(ECHOPIN,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);        
}