【正文】 w。 // BlackWire[sRow]=(LineMin+RightEnge)/2。 BlackWire[sRow]=LineMin。 sRow=0。 break。//break while }else if((rfind==0)amp。amp。(lfind==1)amp。amp。(LeftEnge+6LineMax)){ EndLine=sRow。 // BlackWire[sRow]=(LineMax+LeftEnge)/2。 BlackWire[sRow]=LineMax。 sRow=0。 break。 }else { // keep BlackWire[sRow]=BlackWire[sRow+1]。 stt=1。 if(++NoLineCtr=3){ EndLine=(sRow20?(sRow2):0)。 sRow=0。 break。 } }// end if /*******************************************************************/// if(stt==1){ if(Esrch==1){ /* 如果允許查找 */ char vl2,vl1,mm0,vr1,vr2,t,tmid。//,tl1,tl2,tr1,tr2。 tmid=BlackWire[sRow]。//(LeftEnge+RightEnge)/2。 t=WireLen/2。 mm0=gmidv(ImageData[sRow][tmid],ImageData[sRow][tmid+1],ImageData[sRow][tmid+2])。 /* 相鄰點平均或者中值濾波 可以去除干擾 */ vl1=gmidv(ImageData[sRow][tmid122],ImageData[sRow][tmid132],ImageData[sRow][tmid112])。 vl2=gmidv(ImageData[sRow][tmid142],ImageData[sRow][tmid152],ImageData[sRow][tmid162])。 vr1=gmidv(ImageData[sRow][tmid+14+2],ImageData[sRow][tmid+15+2],ImageData[sRow][tmid+13+2])。 vr2=gmidv(ImageData[sRow][tmid+18+2],ImageData[sRow][tmid+19+2],ImageData[sRow][tmid+17+2])。 if((mm043)amp。amp。 (cabs(vl1mm0)Valve) amp。amp。(cabs(vl2mm0)Valve) amp。amp。(cabs(vr1mm0)Valve) amp。amp。(cabs(vr2mm0)Valve) ){ bg=sRow。 BeginPointCounter++。 Esrch=0。 Etr=1。 } // end if } // end if // }// end if sRow。}/********** 濾波 2*******/ for(sRow=StartLine1。sRowEndLine+1。 sRow){ BlackWire[sRow]=gmidv(BlackWire[sRow+1],BlackWire[sRow],BlackWire[sRow1])。 CurveX+=BlackWire[sRow]。 }} //end GetBlackWire()/******************************************************************************//*** 獲取賽道信息 ****/void GetRoadInfo(void){ char aveNear。 char aveFar。 char aveMid。 char i。 int aa。 if(StartLineEndLine4){ if(preAngle00){ SteerAngle=SteerRight。 }else if(preAngle00){ SteerAngle=SteerLeft。 }else{ SteerAngle=SteerCenter。 } // SetSpeed(35)。 Curve=100。 return。 } MidLine=(StartLine+EndLine)/2。 aveNear=gmidv(BlackWire[StartLine1],BlackWire[StartLine],BlackWire[StartLine2])。 aveFar=gmidv(BlackWire[EndLine],BlackWire[EndLine+1],BlackWire[EndLine+2])。 aveMid=gmidv(BlackWire[MidLine],BlackWire[MidLine1],BlackWire[MidLine+1])。 K1=aveFaraveMid。 K2=aveMidaveNear。 deltaK12=K1K2。 if((K10amp。amp。K20)||(K10amp。amp。K20)){ TestCurve0=aveFarLineCenter。 }else{ TestCurve0=cabs(deltaK12)。 } TestCurve0=(TestCurve010?10:TestCurve0)。 Angle0=(aveFarLineCenter)*30/(SampledRowMax3EndLine)。 if(Angle0=10||Angle0=10){ Kp=80。 }else if(Angle0=20||Angle0=20){ Kp=130。 }else { Kp=100。 } Angle=Angle0*Kp/100。 TestAngle0=(aveFarLineCenter)*30*10/(SampledRowMax3EndLine)/Interval。 TestMidAngle0=(aveMidLineCenter)*30*10/(MidLineEndLine)/Interval。 define SteerLen (Angle0?SteerRightLen:SteerLeftLen) /****************/ aa=SteerCenterAngle*SteerLen/AngleEnge。 aaa=aa。 strlen1=SteerLen。 if(aaSteerRight){ SteerAngle=SteerRight。 }else if(aaSteerLeft){ SteerAngle=SteerLeft。 }else{ SteerAngle=aa。 } preAngle0=Angle0。 preAngle=Angle。 Curve=0。 aveCurveX=(unsigned char)(CurveX/(StartLineEndLine2))。 // 平均x坐標(biāo) for(i=StartLine1。iEndLine+1。 i){ Curve+=((BlackWire[i]aveCurveX0)? (BlackWire[i]aveCurveX):(aveCurveXBlackWire[i])) 。 //1080 } // Curve 越大，速度應(yīng)該越慢 if(Curve=100){ Curve=100。 } }// end GetRoadInfo() /**********************************************************************************************//** 控制 **//*********************/void SetSteer(void){ PWMDTY45=SteerAngle 。 if(PWMDTY45==SteerRight||PWMDTY45==SteerLeft){ SingEnable=1。 }else{ SingEnable=0。 }}void GetSpeed(void){ char k。 unsigned int GetPulse。 for(k=0。k5。k++){ Pulse[k]=Pulse[k+1]。 } Pulse[5]=PACN10。 GetPulse=Pulse[5]Pulse[2]。 // 2倍 PreSpeed=NowSpeed。 NowSpeed=GetPulse/10。} /* * * * */ void SetSpeed(unsigned char dsrSpd){ unsigned char NowSpeed2。 NowSpeed2=(unsigned char )NowSpeed。 if(MoterStop==1){ if(NowSpeed2=SpeedBackPoint){ PWMDTY2=PWMBottom 。 PWMDTY3=PWMBottom。 }else{ PWMDTY3=SpeedZero。 PWMDTY2=SpeedZero。 } return。 } if(dsrSpd=5){ if(NowSpeed2dsrSpd+5){ PWMDTY3=PWMBottom。 PWMDTY2=PWMBottom。 }else if(NowSpeed2=dsrSpd){ PWMDTY2=SpeedZero。 PWMDTY3=SpeedZero。 }else { PWMDTY3=140。 PWMDTY2=140。 } return。 } if(NowSpeed2(dsrSpd5)){ PWMDTY3=PWMTop。 PWMDTY2=PWMTop。 }else if(NowSpeed2(dsrSpd+5)){ PWMDTY3=PWMBottom。 PWMDTY2=PWMBottom。 }else if((NowSpeed2=(dsrSpd5))amp。amp。(NowSpeed2dsrSpd)){ PWMDTY3= PWMDTY2= }else if((NowSpeed2=(dsrSpd+5))amp。amp。(NowSpeed2=dsrSpd)){ PWMDTY2=SpeedZero。 PWMDTY3=SpeedZero。 } return。}// end SetSpeed()。 /*void SetSpeed(unsigned char dsrSpd){ if(MoterStop==1){ if(NowSpeed=SpeedBackPoint){ PWMDTY2=PWMBottom 。 PWMDTY3=PWMBottom。 }else{ PWMDTY3=SpeedZero。 PWMDTY2=SpeedZero。 } return。 } if(NowSpeeddsrSpd+2){ PWMDTY3=PWMBottom。 PWMDTY2=PWMBottom。 }else if