超聲波避障

``1.超聲波測距原理
超聲波是一種頻率比較高的聲音，指向性強 ，超聲波測距的原理是利用超聲波在空氣中的傳播速度為已知， 測量聲波在發射后遇到障礙物反射回來的時間，根據發射和接收的時間差計算出發射點到障礙物的實際距離，
測距的公式表示為:L＝CxT，L是測量距離的長度，C為超聲波在空氣中的傳播速度，T是測量距離傳播的時間差，也就是發射到接受時間數值的一半，
2.超聲波在智能小車中的安裝效果圖

3.HC-SR04產品特點
(1)典型作業用電壓:5v
(2)超小靜態作業電流:小于2mA
(3)感應角度:不大于十五度
(4)探測距離:2cm~400cm
(5)高精度可達0.3cm
(6)盲區(2cm)超近
4.HC-SR04介面定義
Vcc,Gnd,Trig(控制端),Echo(接收端)，
控制端和接收端和單片機引腳相連，控制口發一個10us以上的高電平,就可以在接收口 等待高電平輸出，一有輸出就可以開定時器計時，當此口變為低電平時，就可以讀定時器的值，此時就為此次測距的時間，方可算出距離，如此不斷地周期測， 就可以達到移動測量的值了，
5.超聲波時序圖

6.智能小車超聲波模塊io口
采用51單片機的p2.0，p2.1連接控制超聲波模塊， 其中p2.0為Echo,p2.1為Trig

7代碼測驗
`#include <Servo.h>

#define STOP 0
#define FORWARD 1
#define BACKWARD 2
#define TURNLEFT 3
#define TURNRIGHT 4

int leftMotor1 = 16;
int leftMotor2 = 17;
int rightMotor1 = 18;
int rightMotor2 = 19;

int leftPWM = 5;
int rightPWM = 6;

Servo myServo; //舵機

int inputPin=7; // 定義超聲波信號接收介面
int outputPin=8; // 定義超聲波信號發出介面

void setup() {
// put your setup code here, to run once:
//串口初始化
Serial.begin(9600);
//舵機引腳初始化
myServo.attach(9);
//測速引腳初始化
pinMode(leftMotor1, OUTPUT);
pinMode(leftMotor2, OUTPUT);
pinMode(rightMotor1, OUTPUT);
pinMode(rightMotor2, OUTPUT);
pinMode(leftPWM, OUTPUT);
pinMode(rightPWM, OUTPUT);
//超聲波控制引腳初始化
pinMode(inputPin, INPUT);
pinMode(outputPin, OUTPUT);
}

void loop() {
// put your main code here, to run repeatedly:
avoidance();
}
void motorRun(int cmd,int value)
{
analogWrite(leftPWM, value); //設定PWM輸出，即設定速度
analogWrite(rightPWM, value);
switch(cmd){
case FORWARD:
Serial.println(“FORWARD”); //輸出狀態
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);
break;
case BACKWARD:
Serial.println(“BACKWARD”); //輸出狀態
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, HIGH);
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, HIGH);
break;
case TURNLEFT:
Serial.println(“TURN LEFT”); //輸出狀態
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, HIGH);
break;
case TURNRIGHT:
Serial.println(“TURN RIGHT”); //輸出狀態
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, HIGH);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);
break;
default:
Serial.println(“STOP”); //輸出狀態
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);
}
}
void avoidance()
{
int pos;
int dis[3];//距離
motorRun(FORWARD,200);
myServo.write(90);
dis[1]=getDistance(); //中間

if(dis[1]<30)
{
motorRun(STOP,0);
for (pos = 90; pos <= 150; pos += 1)
{
myServo.write(pos); // tell servo to go to position in variable ‘pos’
delay(15); // waits 15ms for the servo to reach the position
}
dis[2]=getDistance(); //左邊
for (pos = 150; pos >= 30; pos -= 1)
{
myServo.write(pos); // tell servo to go to position in variable ‘pos’
delay(15); // waits 15ms for the servo to reach the position
if(pos==90)
dis[1]=getDistance(); //中間
}
dis[0]=getDistance(); //右邊
for (pos = 30; pos <= 90; pos += 1)
{
myServo.write(pos); // tell servo to go to position in variable ‘pos’
delay(15); // waits 15ms for the servo to reach the position
}
if(dis[0]<dis[2]) //右邊距離障礙的距離比左邊近
{
//左轉
motorRun(TURNLEFT,250);
delay(500);
}
else //右邊距離障礙的距離比左邊遠
{
//右轉
motorRun(TURNRIGHT,250);
delay(500);
}
}
}
int getDistance()
{
digitalWrite(outputPin, LOW); // 使發出發出超聲波信號介面低電平2μs
delayMicroseconds(2);
digitalWrite(outputPin, HIGH); // 使發出發出超聲波信號介面高電平10μs，這里是至少10μs
delayMicroseconds(10);
digitalWrite(outputPin, LOW); // 保持發出超聲波信號介面低電平
int distance = pulseIn(inputPin, HIGH); // 讀出脈沖時間
distance= distance/58; // 將脈沖時間轉化為距離（單位：厘米）
Serial.println(distance); //輸出距離值

if (distance >=50)
{
//如果距離小于50厘米回傳資料
return 50;
}//如果距離小于50厘米小燈熄滅
else
return distance;
}`

8.智能小車避障解釋
小車前程序序中只檢測前方距離障礙的距離， 并且控制多機保持超聲波模塊位于正前方 ，當檢測到小車前方距離障礙小于30cm時停車檢測兩邊距離 ，控制多機每次運動一個周期后都回傳正前方位置，由于舵機運動需要一定的時間，因此在每轉過一個角度的時候都延時 delay(15)供其運動 ，當運動到最左邊時，檢測小車左邊距離障礙的距離，然后向右邊運動，檢測右邊距離 ，將前邊左邊，右邊距離障礙的都檢測結束之后，舵機回到最初位置 ，