循跡自走車加上超音波避障，功能失常

csddd888

板子 arduino uno
問題:在未加上超音波(hcsr-04)前，紅外線5路循跡一切正常，裝上後超音波避障正常，可是循跡部分常常感應到，卻沒做出應該的反應，求解謝謝!
備註:把超音波部分先註解掉，循跡又恢復正常了

程式碼:
#include <Servo.h>
Servo servoLeft;
Servo servoRight;
int trigPin = 11; //Trig Pin
int echoPin = 10; //Echo Pin
long duration ,cm;

int rD,rB,rA,rC,rE,lt;
int redD=8;
int redB=7;
int redA=6;
int redC=5;
int redE=4;
int red=1;

void setup()
{
  Serial.begin(9600);   
  pinMode(redD,INPUT);
  pinMode(redB,INPUT);
  pinMode(redA,INPUT);
  pinMode(redC,INPUT);
  pinMode(redE,INPUT);

  pinMode(trigPin, OUTPUT);  //Define inputs and outputs
  pinMode(echoPin, INPUT);

  servoRight.attach(13);  
  servoLeft.attach(12);
}

void loop()
{
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);     // 給 Trig 高電位，持續 10微秒
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  pinMode(echoPin, INPUT);             // 讀取 echo 的電位
  duration = pulseIn(echoPin, HIGH);   // 收到高電位時的時間
  
  cm = (duration/2) / 29.1;
/*
  Serial.println(cm);
  delay(250);
*/
  rD=digitalRead(redD);
  rB=digitalRead(redB);
  rE=digitalRead(redE);
  rC=digitalRead(redC);
  rA=digitalRead(redA);

  /* 循跡判斷 */
  
if(cm<=10)
  {
    turnRight(500);
    forward(1300);
    turnLeft(1000);
    forward(1300);
    }
else{
   
if((rA==0&&rB==0&&rC==0&&rD==0&&rE==0)||(rA==0&&rB==0&&rC==0&&rD==1&&rE==1)||(rA==0&&rB==1&&rC==1&&rD==1&&rE==1)||(rA==1&&rB==0&&rC==0&&rD==1&&rE==1)||(rA==1&&rB==0&&rC==0&&rD==0&&rE==0))/*A-E,ABC,A.BC.DBCE黑線*/
  {
    forward(50);
  }
  /* 右轉區間 */
  else if((rA==0&&rB==1&&rC==0&&rD==1&&rE==1)||(rA==1&&rB==1&&rC==0&&rD==1&&rE==1)||(rA==0&&rB==1&&rC==0&&rD==0&&rE==1))/* AC.C.ACD 右轉 小*/
  {
    turnRight(50);
    forward(10);
    }
  else if((rA==1&&rB==1&&rC==0&&rD==1&&rE==0)||(rA==1&&rB==1&&rC==1&&rD==1&&rE==0))/*CE.E 右轉 中*/
  {
    turnRight(150);
    forward(10);
    }
  else if((rA==0&&rB==1&&rC==1&&rD==1&&rE==0)||(rA==0&&rB==1&&rC==0&&rD==1&&rE==0)||(rA==1&&rB==0&&rC==1&&rD==1&&rE==0)||(rA==0&&rB==0&&rC==0&&rD==1&&rE==0))/*AE.ACE.BE.BACE 右轉 大*/
  {
   turnRight(250);
   forward(10);
    }
  /*左轉區間*/
  else if ((rA==0&&rB==0&&rC==1&&rD==1&&rE==1)||(rA==1&&rB==0&&rC==1&&rD==1&&rE==1)||(rA==0&&rB==0&&rC==1&&rD==1&&rE==0)) /*BA.B 左轉 小*/
  {
    turnLeft(50);
    forward(10);
    }
  else if((rA==1&&rB==0&&rC==1&&rD==0&&rE==1)||(rA==1&&rB==1&&rC==1&&rD==0&&rE==1))/*DB.D 左轉 中*/
  {
    turnLeft(130);
    forward(10);
    }
  else if((rA==0&&rB==1&&rC==1&&rD==0&&rE==1)||(rA==0&&rB==0&&rC==1&&rD==0&&rE==1)||(rA==1&&rB==1&&rC==0&&rD==0&&rE==1)||(rA==0&&rB==0&&rC==0&&rD==0&&rE==1)) /*DA.DBA.DC.CABD 左轉 大*/
  {
    turnLeft(250);
    forward(10);
    }

  else
  {
    forward(50);
    }
}
}

  void forward(int time){
  servoRight.writeMicroseconds(1300);
  servoLeft.writeMicroseconds(1700);
  delay(time);
  }
  void turnLeft(int time){
  servoRight.writeMicroseconds(1000);
  servoLeft.writeMicroseconds(1512);
  delay(time);
  }
  void turnRight(int time){
  servoRight.writeMicroseconds(1488);
  servoLeft.writeMicroseconds(2000);
  delay(time);
  }
  void back(int time){
  servoRight.writeMicroseconds(1700);
  servoLeft.writeMicroseconds(1300);
  delay(time);
  }

超新手

可能是執行 pulseIn  需要點時間, 解決方法是
1. 不用每次執行, 例如 10 次 IR 執行一次Sonar即可
2. pulseIn 如果逾時, 會回傳 0, 所以要排除, 才不會有問題
    也就? cm > 0 而且 cm <  10 才轉彎
3. pulseIn 可以設 TIME OUT 時間,以免執行過久
例如
duration = pulseIn(echoPin, HIGH, 500);
就不會一直卡在這, 沒時間看 IR