#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)向舵機
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); //定義舵機所用引腳
s.write(90); //初始化舵機角度
tone(12,800,500);
delay(2000); //開機延時
}
void loop() { //主函數(shù)
s.write(90); //舵機中位
range(); //執(zhí)行測距函數(shù)
if(S<20){ //判斷障礙物距離，距離太近
back(); //后退
delay(1000); //后退時間
}
if(S<=50&&S>20){ //距離中等
turn(); //運行轉(zhuǎn)向判斷函數(shù)
}
if(S>50){ //距離充足
line(); //運行直行函數(shù)
}
}
void turn(){ //判斷轉(zhuǎn)向函數(shù)
lull(); //停止所用電機
s.write(170); //舵機轉(zhuǎn)到170度既左邊（角度與安裝方式有關(guān)）
delay(1000); //留時間給舵機轉(zhuǎn)向
range(); //運行測距函數(shù)
s.write(90); //測距完成，舵機回到中位
delay(1000); //留時間給舵機轉(zhuǎn)向
if (S>40) {L();} //判斷左邊障礙物距離，如果距離充足,運行左轉(zhuǎn)
else {
s.write(10); //否則，舵機轉(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的隨機數(shù)
if (x=0){R();}
else{L();} //判斷隨機數(shù)
} //否則后退，并隨機轉(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(); //運行大燈函數(shù)
digitalWrite(motorR1,HIGH); //啟動所有電機向前
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(); //暫停所有電機
}
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);
}
|
