超声波小车舵机转动避障

/****************************************************************************硬件连接****************************************************************************/#includeAT89x51.H#includeintrins.h#defineSevro_moto_pwmP2_7//接舵机信号端输入PWM信号调节速度#defineECHOP2_4//超声波接口定义#defineTRIGP2_5//超声波接口定义#defineLeft_moto_go{P1_0=1,P1_1=0,P1_2=1,P1_3=0;}//左边两个电机向前走#defineLeft_moto_back{P1_0=0,P1_1=1,P1_2=0,P1_3=1;}//左边两个电机向后转#defineLeft_moto_Stop{P1_0=0,P1_1=0,P1_2=0,P1_3=0;}//左边两个电机停转#defineRight_moto_go{P1_4=1,P1_5=0,P1_6=1,P1_7=0;}//右边两个电机向前走#defineRight_moto_back{P1_4=0,P1_5=1,P1_6=0,P1_7=1;}//右边两个电机向前走#defineRight_moto_Stop{P1_4=0,P1_5=0,P1_6=0,P1_7=0;}//右边两个电机停转unsignedcharconstdiscode[]={0xC0,0xF9,0xA4,0xB0,0x99,0x92,0x82,0xF8,0x80,0x90,0xBF,0xff/*-*/};unsignedcharconstpositon[3]={0xfe,0xfd,0xfb};unsignedchardisbuff[4]={0,0,0,0,};unsignedcharposit=0;unsignedcharpwm_val_left=0;//变量定义unsignedcharpush_val_left=14;//舵机归中，产生约，1.5MS信号unsignedlongS=0;unsignedlongS1=0;unsignedlongS2=0;unsignedlongS3=0;unsignedlongS4=0;unsignedinttime=0;//时间变量unsignedinttimer=0;//延时基准变量unsignedchartimer1=0;//扫描时间变量/************************************************************************/voiddelay(unsignedintk)//延时函数{unsignedintx,y;for(x=0;xk;x++)for(y=0;y2000;y++);}/************************************************************************/voidDisplay(void)//扫描数码管{if(posit==0){P0=(discode[disbuff[posit]])&0x7f;}//产生点else{P0=discode[disbuff[posit]];}if(posit==0){P2_1=0;P2_2=1;P2_3=1;}if(posit==1){P2_1=1;P2_2=0;P2_3=1;}if(posit==2){P2_1=1;P2_2=1;P2_3=0;}if(++posit=3)posit=0;}/************************************************************************/voidStartModule()//启动测距信号{TRIG=1;_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();_nop_();TRIG=0;}/***************************************************/voidConut(void)//计算距离{while(!ECHO);//当RX为零时等待TR0=1;//开启计数while(ECHO);//当RX为1计数并等待TR0=0;//关闭计数time=TH0*256+TL0;//读取脉宽长度TH0=0;TL0=0;S=(time*1.7)/100;//算出来是CMdisbuff[0]=S%1000/100;//更新显示disbuff[1]=S%1000%100/10;disbuff[2]=S%1000%10%10;}/************************************************************************///前速前进voidrun(void){Left_moto_go;//左电机往前走Right_moto_go;//右电机往前走}/************************************************************************///前速后退voidbackrun(void){Left_moto_back;//左电机往前走Right_moto_back;//右电机往前走}/************************************************************************///左转voidleftrun(void){Left_moto_back;//左电机往前走Right_moto_go;//右电机往前走}/************************************************************************///右转voidrightrun(void){Left_moto_go;//左电机往前走Right_moto_back;//右电机往前走}/************************************************************************///STOPvoidstoprun(void){Left_moto_Stop;//左电机停走Right_moto_Stop;//右电机停走}/************************************************************************/voidCOMM(void){push_val_left=5;//舵机向左转90度timer=0;while(timer=4000);//延时400MS让舵机转到其位置StartModule();//启动超声波测距Conut();//计算距离S2=S;push_val_left=23;//舵机向右转90度timer=0;while(timer=4000);//延时400MS让舵机转到其位置StartModule();//启动超声波测距Conut();//计算距离S4=S;push_val_left=14;//舵机归中timer=0;while(timer=4000);//延时400MS让舵机转到其位置StartModule();//启动超声波测距Conut();//计算距离S1=S;if((S220)||(S420))//只要左右各有距离小于，20CM小车后退{backrun();//后退timer=0;while(timer=4000);}if(S2S4){rightrun();//车的左边比车的右边距离小右转timer=0;while(timer=4000);}else{leftrun();//车的左边比车的右边距离大左转timer=0;while(timer=4000);}}/************************************************************************//*PWM调制电机转速*//************************************************************************//*左电机调速*//*调节push_val_left的值改变电机转速,占空比*/voidpwm_Servomoto(void){if(pwm_val_left=push_val_left)Sevro_moto_pwm=1;elseSevro_moto_pwm=0;if(pwm_val_left=200)pwm_val_left=0;}/***************************************************////*TIMER1中断服务子函数产生PWM信号*/voidtime1()interrupt3using2{TH1=(65536-100)/256;//100US定时TL1=(65536-100)%256;timer++;//定时器100US为准。在这个基础上延时pwm_val_left++;pwm_Servomoto();timer1++;//2MS扫一次数码管if(timer1=20){timer1=0;Display();}}/***************************************************////*TIMER0中断服务子函数产生PWM信号*/voidtimer0()interrupt1using0{}/***************************************************/voidmain(void){TMOD=0X11;TH1=(65536-100)/256;//100US定时TL1=(65536-100)%256;TH0=0;TL0=0;TR1=1;ET1=1;ET0=1;EA=1;delay(100);push_val_left=14;//舵机归中while(1)/*无限循环*/{if(timer=1000)//100MS检测启动检测一次{timer=0;StartModule();//启动检测Conut();//计算距离if(S20)//距离小于20CM{stoprun();//小车停止COMM();//方向函数}elseif(S30)//距离大于，30CM往前走run();}}}