超音波避障實驗

#include <Servo.h>

Servo myservo;

int pp; //temp value

int in1=4;

int in2=5;

int in3=6;

int in4=7;

int d = 0;

int trigPin=9;

int echoPin=8;

void setup(){

//Serial.begin(9600);

myservo.attach(3);

myservo.write(90);

//delay(2000);

}

unsigned long ping_d() {

digitalWrite(trigPin,HIGH) ;

delayMicroseconds(1000) ;

digitalWrite(trigPin,LOW);

return ( pulseIn(echoPin,HIGH)/58) ;

}

void loop()

{

myservo.write(90);

int left_d, right_d ; //紀錄左,右邊障礙物距離

d = ping_d() ; //偵測前方障礙物距離

//Serial.println(d);

// 如果前方30cm處有障礙物,自走車需要進入判斷模式,決定行進方式

if( d<=30)

{

Stop() ; //自走車停止前進

myservo.write(20) ; // 伺服馬達轉向右邊

delay(500) ;

right_d = ping_d() ; // 取得右邊障礙物距離

delay(20) ;

myservo.write(150) ; // 伺服馬達轉向左邊

delay(500) ;

left_d = ping_d() ; // 取得左邊障礙物的距離

myservo.write(85) ; // 轉動伺服馬達,使超音波回到正前方

// 如果左邊空間大 且 障礙物距離超過30cm以上 ---> 左轉彎後繼續前進

if( (left_d>right_d) && (left_d>30)) { //左邊有空間

Left() ;

delay(350) ;

forward() ;

} else if( (right_d>=left_d) && (right_d>30)) { // 右邊空間大且右邊障礙物距離大於30cm以上 -->右轉彎後前進

Right() ;

delay(350) ;

forward() ;

} else { // 前,左,右障礙物距離都小於30公分 --->後退->轉彎->前進

backward() ;

delay(1500) ;

Right() ;

delay(350) ;

forward() ;

}

}

//delay(30) ;

}

int forward()

{

digitalWrite(in1, LOW);

digitalWrite(in2, HIGH);

digitalWrite(in3, HIGH);

digitalWrite(in4, LOW);

}

int backward()

{

digitalWrite(in1, HIGH);

digitalWrite(in2, LOW);

digitalWrite(in3, LOW);

digitalWrite(in4, HIGH);

}

int Right()

{

digitalWrite(in1, LOW);

digitalWrite(in2, HIGH);

digitalWrite(in3, LOW);

digitalWrite(in4, LOW);

}

int Left()

{

digitalWrite(in1, LOW);

digitalWrite(in2, LOW);

digitalWrite(in3, HIGH);

digitalWrite(in4, LOW);

}

int Stop()

{

digitalWrite(in1, LOW);

digitalWrite(in2, LOW);

digitalWrite(in3, LOW);

digitalWrite(in4, LOW);

}