if(hongwai6==0)ATDH[6]=0;else ATDH[6]=1; }
/****************边界值提取函数**********/ void laser_chuli()
{ saomiao();
for(j=1;j<=9;j++){ if(ATDR[j]==1)numr++;} //计算上排激光黑点个数 for(i=1;i<=9;i++){ if(ATDL[i]==1)numl++;}
for(j=9;j>=0;j--){ if(j==0) {rbianjie=0;break;}if(*(ATDR+j)==1){rbianjie=j;;break;}} //上排激光取边界值
for(i=9;i>=0;i--){ if(i==0) {lbianjie=0;break;}if(*(ATDL+i)==1){lbianjie=i;;break;}} }
/****************边界值累加求和函数***********/ void bianjie_sum() {
/**************10次边界值求和****************************/
*INCR=rbianjie+*save_rbianjie+*(save_rbianjie+1)+*(save_rbianjie+2)+*(save_rbianjie+3)+*(s ave_rbianjie+4)+*(save_rbianjie+5)+*(save_rbianjie+6)+*(save_rbianjie+7)+*(save_rbianjie+8);
*INCL=lbianjie+*save_lbianjie+*(save_lbianjie+1)+*(save_lbianjie+2)+*(save_lbianjie+3)+*(save_lbianjie+4)+*(save_lbianjie+5)+*(save_lbianjie+6)+*(save_lbianjie+7)+*(save_lbianjie+8); *INC=*INCR+*INCL;
*(save_rbianjie+9)=*save_rbianjie;*(save_rbianjie+8)=*save_rbianjie; //存储10次边界值
*(save_rbianjie+7)=*save_rbianjie;*(save_rbianjie+6)=*save_rbianjie; *(save_rbianjie+5)=*save_rbianjie;*(save_rbianjie+4)=*save_rbianjie; *(save_rbianjie+3)=*save_rbianjie;*(save_rbianjie+2)=*save_rbianjie; *(save_rbianjie+1)=*save_rbianjie;*save_rbianjie=rbianjie;
*(save_lbianjie+9)=*save_lbianjie;*(save_lbianjie+8)=*save_lbianjie; *(save_lbianjie+7)=*save_lbianjie;*(save_lbianjie+6)=*save_lbianjie; *(save_lbianjie+5)=*save_lbianjie;*(save_lbianjie+4)=*save_lbianjie; *(save_lbianjie+3)=*save_lbianjie;*(save_lbianjie+2)=*save_lbianjie; *(save_lbianjie+1)=*save_lbianjie;*save_lbianjie=lbianjie;
if((numr>4)&&(numl>4)){start_judge_flag=1;hei++;}//左右两边黑点个数大于5赋标志位 ,坡道识别
else{start_judge_flag=1;hei=0;}
if(hei>10){po_judge_flag=1;} else{po_judge_flag=0;} numr=0;numl=0; }
/****************边界处理函数***********/ void bianjie_value(void) //激光灯扫描 {
laser_chuli(); check_start();
28
/****************十字处理*******************/
if(*INCR<10||*INCL<10)bai_counts++; else bai_counts=0; //十字处理，累加白道 if(bai_counts>200) {shizi_flag=1;if(shizi_flag==1) lvbo_flag=0;}else shizi_flag=0;
if(lvbo_flag==1) //十字不滤波
{if(abs(*save_rbianjie-rbianjie)>2) rbianjie=*save_rbianjie; //滤波 if(abs(*save_lbianjie-lbianjie)>2) lbianjie=*save_lbianjie;} lvbo_flag=1;
if((*(save_rbianjie)==9)&&(rbianjie!=8)&&(rbianjie!=7)&&(rbianjie!=6))
{ rbianjie=9;lbianjie=0; }//保持
if((*(save_lbianjie)==9)&&(lbianjie!=8)&&(lbianjie!=7)&&(lbianjie!=6))
{ lbianjie=9;rbianjie=0; } bianjie_sum(); }
/****************起跑线识别**********************/ void check_start(void) {
/**************起跑线******************************/
if((ATDH[1]|ATDH[2])==1&&(ATDH[5]==1|ATDH[6])==1)start_flag=1; if(start_flag==1)start_counts++;
if(start_counts>=1000)start_counts=1000; if(start_counts>=1000)
if((ATDH[1]|ATDH[2])==1&&(ATDH[5]|ATDH[6])==1)start=1; if(start==1) { PWMDTY45=10;PWMDTY67=0;DisableInterrupts;}
}
/****************道路判断**********************/ void path_judge() {
if((*INCR<35||*INCL<35)&&(pulse_counts>0))Straight_counts++;else Straight_counts=0;
if(Straight_counts>10) Straight_flag1[0]=1; else Straight_flag1[0]=0 ; if(Straight_counts>30) Straight_flag2[0]=1; else {Straight_flag2[0]=0;} if(Straight_counts>60) Straight_flag3[0]=1; else {Straight_flag3[0]=0;} if(Straight_counts>80) Straight_flag4[0]=1; else {Straight_flag4[0]=0;} if(Straight_counts>150) Straight_flag5[0]=1; else {Straight_flag5[0]=0;}
if((*INC)>=70&&(pulse_counts>pulse_counts2)) //刹车1 {turn2=25;rrr=0;}
if((*INC)>45&&(pulse_counts>pulse_counts1)) //刹车2 {turn=40;rrr=0;}
}
/****************速度值设定**********************/ Void PID()
{ek3=ek2;
ek2=ek1;
29
ek1= Speed_Set t-pulse_counts;
speed_value+=KP*(ek1-ek2)+KI*ek1+KD*(ek1-2*ek2+ek3);
if(speed_value<50) speed_value=50;
if(speed_value>150) speed_value=150;
}
void speed_given()
{ if(Straight_flag1[0]==1) Speed_Set=speed_set11; //直道速度设定 if(Straight_flag2[0]==1) Speed_Set=speed_set11+30; if(Straight_flag3[0]==1) Speed_Set=speed_set11+50; if(Straight_flag4[0]==1) Speed_Set=speed_set11+70; if(Straight_flag5[0]==1) Speed_Set=speed_set11+70;
if(((*INCR)>40&&(*INCR)<=60) ||((*INCL)>40&&(*INCL)<=60)) Speed_Set=speed_set2+40;
if(((*INCR)>60&&(*INCR)<=80) ||((*INCL)>60&&(*INCL)<=80)) Speed_Set=speed_set2+20;
if(((*INCR)>80&&(*INCR)<=90) ||((*INCL)>80&&(*INCL)<=90)) Speed_Set=speed_set2; }
void jiansu() {
if(pulse_counts>50&&*INC>80){PWMDTY67=50;}
if(rrr==0)
{ if(turn>0) {shache1();}
if(turn2>0&&turn==0){shache2();}} else PWMDTY45=0; }
/****************转向舵机控制函数***********/
void dj_control(void) {
if(po_judge_flag==1)PWMDTY01=2160; bianjie=rbianjie-lbianjie; switch(bianjie) {
case 0: PWMDTY01_Temp=2160 ;break; case 1: PWMDTY01_Temp=2160+10 ;break; case 2: PWMDTY01_Temp=2170+20 ;break; case 3: PWMDTY01_Temp=2190+30;break; case 4: PWMDTY01_Temp=2220+40 ;break; case 5: PWMDTY01_Temp=2260+50 ;break; case 6: PWMDTY01_Temp=2310+70;break; case 7: PWMDTY01_Temp=2380+80 ;break;
30
case 8: PWMDTY01_Temp=2460+90 ;break; case 9: PWMDTY01_Temp=2550 ;break;
case -1:PWMDTY01_Temp=2160-10 ;break; case -2:PWMDTY01_Temp=2150-20 ;break; case -3:PWMDTY01_Temp=2130-30 ;break; case -4:PWMDTY01_Temp=2100-40 ;break; case -5:PWMDTY01_Temp=2060-50;break; case -6:PWMDTY01_Temp=2010-70;break; case -7:PWMDTY01_Temp=1940-80 ;break; case -8:PWMDTY01_Temp=1860-90;break; case -9:PWMDTY01_Temp=1750;break; }
if(pwmdty01>2550) pwmdty01=2550;//轮子右极限,左激光在黑线上 if(pwmdty01<1750) pwmdty01=1750;//轮子左极限,右激光在黑线上 PWMDTY01=PWMDTY01_Temp;
}
void main(void) {
/* put your own code here */
DisableInterrupts;
SET_PLL(); Io_Init(); Servo_Init (); Forward_Init(); delay2s();
PIT_Init();//定时10ms初始化 TIM_Init();//脉冲累计初始化 qidong();
EnableInterrupts;
for(;;){ _FEED_COP();/* feeds the dog */ elect_boma(); bianjie_value(); dj_control(); path_judge(); } }
/***********************测速***********************/ #pragma CODE_SEG NON_BANKED void interrupt 66 PIT0(void) {
PITTF_PTF0=1;
pulse_counts=PACNT; //PT7接受脉冲 PACNT=0;
speed_given();
31
drive(); jiansu(); }
#pragma CODE_SEG DEFAULT
32
