超声波避障程序

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./*程序的开头要描述你的设备再来要说是脽写的日期版本然后碰到问题一次一次都要写清楚马达几个驱动电路都要描述L=左R=右F=刖B=后*/#include<Servo.h>intpinLB=6;intpinLF=9;//定义8脚位左后//定义9脚位左刖intpinRB=10;//定义10脚位右后intpinRF=11;//定义11脚位右刖intinputPin=A0;//定义超音波信号接收脚位intoutputPin=A1;//定义超音波信号发射脚位intFspeedd=0;//前速intRspeedd=0;//右速intLspeedd=0;//左速intdirectionn=0; //前=8后=2左=4右=6Servomyservo;//设myservointdelaytime=250;//伺服马达转向后的稳定时间intFgo=8;//前进intRgo=6;//右转intLgo=4;//左转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) //右转(单轮){digitalWrite(pinRB,LOW); //使马达(右后)动作digitalWrite(pinRF,HIGH);digitalWrite(pinLB,HIGH);digitalWrite(pinLF,HIGH);delay(b*20);}voidleft(intc) //左转(单轮){digitalWrite(pinRB,HIGH);digitalWrite(pinRF,HIGH);digitalWrite(pinLB,LOW); //使马达(左后)动作digitalWrite(pinLF,HIGH);delay(c*20);}voidturnR(intd) //右转(双轮){digitalWrite(pinRB,LOW);//使马达(右后)动作digitalWrite(pinRF,HIGH);digitalWrite(pinLB,HIGH);digitalWrite(pinLF,LOW);//使马达(左前)动作delay(d*15);}voidturnL(inte) //左转(双轮){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; //伺服马达转向后的稳定时间//读取前方距离//假如前方距离小于10公分//清除输出数据//后退0.2秒//假如前方距离小于25公分//清除输出数据//读取左方距离//等待伺服马达稳定//读取右方距离//等待伺服马达稳定//假如左边距离大于右边距离//向右走//假如左边距离小于或等于右边距离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);//让超声波发射低电压2ps. delayMicroseconds(2);. digitalWrite(outputPin,HIGH);//让超声波发射高电压10ps，这里至少是10psdelayMicroseconds(10);digitalWrite(outputPin,LOW);//维持超声波发射低电压差时间floatFdistance=pulseIn(inputPin,HIGH); //读差相Fdistance=Fdistance/5.8/10;//将时间转为距离距离（单位：公分）分）Serial.print("Fdistance:");//输出距离（单位：公-Serial.println(Fdistance);//显示距离Fspeedd=Fdistance;//将距离读入Fspeedd(前速). }. void ask_pin_L()// 量出左边距离. {. myservo.write(5);. delay(delay_time);. digitalWrite(outputPin,LOW);//让超声波发射低电压2ps. delayMicroseconds(2);. digitalWrite(outputPin,HIGH);//让超声波发射高电压10ps，这里至少是10ps

165.delayMicroseconds(10);166.压167.差时间digitalWrite(outputPin,LOW); //维持超声波发射低电floatLdistance=pulseIn(inputPin,HIGH); //读差相//将时间转为距离距//输出距离（单位：公//显示距离////将时间转为距离距//输出距离（单位：公//显示距离//将距离读入Serial.print("Ldistance:");分)Serial.println(Ldistance);Lspeedd=Ldistance;Lspeedd(左速)}173.voidask_pin_R()//量出右边距离{myservo.write(177);delay(delay_time);digitalWrite(outputPin,LOW);//让超声波发射低电压2psdelayMicroseconds(2);179. digitalWrite(outputPin,HIGH);//让超声波发射高电压10ps，这里至少是10psdelayMicroseconds(10);digitalWrite(outputPin,LOW);//维持超声波发射低电压floatRdistance=pulseIn(inputPin,HIGH);//读差相差时间//将时间转为距离距//输出距离（单位：公//显示距离////将时间转为距离距//输出距离（单位：公//显示距离//将距离读入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)//让伺服马达回归预备位置准备下一次的测//测量角度并且判断要往哪一方向移动//假如directionn（方向）=2（倒195.{196.back(8);//倒退（车）197.turnL(2);//些微向左方移动（防止卡在夕匕巷里）198.Serial.print("Reverse");//显示方向（倒退）199.}200.if(directionn==6)//假女口directionn(方向）=6（右转）201.{202.back(1);203.turnR(6);//右转204.Serial.print("Right");//显示方向（左转）205.}206.if(direc