机器人控制程序

*/*//*/*Thisprogramisdesignedbyokbo*//*NanjingUniversityOfTechnology*//*CollegeOfAutomation*/*//*SpeciallyForMyPet->Jyy<-*//*Ver2.1*/*//**/*//*直线行走修正用传感器返回值定义pho5->(左),pho6->(右)*/intpho5=0;intpho6=0;intdif_pho56=0;/*pho5pho6光敏相差的标准值*/intstad_dif_pho5=20;/*<事先测定*/intstad_dif_pho6=20;/*<事先测定*//*直线行走偏转标志位*/intflag_right=0;intflag_left=0;intpho1=0;intpho2=0;intdif_pho12=0;intsum_pho12=0;/*光敏相差的标准值*/intstad_dif_pho1=20;/*<事先测定*/intstad_dif_pho2=20;/*<事先测定*/intstad_sum_pho12=20;/*<事先测定*//*转弯判断偏转标志位*/intflag_left_line=0;intflag_right_line=0;intflag_double_line=0;/*以下是定义电机的转速*/intleft_vel=0;intright_vel=0;/*直线及直线行走修正函数*/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);}/*方案一路线数组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;*//*方案二路线数组*/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;/*转弯函数*/voidturn(intb){/*b为0时继续直线行走*/if(b==0){return;}/*b为1左转90*/if(b==1){stop();left_vel=-20;right_vel=20;motor(1,left_vel);motor(2,right_vel);wait(0.53);}/*b为2右转90*/if(b==2){stop();left_vel=20;right_vel=-20;motor(1,left_vel);motor(2,right_vel);wait(0.53);}/*b为3左转30*/if(b==3){stop();left_vel=-20;right_vel=20;motor(1,left_vel);motor(2,right_vel);wait(0.18);}/*b为4右转30*/if(b==4){stop();left_vel=20;right_vel=-20;motor(1,left_vel);motor(2,right_vel);wait(0.18);}/*反转*/if(b==5){stop();motor(1,20);motor(2,-20);wait(0.9);return;}/*以下是主函数*/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;/*取绝对值*/if(pho5<pho6){dif_pho56=-dif_pho56;}if(pho1<pho6){dif_pho56=-dif_pho12;}/*转弯判断*/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;}}/*右边传感器探测到白线，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;}}/*左边传感器探测到白