【正文】 ,D=70。 //差速減速變量/***************************MAIN設(shè)置變量*************************************/void Setup_val(void){ if(!setup1_GetVal()) //調(diào)試模式 { LED1_PutVal(0)。 Dat_P=。 //電機(jī)I Dat_D=。 //舵機(jī)A B=35。 //舵機(jī)D Pc=。 //減速 needforspeed=80。 //最高速度 lowspeed=55。 //棒棒閾值 Stop_Enable()。 //禁用起跑線掃描 } else LED2_PutVal(1)。 Dat_P=。 //電機(jī)I Dat_D=。 //舵機(jī)A B=35。 //舵機(jī)D Pc=。 //減速 needforspeed=90。 //最高速度 lowspeed=60。 //棒棒閾值 Stop_Enable()。 //禁用起跑線掃描 } else LED3_PutVal(1)。 Dat_P=。 //電機(jī)I Dat_D=。 //舵機(jī)A B=35。 //舵機(jī)D Pc=。 //減速 needforspeed=95。 //最高速度 lowspeed=60。 //棒棒閾值 Stop_Enable()。 //禁用起跑線掃描 } /**********************X*********************/ else LED4_PutVal(1)。}/***************MAIN二值化amp。 //掃描位置變量 unsigned char static CoordinateLeft=65。 //掃描右起點(diǎn) unsigned char static flagleft=0。 //左邊丟線標(biāo)志位 unsigned char static flagright=0。 //右邊丟線標(biāo)志位 unsigned char static HistoryData[10]={63,63,63,63,63,63,63,63,63,63}。 //歷史數(shù)組 unsigned char static HistoryDataR[10]={63,63,63,63,63,63,63,63,63,63}。 unsigned char static blockflag。 =55 = Dat_P。 //速度控制I變量 = Dat_D。i0。 break。i127。 break。 } else lostright=0。 } else lostleft=0。amp。 =HistoryData[6]。 //中線偏差計(jì)算 if(HistoryData[5]67 amp。 HistoryData[5]53) { CoordinateLeft = 61。 } else if(HistoryData[6]60) { CoordinateLeft = 65。 //重置右邊掃描位置初值 } else { CoordinateLeft = 55。 //重置右邊掃描位置初值 } }/******************************補(bǔ)線處理*********************************/ else { lostflag=0。 //線性補(bǔ)線 } else if(20) { =。 //計(jì)算實(shí)際中線位置 } CoordinateLeft = (+)/2。//重置右邊掃描位置初值 =。 HistoryData[8]=HistoryData[7]。 HistoryData[6]=HistoryData[5]。 HistoryData[4]=HistoryData[3]。 HistoryData[2]=HistoryData[1]。 HistoryData[0]=。 HistoryDataL[8]=HistoryDataL[7]。 HistoryDataL[6]=HistoryDataL[5]。 HistoryDataL[4]=HistoryDataL[3]。 HistoryDataL[2]=HistoryDataL[1]。 HistoryDataL[0]=。 HistoryDataR[8]=HistoryDataR[7]。 HistoryDataR[6]=HistoryDataR[5]。 HistoryDataR[4]=HistoryDataR[3]。 HistoryDataR[2]=HistoryDataR[1]。 HistoryDataR[0]=。 if(Distance30 amp。 Distance10) { if(70 amp。 60 amp。 46 amp。 abs([3])5) blockflag=1。amp。amp。amp。 else blockflag=0。 } /*************************障礙處理******************************/ if(blockflag || blockdelayflag1) { if(blockflag) { beep_PutVal(0)。 } if(blockdelayflag0) blockdelayflag。 //中線偏差計(jì)算 else =*()。 //中線偏差計(jì)算 else =*()。 //中線偏差計(jì)算 beep_PutVal(1)。 //flag清零 flagright=0。//KP動態(tài) = D。 //MAIN舵機(jī)PD算法 if(=()) //舵機(jī)限幅 { Steering_SetRatio16()。 }else { Steering_SetRatio16()。 //記錄上一次的偏差 //LcdWriteNum(0,0,)。 //LcdWriteNum1(2,50,)。 /**************************電機(jī)控制******************************/ if(((abs(HistoryData[9]60)5) amp。 (97 || 23) amp。 !lostflag) || delayflag) { delayflag=1。 if(60) slowflag=2。 if(abs(HistoryData[9]60)19 || (95 amp。 25) || lostflag || 40||40) delayflag=0。 slowflag=0。 //左邊線標(biāo)志位 unsigned char i。i0。 if(flagleft=3) { =i+3。 } else =0。 } flagleft=0。 unsigned char i,j。 if(TimerCnt10ms = 10) { TimerCnt10ms=0。 //編碼器計(jì)數(shù)結(jié)算 ImageCapture()。 //CCD2采集 BlackCapture(,)?？刂扑惴? //BlackCaptureCar()。 //電機(jī)控制 Setup_val()。 if(j=4) { j=0。i127。 } CcdUart_SendBlock(,132,amp。 } /***********************************************/ }}/*************************差速控制函數(shù)************************************/void MotorSpeedcontrol(void){ unsigned char static x=0。 = needforspeedPc*(unsigned char)(abs( )/Pj)。 } if(lowspeed) { =lowspeed。 = needforspeed+Pc*(unsigned char)(abs( )/Pj)。 } if(lowspeed) { =lowspeed。 Motor1_1_PutVal(0)。 if(x70) { Motor1_SetRatio8(5)。 } else { Motor1_SetRatio8(5)。 } if(x80)x=75。 Motor2_1_PutVal(1)。 Motor2_SetRatio8(230)。 Motor2_1_PutVal(1)。 Motor2_SetRatio8(170)。 Motor2_1_PutVal(0)。 Motor2_SetRatio8(PidRealize_M2())。 //上次偏差 signed int static Historyerror1_2。 //速度誤差計(jì)算 if(bangbangset) //棒棒控制判斷 { +=*()+*+ \ *(*Historyerror1_1+Historyerror1_2)。 //棒棒控制 } Historyerror1_2=Historyerror1_1。 //記錄上次偏差 if(=250)= return 。 //上次偏差 signed int static Historyerror2_2。 //速度誤差計(jì)算 if(bangbangset) //棒棒控制判斷 { +=*()+*+ \ *(*Historyerror2_1+Historyerror2_2)。 //棒棒控制 } Historyerror2_2=Historyerror2_1。 //記錄上次偏差 if(=250)= return 。}/********************初始化程序***********************/void Initialize(void){ TI1_Disable()。 CcdA0_Start()。 TI1_Enable()。 Stop_Enable()。 LED2_PutVal(1)。 LED4_PutVal(1)。}/***********************延時(shí)函數(shù)************************/void DelayModule(unsigned long time) { while(time) { __asm(nop)。 Time++。 [1] = ~2。 [131] = 2。 = *Motor2Speed。 Motor2Speed = 0。 unsigned char Num。p)。 BaQi[0] = 0。 BaQi[0] = 0。 BaQi[0] = 0。 BaQi[0] = 0。 BaQi[0] = 0。 BaQi[0]