機(jī)器人控制程序

*/*//*/*Thisprogramisdesignedbyokbo*//*NanjingUniversityOfTechnology*//*CollegeOfAutomation*/*//*SpeciallyForMyPet->Jyy<-*//*Ver2.1*/*//**/*//*直線行走修正用傳感器返回值定義pho5->(左),pho6->(右)*/intpho5=0;intpho6=0;intdif_pho56=0;/*pho5pho6光敏相差的標(biāo)準(zhǔn)值*/intstad_dif_pho5=20;/*<事先測(cè)定*/intstad_dif_pho6=20;/*<事先測(cè)定*//*直線行走偏轉(zhuǎn)標(biāo)志位*/intflag_right=0;intflag_left=0;intpho1=0;intpho2=0;intdif_pho12=0;intsum_pho12=0;/*光敏相差的標(biāo)準(zhǔn)值*/intstad_dif_pho1=20;/*<事先測(cè)定*/intstad_dif_pho2=20;/*<事先測(cè)定*/intstad_sum_pho12=20;/*<事先測(cè)定*//*轉(zhuǎn)彎判斷偏轉(zhuǎn)標(biāo)志位*/intflag_left_line=0;intflag_right_line=0;intflag_double_line=0;/*以下是定義電機(jī)的轉(zhuǎn)速*/intleft_vel=0;intright_vel=0;/*直線及直線行走修正函數(shù)*/voidnormal(){left_vel=13;right_vel=13;motor(1,left_vel);motor(2,right_vel);wait(0.1);}voidmodif_right(){left_vel=17;right_vel=10;motor(1,left_vel);motor(2,right_vel);wait(0.15);}voidmodif_left(){left_vel=10;right_vel=17;motor(1,left_vel);motor(2,right_vel);wait(0.15);}/*方案一路線數(shù)組intpath[]={2,1,1,2,0,2,0,1,1,1,1,1,1,2,2,2,2,2,1,1,0,0,3,2,2,0,4,0,2,3,4,2,2,2,0,2,2,1,1,1,1,1,0,0,2,0,0,0,0};inti=0;*//*方案二路線數(shù)組*/intpath[]={2,1,1,2,0,2,0,1,1,1,1,1,1,2,2,0,2,2,2,1,2,2,2,0,2,2,1,1,1,1,1,0,0,2,0,1,1,1,2,2,2,2,0,2,2,2,1,1,0,0,3,2,2,0,4,2,1,0,0};inti=0;/*轉(zhuǎn)彎函數(shù)*/voidturn(intb){/*b為0時(shí)繼續(xù)直線行走*/if(b==0){return;}/*b為1左轉(zhuǎn)90*/if(b==1){stop();left_vel=-20;right_vel=20;motor(1,left_vel);motor(2,right_vel);wait(0.53);}/*b為2右轉(zhuǎn)90*/if(b==2){stop();left_vel=20;right_vel=-20;motor(1,left_vel);motor(2,right_vel);wait(0.53);}/*b為3左轉(zhuǎn)30*/if(b==3){stop();left_vel=-20;right_vel=20;motor(1,left_vel);motor(2,right_vel);wait(0.18);}/*b為4右轉(zhuǎn)30*/if(b==4){stop();left_vel=20;right_vel=-20;motor(1,left_vel);motor(2,right_vel);wait(0.18);}/*反轉(zhuǎn)*/if(b==5){stop();motor(1,20);motor(2,-20);wait(0.9);return;}/*以下是主函數(shù)*/voidmain(){while(1){normal();pho5=analogport(5);pho6=analogport(6);dif_pho56=pho5-pho6;pho1=photo(1);pho2=photo(2);dif_pho12=pho1-pho2;sum_pho12=pho1+pho2;/*取絕對(duì)值*/if(pho5<pho6){dif_pho56=-dif_pho56;}if(pho1<pho6){dif_pho56=-dif_pho12;}/*轉(zhuǎn)彎判斷*/if((dif_pho12>stad_dif_pho1)&&(pho1>pho2)){if(flag_right_line!=3){flag_right_line++;}if(flag_right==3){turn(i);i++;flag_right_line=0;flag_left_line=0;flag_double_line=0;}}if((dif_pho12>stad_dif_pho2)&&(pho2>pho1)){if(flag_left_line!=3){flag_left_line++;}if(flag_left==3){turn(i);i++;flag_right_line=0;flag_left_line=0;flag_double_line=0;}}if(sum_pho12>stad_sum_pho12)if(flag_double_line!=3){flag_double_line++;}if(flag_double_line==3){turn(i);i++;flag_right_line=0;flag_left_line=0;flag_double_line=0;}}/*右邊傳感器探測(cè)到白線，pho5大于pho6左輪加速*/if((dif_pho56>stad_dif_pho5)&&(pho5>pho6)){if(flag_right!=3){flag_right++;}if(flag_right==3){modif_right();flag_right=0;}}/*左邊傳感器探測(cè)到白