標(biāo)題: 超聲波避障小車源程序 [打印本頁]
作者: 一二三四五    時間: 2017-7-7 15:44
標(biāo)題: 超聲波避障小車源程序
#include <Servo.h>
int motorL1=6; //左邊輪子
int motorL2=5;
int motorR1=9; //右邊輪子
int motorR2=11;
int ledL=7; //轉(zhuǎn)彎燈
int ledR=8;
int ledB=12; //倒車燈
int ledG=13; //大燈
Servo s; //超聲波轉(zhuǎn)向舵機(jī)
int trig=4; //發(fā)射信號
int echo=2; //接收信號
unsigned int S; //距離存儲
void setup() {
Serial.begin(9600); //設(shè)置波特率
pinMode(trig,OUTPUT); //設(shè)置引腳模式
pinMode(echo,INPUT);
pinMode(motorL1,OUTPUT);
pinMode(motorL2,OUTPUT);
pinMode(motorR1,OUTPUT);
pinMode(motorR2,OUTPUT);
pinMode(ledL,OUTPUT);
pinMode(ledR,OUTPUT);
pinMode(ledB,OUTPUT);
pinMode(ledG,OUTPUT);
//pinMode(12,OUTPUT);
s.attach(3); //定義舵機(jī)所用引腳
s.write(90); //初始化舵機(jī)角度
tone(12,800,500);
delay(2000); //開機(jī)延時
}


void loop() { //主函數(shù)
s.write(90); //舵機(jī)中位
range(); //執(zhí)行測距函數(shù)
if(S<20){ //判斷障礙物距離，距離太近
back(); //后退
delay(1000); //后退時間
}
if(S<=50&&S>20){ //距離中等
turn(); //運(yùn)行轉(zhuǎn)向判斷函數(shù)
}
if(S>50){ //距離充足
line(); //運(yùn)行直行函數(shù)
}
}


void turn(){ //判斷轉(zhuǎn)向函數(shù)
lull(); //停止所用電機(jī)
s.write(170); //舵機(jī)轉(zhuǎn)到170度既左邊（角度與安裝方式有關(guān)）
delay(1000); //留時間給舵機(jī)轉(zhuǎn)向
range(); //運(yùn)行測距函數(shù)
s.write(90); //測距完成，舵機(jī)回到中位
delay(1000); //留時間給舵機(jī)轉(zhuǎn)向
if (S>40) {L();} //判斷左邊障礙物距離，如果距離充足,運(yùn)行左轉(zhuǎn)
else {
s.write(10); //否則，舵機(jī)轉(zhuǎn)動到10度，測右邊距離
delay(1000);
range(); //測距
s.write(90); //中位
delay(1000);
if(S>40){ R();
} //右轉(zhuǎn)
else{ back(); //判斷右邊距離，距離充足右轉(zhuǎn)否則后退
int x=random(1); //產(chǎn)生一個0到1的隨機(jī)數(shù)
if (x=0){R();}
else{L();} //判斷隨機(jī)數(shù)
} //否則后退，并隨機(jī)轉(zhuǎn)向
}
}


void range(){ //測距函數(shù)
digitalWrite(trig,LOW); //測距
delayMicroseconds(2); //延時2微秒
digitalWrite(trig,HIGH);
delayMicroseconds(20);
digitalWrite(trig,LOW);
int distance = pulseIn(echo,HIGH); //讀取高電平時間
distance = distance/58; //按照公式計算
S = distance; //把值賦給S
Serial.println(S); //向串口發(fā)送S的值，可以在顯示器上顯示距離
if (S<40){
tone(12,800,50);
delay(50); //延時
}
}


void line(){
ledoff(); //關(guān)閉所有LED
ledg(); //運(yùn)行大燈函數(shù)
digitalWrite(motorR1,HIGH); //啟動所有電機(jī)向前
digitalWrite(motorL1,HIGH);
digitalWrite(motorR2,LOW);
digitalWrite(motorL2,LOW);
}

void L(){
ledoff();
digitalWrite(motorL1,LOW);
digitalWrite(motorR2,LOW);
analogWrite(motorL2,100);
analogWrite(motorR1,100);
for (int x=0;x<5;x++){ //執(zhí)行5次
ledl(); //右邊LED函數(shù)
}
lull(); //暫停所有電機(jī)
}
void R(){
ledoff();
digitalWrite(motorL2,LOW);
digitalWrite(motorR1,LOW);
analogWrite(motorL1,100);
analogWrite(motorR2,100);
for (int x=0;x<5;x++){
ledr();
}
lull();
}


void back(){ //后退函數(shù)
ledoff();
ledb();
digitalWrite(motorL1,LOW);
digitalWrite(motorR1,LOW);
analogWrite(motorL2,100);
analogWrite(motorR2,100);
ledb();
}

void lull(){
digitalWrite(motorL1,LOW);
digitalWrite(motorL2,LOW);
digitalWrite(motorR1,LOW);
digitalWrite(motorR2,LOW);
}


void ledl(){
digitalWrite(ledL,HIGH);
delay(50);
digitalWrite(ledL,LOW);
delay(50);
}


void ledr(){
digitalWrite(ledR,HIGH);
delay(50);
digitalWrite(ledR,LOW);
delay(50);
}


void ledb(){
digitalWrite(ledB,HIGH);
}


void ledg(){
digitalWrite(ledG,HIGH);
}


void ledoff(){
digitalWrite(ledL,LOW);
digitalWrite(ledL,LOW);
digitalWrite(ledL,LOW);
digitalWrite(ledL,LOW);
}






歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1