超聲波避障程序

1.2.3.4.5.6.7.8.9.10.11.12.13.14.15.16.17.18.19.20.21.22.23.24.25.26.27.28.29.30.31.32.33.34.35.36.37.38.39.40.41.42.43.44./*程序的開頭要描述你的設(shè)備再來要說是脽寫的日期版本然后碰到問題一次一次都要寫清楚馬達幾個驅(qū)動電路都要描述L=左R=右F=刖B=后*/#include<Servo.h>intpinLB=6;intpinLF=9;//定義8腳位左后//定義9腳位左刖intpinRB=10;//定義10腳位右后intpinRF=11;//定義11腳位右刖intinputPin=A0;//定義超音波信號接收腳位intoutputPin=A1;//定義超音波信號發(fā)射腳位intFspeedd=0;//前速intRspeedd=0;//右速intLspeedd=0;//左速intdirectionn=0; //前=8后=2左=4右=6Servomyservo;//設(shè)myservointdelaytime=250;//伺服馬達轉(zhuǎn)向后的穩(wěn)定時間intFgo=8;//前進intRgo=6;//右轉(zhuǎn)intLgo=4;//左轉(zhuǎn)intBgo=2;//倒車voidsetup(){Serial.begin(9600); //定義馬達輸出腳位pinMode(pinLB,OUTPUT);//腳位8(PWM)pinMode(pinLF,OUTPUT);//腳位9(PWM)pinMode(pinRB,OUTPUT);//腳位10(PWM)pinMode(pinRF,OUTPUT);//腳位11(PWM)pinMode(inputPin,INPUT);//定義超音波輸入腳位

pinMode(outputPin,OUTPUT);//定義超音波輸出腳位myservo.attach(5);//定義伺服馬達輸出第5腳位(PWM)}45.45.46.47.48.49.50.51.52.53.54.55.56.57.58.59.60.61.62.63.64.65.66.67.68.69.70.71.72.73.74.75.76.77.78.79.80.81.82.83.84.85.86.87.88.voidadvance(inta) //前進{digitalWrite(pinRB,LOW);//使馬達(右后)動作digitalWrite(pinRF,HIGH);digitalWrite(pinLB,LOW);//使馬達(左后)動作digitalWrite(pinLF,HIGH);delay(a*50);}voidright(intb) //右轉(zhuǎn)(單輪){digitalWrite(pinRB,LOW); //使馬達(右后)動作digitalWrite(pinRF,HIGH);digitalWrite(pinLB,HIGH);digitalWrite(pinLF,HIGH);delay(b*20);}voidleft(intc) //左轉(zhuǎn)(單輪){digitalWrite(pinRB,HIGH);digitalWrite(pinRF,HIGH);digitalWrite(pinLB,LOW); //使馬達(左后)動作digitalWrite(pinLF,HIGH);delay(c*20);}voidturnR(intd) //右轉(zhuǎn)(雙輪){digitalWrite(pinRB,LOW);//使馬達(右后)動作digitalWrite(pinRF,HIGH);digitalWrite(pinLB,HIGH);digitalWrite(pinLF,LOW);//使馬達(左前)動作delay(d*15);}voidturnL(inte) //左轉(zhuǎn)(雙輪){digitalWrite(pinRB,HIGH);digitalWrite(pinRF,LOW);//使馬達(右前)動作digitalWrite(pinLB,LOW);//使馬達(左后)動作digitalWrite(pinLF,HIGH);delay(e*15);}voidstopp(intf) //停止{digitalWrite(pinRB,HIGH);

digitalWrite(pinRF,HIGH);digitalWrite(pinLB,HIGH);digitalWrite(pinLF,HIGH);delay(f*50);}voidback(intg) //后退{digitalWrite(pinRB,HIGH); //使馬達(右后)動作digitalWrite(pinRF,LOW);digitalWrite(pinLB,HIGH); //使馬達(左后)動作digitalWrite(pinLF,LOW);89.90.91.92.93.94.95.96.97.98.99.100101102103104105106107108109110111112113114115116117118119120121122123124125126127128delay(g*50);delay(g*50);}voiddetection。{intdelay_timeask_pin_F();if(Fspeedd<5){stopp(1);back(2);}if(Fspeedd<10){stopp(1);ask_pin_L();delay(delay_time);ask_pin_R();delay(delay_time);if(Lspeedd>Rspeedd){directionn=Rgo;}if(Lspeedd<=Rspeedd)//測量3個角度(0.90.179)=250; //伺服馬達轉(zhuǎn)向后的穩(wěn)定時間//讀取前方距離//假如前方距離小于10公分//清除輸出數(shù)據(jù)//后退0.2秒//假如前方距離小于25公分//清除輸出數(shù)據(jù)//讀取左方距離//等待伺服馬達穩(wěn)定//讀取右方距離//等待伺服馬達穩(wěn)定//假如左邊距離大于右邊距離//向右走//假如左邊距離小于或等于右邊距離129.130.131.directionn}130.131.directionn}Lgo;//向左走132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164. if(Lspeedd<5&&Rspeedd<5) //假女口左邊距離和右邊距離皆小于10公分TOC\o"1-5"\h\z. {. directionn=Bgo; //向后走. }. }. else //加如前方不小于(大于)25公分. {. directionn=Fgo; //向前走. }. }.voidask_pin_F()//量出前方距離. {. myservo.write(90);. digitalWrite(outputPin,LOW);//讓超聲波發(fā)射低電壓2ps. delayMicroseconds(2);. digitalWrite(outputPin,HIGH);//讓超聲波發(fā)射高電壓10ps，這里至少是10psdelayMicroseconds(10);digitalWrite(outputPin,LOW);//維持超聲波發(fā)射低電壓差時間floatFdistance=pulseIn(inputPin,HIGH); //讀差相Fdistance=Fdistance/5.8/10;//將時間轉(zhuǎn)為距離距離（單位：公分）分）Serial.print("Fdistance:");//輸出距離（單位：公-Serial.println(Fdistance);//顯示距離Fspeedd=Fdistance;//將距離讀入Fspeedd(前速). }. void ask_pin_L()// 量出左邊距離. {. myservo.write(5);. delay(delay_time);. digitalWrite(outputPin,LOW);//讓超聲波發(fā)射低電壓2ps. delayMicroseconds(2);. digitalWrite(outputPin,HIGH);//讓超聲波發(fā)射高電壓10ps，這里至少是10ps

165.delayMicroseconds(10);166.壓167.差時間digitalWrite(outputPin,LOW); //維持超聲波發(fā)射低電floatLdistance=pulseIn(inputPin,HIGH); //讀差相//將時間轉(zhuǎn)為距離距//輸出距離（單位：公//顯示距離////將時間轉(zhuǎn)為距離距//輸出距離（單位：公//顯示距離//將距離讀入Serial.print("Ldistance:");分)Serial.println(Ldistance);Lspeedd=Ldistance;Lspeedd(左速)}173.voidask_pin_R()//量出右邊距離{myservo.write(177);delay(delay_time);digitalWrite(outputPin,LOW);//讓超聲波發(fā)射低電壓2psdelayMicroseconds(2);179. digitalWrite(outputPin,HIGH);//讓超聲波發(fā)射高電壓10ps，這里至少是10psdelayMicroseconds(10);digitalWrite(outputPin,LOW);//維持超聲波發(fā)射低電壓floatRdistance=pulseIn(inputPin,HIGH);//讀差相差時間//將時間轉(zhuǎn)為距離距//輸出距離（單位：公//顯示距離////將時間轉(zhuǎn)為距離距//輸出距離（單位：公//顯示距離//將距離讀入Serial.print("Rdistance:");分)Serial.println(Rdistance);Rspeedd=Rdistance;Rspeedd(右速)187.188.189.190.voidloop(){191.量myservo.write(90);192.189.190.voidloop(){191.量myservo.write(90);192.detection。；194.車)if(directionn==2)//讓伺服馬達回歸預(yù)備位置準(zhǔn)備下一次的測//測量角度并且判斷要往哪一方向移動//假如directionn（方向）=2（倒195.{196.back(8);//倒退（車）197.turnL(2);//些微向左方移動（防止卡在夕匕巷里）198.Serial.print("Reverse");//顯示方向（倒退）199.}200.if(directionn==6)//假女口directionn(方向）=6（右轉(zhuǎn)）201.{202.back(1);203.turnR(6);//右轉(zhuǎn)204.Serial.print("Right");//顯示方向（左轉(zhuǎn)）205.}206.if(direc