Arduino智能循跡小車程序 8路循跡+左前右超聲避障

ID:261237 · 發表于 2017-12-17 20:55

8路循跡+左前右超聲避障程序全部資料51hei下載地址：

Ranger-Red.rar (2.94 KB, 下載次數: 103)

Arduino源程序：

//===============================================================
//雷達消零減為2，加入findspot函，加入checkforward函數,已加入AT==0時回避對方配送站算，已加入角落修正點(AT雙向)
//#include <Servo.h>
int right1=30;//in1
int right2=31;//in2
int rightpwm=4;//En1
int left1=33;//后片in3
int left2=34;//后片in4
int leftpwm=3;//后片EnB
int EMP1=36;
int EMP2=37;
int LED1=25;
int LED2=22;
int LED3=23;
int LED4=24;
int LEDL=28;
int LEDF=27;
int LEDR=26;
int Color = 29;
int TrigF = A2;
int EchoF = A3;
int TrigL = A4;
int EchoL = A5;
int TrigR = A6;
int EchoR = A7;
int key = A8;
int beep = A9;
const int Sensor1 = 15;
const int Sensor2 = 16;
const int Sensor3 = 17;
const int Sensor4 = 18;
const int Sensor5 = 19;
const int Sensor6 = 20;
const int SensorF1 = 14;
const int SensorF2 = 21;
int FD = 0;
int RD = 0;
int LD = 0;
int S1;
int S2;
int S3;
int S4;
int S5;
int S6;
int SF1;
int SF2;
int C;
int X = 0;
int Y = 0;
int TR = 0;
int TL = 0;
int Z = 0;
int P = 0; //記錄拐點數
int AT =0;//AT=0進攻狀態,AT=1撤退狀態
void setup()
{
Serial.begin(9600);
pinMode(left1,OUTPUT);
pinMode(left2,OUTPUT);
pinMode(leftpwm,OUTPUT);
pinMode(right1,OUTPUT);
pinMode(right2,OUTPUT);
pinMode(rightpwm,OUTPUT);
pinMode(EMP1,OUTPUT);
pinMode(EMP2,OUTPUT);
pinMode(LED1,OUTPUT);
pinMode(LED2,OUTPUT);
pinMode(LED3,OUTPUT);
pinMode(LED4,OUTPUT);
pinMode(LEDL,OUTPUT);
pinMode(LEDF,OUTPUT);
pinMode(LEDR,OUTPUT);
pinMode(Color, INPUT);
pinMode(key, INPUT);
pinMode(beep, OUTPUT);
pinMode(Sensor1, INPUT);
pinMode(Sensor2, INPUT);
pinMode(Sensor3, INPUT);
pinMode(Sensor4, INPUT);
pinMode(Sensor5, INPUT);
pinMode(Sensor6, INPUT);
pinMode(SensorF1, INPUT);
pinMode(SensorF2, INPUT);
pinMode(EchoF, INPUT);
pinMode(EchoR, INPUT);
pinMode(EchoL, INPUT);
pinMode(TrigF, OUTPUT);
pinMode(TrigR, OUTPUT);
pinMode(TrigL, OUTPUT);
}
void run(int time)
{
digitalWrite(left1,HIGH);
digitalWrite(left2,LOW);
digitalWrite(leftpwm,HIGH);
analogWrite(leftpwm,240);
digitalWrite(right1,HIGH);
digitalWrite(right2,LOW);
digitalWrite(rightpwm,HIGH);
analogWrite(rightpwm,240);
delay(time * 1);
}
void left(int time)
{
digitalWrite(leftpwm,LOW);
digitalWrite(right1,HIGH);
digitalWrite(right2,LOW);
digitalWrite(rightpwm,HIGH);
analogWrite(rightpwm,240);
delay(time * 1);
}
void right(int time)
{
digitalWrite(left1,HIGH);
digitalWrite(left2,LOW);
digitalWrite(leftpwm,HIGH);
analogWrite(leftpwm,240);
digitalWrite(rightpwm,LOW);
delay(time * 1);
}
void brake(int time)
{ digitalWrite(left2,HIGH);
digitalWrite(left1,LOW);
digitalWrite(leftpwm,HIGH);
analogWrite(leftpwm,30);
digitalWrite(right2,HIGH);
digitalWrite(right1,LOW);
digitalWrite(rightpwm,HIGH);
analogWrite(rightpwm,30);
delay(time * 1);
}
//==========================================================
void back(int time)
{
digitalWrite(left2,HIGH);
digitalWrite(left1,LOW);
digitalWrite(leftpwm,HIGH);
analogWrite(leftpwm,240);
digitalWrite(right2,HIGH);
digitalWrite(right1,LOW);
digitalWrite(rightpwm,HIGH);
analogWrite(rightpwm,240);
delay(time * 1);
}
void brakeback(int time)
{
digitalWrite(left1,HIGH);
digitalWrite(left2,LOW);
digitalWrite(leftpwm,HIGH);
analogWrite(leftpwm,30);
digitalWrite(right1,HIGH);
digitalWrite(right2,LOW);
digitalWrite(rightpwm,HIGH);
analogWrite(rightpwm,30);
delay(time * 1);
}
void spinleft(int time)
{
digitalWrite(left2,HIGH);
digitalWrite(left1,LOW);
digitalWrite(leftpwm,HIGH);
analogWrite(leftpwm,240);
digitalWrite(right1,HIGH);
digitalWrite(right2,LOW);
digitalWrite(rightpwm,HIGH);
analogWrite(rightpwm,240);
delay(time * 1);
}
void brakespinleft(int time)
{
digitalWrite(left1,HIGH);
digitalWrite(left2,LOW);
digitalWrite(leftpwm,HIGH);
analogWrite(leftpwm,30);
digitalWrite(right2,HIGH);
digitalWrite(right1,LOW);
digitalWrite(rightpwm,HIGH);
analogWrite(rightpwm,30);
delay(time * 1);
}
void spinright(int time)
{
digitalWrite(left1,HIGH);
digitalWrite(left2,LOW);
digitalWrite(leftpwm,HIGH);
analogWrite(leftpwm,240);
digitalWrite(right2,HIGH);
digitalWrite(right1,LOW);
digitalWrite(rightpwm,HIGH);
analogWrite(rightpwm,240);
delay(time * 1);
}
void brakespinright(int time)
{
digitalWrite(left2,HIGH);
digitalWrite(left1,LOW);
digitalWrite(leftpwm,HIGH);
analogWrite(leftpwm,240);
digitalWrite(right1,HIGH);
digitalWrite(right2,LOW);
digitalWrite(rightpwm,HIGH);
analogWrite(rightpwm,240);
delay(time * 1);
}
void stop(int time)
{
digitalWrite(leftpwm,LOW);
digitalWrite(rightpwm,LOW);
delay(time * 1);
}
//==========================================================
void keysacn()//按鍵掃描
{
int val;
val = digitalRead(key); //讀取電平值賦給val
while (!digitalRead(key)) //當按鍵沒被按下時，一直循環
{
val = digitalRead(key); //此句可省略，可讓循環跑空
}
while (digitalRead(key)) //當按鍵被按下時
{
delay(10); //延時10ms
val = digitalRead(key); //讀取電平值賦給val
if (val == HIGH) //第二次判斷按鍵是否被按下
{
// digitalWrite(beep, HIGH); //蜂鳴器響
// while (!digitalRead(key)) //判斷按鍵是否被松開
digitalWrite(beep, LOW); //蜂鳴器停止
}
else
digitalWrite(beep, LOW); //蜂鳴器停止
}
}
//==========================================================
void gostraight()
{run(120);}
void turnleft()
{//run(360);
spinleft(500);
do {spinleft(1);SF1 =digitalRead(SensorF1);SF2 =digitalRead(SensorF2);}
while(SF1==LOW || SF2==LOW);
brakespinleft(50);
stop(10);
}
void turnright()
{ //run(360);
spinright(500);
do {spinright(1);SF1 =digitalRead(SensorF1);SF2 =digitalRead(SensorF2);}
while(SF1==LOW || SF2==LOW);
brakespinright(50);
stop(10);
}
void rightback()
{ //run(360);
spinright(500);
do {spinright(1);SF1 =digitalRead(SensorF1);SF2 =digitalRead(SensorF2);}
while(SF1==LOW || SF2==LOW);
brakespinright(50);
stop(50);
spinright(500);
do {spinright(1);SF1 =digitalRead(SensorF1);SF2 =digitalRead(SensorF2);}
while(SF1==LOW || SF2==LOW);
brakespinright(50);
stop(10); }
void leftback()
{//run(360);
spinleft(500);
do {spinleft(1);SF1 =digitalRead(SensorF1);SF2 =digitalRead(SensorF2);}
while(SF1==LOW || SF2==LOW);
brakespinleft(50);
stop(50);
spinleft(500);
do {spinleft(1);SF1 =digitalRead(SensorF1);SF2 =digitalRead(SensorF2);}
while(SF1==LOW || SF2==LOW);
brakespinleft(50);
stop(10);}
// void findspot()
//{ run(360);}
void checkforward()
{
S1 = digitalRead(Sensor1);
S2 = digitalRead(Sensor2);
S3 = digitalRead(Sensor3);
S4 = digitalRead(Sensor4);
S5 = digitalRead(Sensor5);
S6 = digitalRead(Sensor6);
// SF1 = digitalRead(SensorF1);
// SF2 = digitalRead(SensorF2);
if (S1 == HIGH & S2 == LOW & S3 == LOW & S4 == LOW & S5 == LOW & S6 == LOW)
{ run(100);
spinright(100);}
else if (S1 == HIGH & S2 == HIGH & S3 == LOW & S4 == LOW & S5 == LOW & S6 == LOW)
{run(80);
spinright(80);}
else if (S1 == LOW & S2 == HIGH & S3 == LOW & S4 == LOW & S5 == LOW & S6 == LOW)
{run(60);
spinright(60);}
else if (S1 == LOW & S2 == HIGH & S3 == HIGH & S4 == LOW & S5 == LOW & S6 == LOW)
{run(40);
spinright(40);}
else if (S1 == LOW & S2 == LOW & S3 == HIGH & S4 == LOW & S5 == LOW & S6 == LOW)
{run(20);
spinright(20);}
else if (S1 == LOW & S2 == LOW & S3 == LOW & S4 == HIGH & S5 == LOW & S6 == LOW)
{run(20);
spinleft(20);}
else if (S1 == LOW & S2 == LOW & S3 == LOW & S4 == HIGH & S5 == HIGH & S6 == LOW)
{run(40);
spinleft(40);}
else if (S1 == LOW & S2 == LOW & S3 == LOW & S4 == LOW & S5 == HIGH & S6 == LOW)
{run(60);
spinleft(60);}
else if (S1 == LOW & S2 == LOW & S3 == LOW & S4 == LOW & S5 == HIGH & S6 == HIGH)
{run(80);
spinleft(80);}
else if (S1 == LOW & S2 == LOW & S3 == LOW & S4 == LOW & S5 == LOW & S6 == HIGH)
{run(100);
spinleft(100);}
else
stop(1);
}
void Distance_testF() // 量出前方距離
{
digitalWrite(LEDF, HIGH);
digitalWrite(TrigF, LOW); // 給觸發腳低電平2μs
delayMicroseconds(2);
digitalWrite(TrigF, HIGH); // 給觸發腳高電平10μs，這里至少是10μs
delayMicroseconds(10);
digitalWrite(TrigF, LOW); // 持續給觸發腳低電
float Fdistance = pulseIn(EchoF, HIGH); // 讀取高電平時間(單位：微秒)
Fdistance= Fdistance/58;
int FDT = Fdistance;
FD = FDT<2?100:FDT;
digitalWrite(LEDF, LOW);
Serial.print("FDistance:");
Serial.println(FD);
}
void Distance_testR() // 量出前方距離
{
digitalWrite(LEDR, HIGH);
digitalWrite(TrigR, LOW); // 給觸發腳低電平2μs
delayMicroseconds(2);
digitalWrite(TrigR, HIGH); // 給觸發腳高電平10μs，這里至少是10μs
delayMicroseconds(10);
digitalWrite(TrigR, LOW); // 持續給觸發腳低電
float Rdistance = pulseIn(EchoR, HIGH); // 讀取高電平時間(單位：微秒)
Rdistance= Rdistance/58;
int RDT = Rdistance;
RD=RDT<2?100:RDT;
digitalWrite(LEDR, LOW);
Serial.print("RDistance:");
Serial.println(RD);
}
void Distance_testL() // 量出前方距離
{ digitalWrite(LEDL, HIGH);
digitalWrite(TrigL, LOW); // 給觸發腳低電平2μs
delayMicroseconds(2);
digitalWrite(TrigL, HIGH); // 給觸發腳高電平10μs，這里至少是10μs
delayMicroseconds(10);
digitalWrite(TrigL, LOW); // 持續給觸發腳低電
float Ldistance = pulseIn(EchoL, HIGH); // 讀取高電平時間(單位：微秒)
Ldistance= Ldistance/58;
int LDT = Ldistance;
LD = LDT<2?100:LDT;
digitalWrite(LEDL, LOW);
Serial.print("LDistance:");
Serial.println(LD);
}
//==========================================================
void loop()
{
keysacn();
delay(100);
TL = 0;
TR = 0;
X = 0;
Y = 0;
P = 0;
Z = 0;
AT = 0;
while (1)
{
switch ((TR - TL) % 4)//4燈順序按逆時針1234排布
{ case 0: digitalWrite(LED1, HIGH);
digitalWrite(LED2, LOW);
digitalWrite(LED3, LOW);
digitalWrite(LED4, LOW);
break;
case 1: digitalWrite(LED1, LOW);
digitalWrite(LED2, HIGH);
digitalWrite(LED3, LOW);
digitalWrite(LED4, LOW);
break;
case 2: digitalWrite(LED1, LOW);
digitalWrite(LED2, LOW);
digitalWrite(LED3, HIGH);
digitalWrite(LED4, LOW);
break;
case 3: digitalWrite(LED1, LOW);
digitalWrite(LED2, LOW);
digitalWrite(LED3, LOW);
digitalWrite(LED4, HIGH);
break;
case -1:digitalWrite(LED1, LOW);
digitalWrite(LED2, LOW);
digitalWrite(LED3, LOW);
digitalWrite(LED4, HIGH);
break;
case -2:digitalWrite(LED1, LOW);
digitalWrite(LED2, LOW);
digitalWrite(LED3, HIGH);
digitalWrite(LED4, LOW);
break;
case -3:digitalWrite(LED1, LOW);
digitalWrite(LED2, HIGH);
digitalWrite(LED3, LOW);
digitalWrite(LED4, LOW);
break;}
S1 = digitalRead(Sensor1);
S2 = digitalRead(Sensor2);
S3 = digitalRead(Sensor3);
S4 = digitalRead(Sensor4);
S5 = digitalRead(Sensor5);
S6 = digitalRead(Sensor6);
SF1 = digitalRead(SensorF1);
SF2 = digitalRead(SensorF2);
C = digitalRead(Color);
// SB1 = digitalRead(SensorB1);
// SB2 = digitalRead(SensorB2);
//*******************************直路行駛開始***********************************
if (S1 == LOW & S2 == LOW & S3 == HIGH & S4 == HIGH & S5 == LOW & S6 == LOW)
run(1);
else if (S1 == LOW & S2 == LOW & S3 == HIGH & S4 == LOW & S5 == LOW & S6 == LOW) //4、LLHLLL，直路修偏
spinleft(1);
else if (S1 == LOW & S2 == LOW & S3 == LOW & S4 == HIGH & S5 == LOW & S6 == LOW) //5、LLLHLL，直路修偏
spinright(1);
//*******************************直路行駛結束***********************************
//*******************************進入右丁字路口***********************************
else if (S1 == LOW & S2 == LOW &(S3 == HIGH || S4 == HIGH) & S5 == HIGH & S6 == HIGH)
{
turnright(); }
//********************右丁字路口結束************************
//********************左丁字路口開始************************
else if (S1 == HIGH & S2 == HIGH & (S3 == HIGH || S4 == HIGH) & S5 == LOW & S6 == LOW)
{ turnleft();}
//********************左丁字路口結束************************
//********************進入十字路口************************
else if (S1 == HIGH & S2 == HIGH &( S3 == HIGH || S4 == HIGH) & S5 == HIGH & S6 == HIGH)
{
gostraight(); }
//********************十字路口結束************************
//********************進入前丁字路口************************
else if (S1 == HIGH & S2 == HIGH & S3 == LOW & S4 == LOW & S5 == HIGH & S6 == HIGH)
{
rightback(); ; }
//********************前丁字路口結束************************
//********************進入右拐角************************
else if (S1 == LOW & S3 == LOW & S4 == LOW & S5 == HIGH & S6 == HIGH)
{
turnright() ; }
//********************右拐角結束************************
//********************進入左拐角************************
else if (S1 == HIGH & S2 == HIGH & S3 == LOW & S4 == LOW & S6 == LOW)
{
turnleft(); }
//********************左拐角結束************************
else if (S1 == LOW & S2 == HIGH & S3 == HIGH & S4 == LOW & S5 == LOW & S6 == LOW) //11、LHHLLL:直道右偏較多
left(1);
else if (S1 == LOW & S2 == LOW & S3 == LOW & S4 == HIGH & S5 == HIGH & S6 == LOW) //12、LLLHHL:直道左偏較多
right(1);
else if (S1 == LOW & S2 == HIGH & S3 == LOW & S4 == LOW & S5 == LOW & S6 == LOW) //13、LHLLLL:直道右偏太多
spinleft(1);
else if (S1 == LOW & S2 == LOW & S3 == LOW & S4 == LOW & S5 == HIGH & S6 == LOW) //14、LLLLHL:直道左偏太多
spinright(1);
// else if (S1 == LOW & S2 == LOW & S3 == HIGH & S4 == HIGH & S5 == HIGH & S6 == LOW) //LLHHHL:剛前出右丁路口時
// left(1);
// else if (S1 == LOW & S2 == HIGH & S3 == HIGH & S4 == HIGH & S5 == LOW & S6 == LOW) //LHHHLL:剛前出左丁路口時
// right(1);
else if (S1 == HIGH & S2 == LOW & S3 == LOW & S4 == LOW & S5 == LOW & S6 == LOW)//17、HLLLLL:行車出現巨大偏差，加這一句用來保險
left(1);
else if (S1 == LOW & S2 == LOW & S3 == LOW & S4 == LOW & S5 == LOW & S6 == HIGH)//18、LLLLLH:行車出現巨大偏差,加這一句用來保險
right(1);
else if (S1 == LOW & S2 == LOW & S3 == LOW & S4 == LOW & S5 == LOW & S6 == LOW) //20、LLLLLL:脫軌
back(1);
else
run(1);
}
}