Robofun 機器人論壇

標題: 循跡自走車加上超音波避障，功能失常 [打印本頁]

作者: csddd888 時間: 2018-12-14 02:04
標題: 循跡自走車加上超音波避障，功能失常
板子 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);
  }

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

歡迎光臨 Robofun 機器人論壇 (https://robofun.net/forum/)