代碼:

1. #include "include.h"
   
2. #include "calculation.h"
   
3. #include"VisualScope.h"
   
4. #include "CCD.h"
   
5. #include "LQ12864.h"
   
6. s16 INT_COUNT=100;          //LPT 產(chǎn)生中斷的計(jì)數(shù)次數(shù)
   
7. extern u32 LPT_INT_count;
   
8. s16 count;
   
9. #define duoji_center 2400//*舵機(jī)中值，根據(jù)自己的小車自行修改
   
10. /*  接口：PWM：PTC1，PTC2，PTC3，PTC4
    
11.           舵機(jī)：PTA8
    
12.          攝像頭：行中斷：PTD13，場中斷：PTC8，PCLK：PTD12，數(shù)據(jù)端：PTD0~PTD7
    
13. 帶有//*的是需要修改的參數(shù)  關(guān)于下面的P、I、D最好先清零，即SpeedKP=0,SpeedKI=0,TurnP=0,TurnD=0*/
    
14. //PID參數(shù)
    
15. float TurnP=4.4,TurnD=0.6;//*轉(zhuǎn)向PD
    
16. float SpeedKP =6;//*速度P  過大，車輪旋轉(zhuǎn)一前轉(zhuǎn)一后轉(zhuǎn)，表現(xiàn)為走走停停
    
17. float SpeedKI =1;//*速度I
    
18. float g_Speedgoal=500;//*目標(biāo)速度 即編碼器的目標(biāo)脈沖值
    
19. //時(shí)間標(biāo)志位
    
20. extern u8 TIME1flag_100ms,flag_1ms ;
    
21. extern u8 TIME1flag_1s   ;  //PT1口1s定時(shí)標(biāo)志位
    
22. int start_flag=0,stop_jiasu=0;//開機(jī)靜止跑道2秒標(biāo)志
    
23. s32 ATimeCount=0;//100ms進(jìn)入pid標(biāo)志
    
24. s32 TimeCount ;//1ms中斷標(biāo)志
    
25. //速度變量
    
26. s16 g_nLeftMotorPulse,g_nRightMotorPulse,g_nLeftMotorPulseSigma,g_nRightMotorPulseSigma,lm,rm;
    
27. extern s32 LeftMotorOut ,RinghtMotorOut ;
    
28. float g_SpeedControlOutNew,g_SpeedControlOutOld ;
    
29. s16 SpeedPeriodCount=0 ;
    
30. s32 SpeedPWM = 0 ;
    
31. s32 LastSpeedPWM = 0 ;
    
32. s32 MotorSpeedPWM,MotorTurnPWM ;
    
33. s32 PWMout ;
    
34. s16 zhidao=1,wandao=0;
    
35. //CCD變量
    
36. extern s32  g_SpeedControlIntegral;
    
37. s16 TurnPeriodCount=0 ;
    
38. s16 TurnPWMOUT=0 ;
    
39. s16 LastTurnPWMOUT=0;
    
40. int sudu_xianshi=0;
    
41. void run();
    
42. void main()
    
43. {
    
44.    DisableInterrupts; //禁止總中斷
    
45.    uart_init (UART0,9600);//初始化UART0，串口頻率 9600
    
46.    FTM2_QUAD_Iint();//一路正交解碼 A相---PTA10 B相---PTA11
    
47.    gpio_init (PORTA , 17, GPO,HIGH); //程序運(yùn)行燈
    
48.   // oled_init();
    
49.    CCD_init ();  //攝像頭初始化
    
50.    pit_init_ms(PIT0, 1);  //初始化PIT0，定時(shí)時(shí)間為： 1ms
    
51.    pit_init_ms(PIT1, 100);//初始化PIT1，定時(shí)時(shí)間為： 100ms
    
52.    set_irq_priority (90,3);//行中斷
    
53.    set_irq_priority (89,2);//場中斷
    
54.    set_irq_priority (84,1);//PIT0中斷 優(yōu)先級(jí)最高
    
55.    FTM_init() ; //FTM初始化
    
56.    lptmr_counter_init(LPT0_ALT2,INT_COUNT,2,LPT_Rising);//PTC5腳 脈沖累加計(jì)數(shù)器
    
57.    EnableInterrupts;//開總中斷  
    
58.    uart_irq_EN(UART0);//開串口0中斷
    
59.    while(1)
    
60.     {
    
61.      if(flag_1ms==1)//運(yùn)行run函數(shù)
    
62.      {
    
63.        flag_1ms=0;
    
64.        run();
    
65.      }
    
66.    // display_suducaiji();//速度顯示函數(shù)
    
67.   }
    
68. }
    
69. void run()//在isr.c中的1ms中斷中調(diào)用  PIT0_IRQHandler
    
70. {   
    
71.   TimeCount++ ;
    
72.   SpeedPeriodCount++;
    
73.   TurnPeriodCount++;
    
74.   MotorTurnPWM  = TurnPWMOut(TurnPWMOUT,LastTurnPWMOUT,TurnPeriodCount) ;
    
75.   MotorSpeedPWM = SpeedPWMOut(g_SpeedControlOutNew ,g_SpeedControlOutOld,SpeedPeriodCount);  
    
76.   if(TimeCount>=5)//讀速度  5us
    
77.   {
    
78.     TimeCount=0;
    
79.     GetMotorPulse();//正交解碼讀取其中一路速度
    
80.   }  
    
81.   else if(TimeCount == 1)//為空
    
82.   {     
    
83.     PWMout=MotorSpeedOut(MotorSpeedPWM);//電機(jī)輸出
    
84.   }
    
85.    else if(TimeCount == 2)//速度控制
    
86.    {
    
87.      ATimeCount ++ ;
    
88.      if(ATimeCount >= 20)
    
89.      {
    
90.        ATimeCount=0;
    
91.        sudu_xianshi=1;//采集完速度的標(biāo)志，用于向上位機(jī)發(fā)送速度
    
92.        count =get_counter_value(); //讀取LPTMR0_CNR之前需要先對(duì)這個(gè)寄存器賦值�。�！
    
93.        lptmr_counter_clean(); //清空脈沖計(jì)數(shù)器計(jì)算值（馬上清空，這樣才能保證計(jì)數(shù)值準(zhǔn)確）
    
94.        g_nLeftMotorPulseSigma=LPT_INT_count*INT_COUNT+count;//每LPT_INT_count個(gè)脈沖產(chǎn)生一次中斷，LPT_INT_count為中斷的次數(shù)，
    
95.        if(PWMout>0)//由于這一路沒有使用正交解碼，因此速度脈沖方向由電機(jī)方向判定
    
96.          g_nLeftMotorPulseSigma=-g_nLeftMotorPulseSigma;
    
97.        LPT_INT_count=0;//對(duì)中斷次數(shù)清零 //count為不足產(chǎn)生一次中斷的值
    
98.        lm=g_nLeftMotorPulseSigma;//獲得左、右輪速度用于上位機(jī)顯示
    
99.        rm=g_nRightMotorPulseSigma;
    
100.        SpeedPID() ; //速度PI調(diào)節(jié)
     
101.        SpeedPeriodCount = 0 ;
     
102.      }
     
103.    }
     
104.   else if(TimeCount == 3)//方向控制
     
105.   {
     
106.      FTM_CnV_REG(FTMx[FTM1], CH0) =MotorTurnPWM+duoji_center;//方向控制  要加上舵機(jī)中值
     
107.      LastTurnPWMOUT = TurnPWMOUT ;
     
108.      TurnPeriodCount = 0 ;
     
109.   }  
     
110.   else if(TimeCount == 4)
     
111.   {
     
112.     if(TIME1flag_1s == 1)//程序運(yùn)行燈
     
113.     {
     
114.       TIME1flag_1s=0;
     
115.       PTA17_OUT=~PTA17_OUT;
     
116.     }
     
117.   }
     
118. }
     
119. 
     

代碼:

1. #include "common.h"
   
2. #include "include.h"
   
3. #include "calculation.h"
   
4. #include "CCD.h"
   
5. #include"VisualScope.h"
   
6. #include "LQ12864.h"
   
7. extern s16 zhidao,wandao;  
   
8. float S_xy=0,S_xx=0,x=0,y=0,S_y=0,S_x=0;//算斜率的
   
9. int slope=0;
   
10. int youbian_ysy=0,jinduan_ybx=0,zuobian_ysy=0,jinduan_zbx=0,ztuichu=0;//擬合中線
    
11. int yici_zuo=0,yici_you=0;
    
12. int zuoqishi=0,youqishi=0,zzxqishi=0,yzxqishi=0,zxqishi=0,zstop=0,ystop=0;
    
13. int yx_left=0,yx_right=0;//根據(jù)近端中線方向判別斷行處應(yīng)該具有的方向
    
14. int youbian_qi=0,zuobian_qi=0;//判斷是否為彎道
    
15. int zuobian_youxiao=0,zuobian_stop=0,youwan_youxiao=0,youwan_flag=0,youwan_center=0,youwan_centerstop=0;
    
16. int youbian_youxiao=0,youbian_stop=0,zuowan_youxiao=0,zuowan_flag=0,zuowan_center=0,zuowan_centerstop=0;
    
17. int value=0,wandao_youxiao=0,zuowan=0,youwan=0;
    
18. int zhangai_qishi=0,zhangai_jieshu=0;//障礙起始  可惜沒用上
    
19. int start_y=0,yous=0,stop_z=0,start_z=0,zuos=0,stop_y=0,zuozs=0,youzs=0,youss=0,zuoss=0;//十字 有點(diǎn)復(fù)雜
    
20. int left_sz=0,right_sz=0,left_xsz=0,right_xsz=0;
    
21. int zshizi=0,zxshizi=0,yxshizi=0;
    
22. int xieshizi_zuoflag=0,youxiaozuoxieshizi=0,xiexs=0,xieys=0;//斜十字標(biāo)志
    
23. int xieshizi_youflag=0,youxiaoyouxieshizi=0,xiees=0,xiefs=0;
    
24. int left=0,right=0,leftzuo=0,rightzuo=0,leftyou=0,rightyou=0,rightsz=0,leftsz=0;
    
25. s16 centerxunizuo[50],centerxuniyou[50];//虛擬中心 判別用
    
26. s16 centerszyupan[50],centerzuoyupan[50],centeryouyupan[50];
    
27. int youxiaoyoubianyan=0,lastyoubianyan=0;
    
28. int youxiaozuo=0,youxiaoyoubian=0;
    
29. int youxiaoyou=0,youxiaozuobian=0;
    
30. int as,bs,cs,ds;
    
31. int is,js,jz,shizi_flag=0,youxiao_shizi=0,jieshu_shizi=0,qishi_shizi=0;
    
32. int xs=0,ys=0,es=0,fs=0;
    
33. int szis=0;
    
34. int jdt=0,id,jd;
    
35. s16 direction,TurnPosition,TurnMidPosition=78;//靠左減小值
    
36. float lasterror ;
    
37. float sum=0;
    
38. int zhengshizi=0;
    
39. int szq=0,szs=0;
    
40. int m1=0,m2=0,m3=0,m4=0,m5=0,m6=0;
    
41. int n1=0,n2=0,n3=0,n4=0,n5=0,n6=0;
    
42. extern float TurnP,TurnD;
    
43. extern s16 g_nDirectionControlFlag ;
    
44. s16 yuzhizhi;//閾
    
45. extern s16 TurnPWMOUT ,LastTurnPWMOUT;//轉(zhuǎn)向PWM
    
46. u8 ImageBuf[DATALINE][lie];
    
47. u8 shizibx[DATACOUNT];
    
48. u8 yuzhipd[50];
    
49. u8 ImageBuf2[DATALINE][DATACOUNT];
    
50. s16 blackright[DATALINE];
    
51. s16 blackleft[DATALINE];
    
52. s16 center[DATALINE];
    
53. s16 valuerownum=0,value_flag=0,value_yici=0;
    
54. s8 badframe=0;
    
55. float CenterEx[3]={0,0,0};
    
56. int CenterEq=0;
    
57. //加權(quán)平均參數(shù)，一般是遠(yuǎn)大近小，間隔均勻，根據(jù)小車情況調(diào)
    
58. float CorrectEx[50]={2.6,2.6,2.6,2.6,2.6,2.56,2.52,2.48,2.44,2.4,      
    
59.                      2.36,2.32,2.3,2.26,2.22,2.18,2.14,2.1,2.06,2.02,
    
60.                      1.98,1.94,1.9,1.86,1.82,1.78,1.74,1.7,1.66,1.62,
    
61.                      1.58,1.54,1.5,1.47,1.42,1.38,1.34,1.3,1.26,1.22,
    
62.                      1.18,1.14,1.10,1.07,1.03,1.0,1.0,1.0,1.0,1.0
    
63.                     };
    
64. u8 bxiankuan[50]={21,22,22,23,24,24,25,26,26,27,
    
65.                    28,28,29,30,30,31,32,33,33,34,
    
66.                    34,35,36,37,37,38,39,40,41,42,
    
67.                    43,45,46,47,49,50,51,52,53,54,
    
68.                    55,56,57,58,59,59,59,60,60,61};//賽道寬度的一半
    
69. void CCD_init (void)
    
70. {
    
71.   exti_init(PORTC,8, falling_down) ;          //場中斷
    
72.   exti_init(PORTD,13, falling_down) ;         //行中斷
    
73. }
    
74. void fasongtuxiang()//發(fā)送原始圖像到紅樹偉業(yè)上位機(jī)
    
75. {
    
76.   int i,j;
    
77.   for(i=0;i<DATALINE;i++)//采樣行數(shù) 50
    
78.   {        
    
79.     for(j=32;j<lie;j++)//采樣點(diǎn)數(shù) 100
    
80.     {
    
81.       if(ImageBuf[i][j]==0xff)
    
82.         ImageBuf[i][j]=0xfe;
    
83.       uart_putchar (UART0,ImageBuf[i][j]);
    
84.     }  
    
85.   }
    
86.   uart_putchar (UART0,0xff );
    
87. }  
    
88. float absx(float i)//作用： 求絕對(duì)值  浮點(diǎn)型
    
89. {
    
90.   if(i<0) return (-i);
    
91.   else  return i;
    
92. }
    
93. s32 absi(s32 i)//作用： 求絕對(duì)值  int型
    
94. {
    
95.   if(i<0) return (-i);
    
96.   else  return i;
    
97. }
    
98. u16  yuzhi(u16 i)//作用： 求一行的閾值
    
99. {
    
100.   u16 j,max=0,min=255;
     
101.   for(j=0;j<DATACOUNT;j++)//采樣點(diǎn)數(shù)  100
     
102.   {                                                                                                                                
     
103.    if(ImageBuf[i][j]>max)
     
104.    {
     
105.      max=ImageBuf[i][j];
     
106.    }
     
107.    else if(ImageBuf[i][j]<min)
     
108.    {
     
109.      min=ImageBuf[i][j];
     
110.     }
     
111.   }  
     
112.   return (u16)((min+max)/2);//此處需要-20;自己試試
     
113. }
     
114. 
     
115. void erzhihua()//作用： 將采集的數(shù)據(jù)二值化
     
116. {
     
117.   u16 i,j;
     
118.   for(i=0;i<DATALINE;i++)//由于實(shí)驗(yàn)室光線不均勻，近端與遠(yuǎn)端使用不同的閾值
     
119.   {  
     
120.     if(i>30)//近端
     
121.       yuzhizhi=yuzhi(i)+12;//加大后黑色變多  根據(jù)場地不同調(diào)參數(shù)
     
122.     else    //遠(yuǎn)端
     
123.       yuzhizhi=yuzhi(i)+19;//根據(jù)場地不同調(diào)參數(shù)
     
124.       for(j=32;j<lie;j++)
     
125.      {     
     
126.        if(ImageBuf[i][j]<yuzhizhi)   //二值化為0和1 白為0，黑為1
     
127.          ImageBuf2[i][j-32]=0x01;        
     
128.        else
     
129.          ImageBuf2[i][j-32]=0x00;   
     
130.     }  
     
131.   }
     
132. }
     
133. s16 yp_left=0,yp_right=0;
     
134. s16 yupan_fangxiang(s16 yp_qishi)//只是預(yù)判，與最終中線無關(guān)！�。�
     
135. {
     
136.   yp_left=0,yp_right=0,szis=0;
     
137.   for(is=yp_qishi;is<=48;is++)
     
138.   {
     
139.      if(blackright[is]>(DATACOUNT-2)&&blackleft[is]==0)
     
140.        centerzuoyupan[is]=DATACOUNT/2;//中心預(yù)判
     
141.      else if(blackright[is]>(DATACOUNT-2)&&blackleft[is]!=0&&(is>0))
     
142.        centerzuoyupan[is]=blackleft[is]+bxiankuan[is];//一端沒線，粗略地根據(jù)賽道進(jìn)行平移
     
143.      else if(blackright[is]<(DATACOUNT-2)&&blackleft[is]==0&&(is>0))
     
144.        centerzuoyupan[is]=blackright[is]-bxiankuan[is];
     
145.      else
     
146.        centerzuoyupan[is]=(blackright[is]+blackleft[is])/2;//兩邊都有，取中值
     
147.      szis++;
     
148.    if(szis>=1)
     
149.    {
     
150.       if((centerzuoyupan[is]-centerzuoyupan[is-1])<=-1)
     
151.         yp_right++;//右傾
     
152.       else if((centerzuoyupan[is]-centerzuoyupan[is-1])>=1)
     
153.         yp_left++;//左傾
     
154.    }
     
155. }
     
156. if((yp_right>=(yp_right+yp_left+1)/2))
     
157.    return 1 ;//右傾較多
     
158. if((yp_left>=(yp_right+yp_left+1)/2))
     
159.    return 2;//左傾較多
     
160. }
     
161. //作用： 求有效行，并且求左右邊緣,最終求的中心
     
162. void countcenter()
     
163. {
     
164.   s16 i,j,m;
     
165.   s16 rightstart,leftstart,rightend,leftend;
     
166.   m1=0,m2=0,n1=0,n2=0;
     
167.   for(i=0;i<DATALINE;i++)//對(duì)圖像處理參數(shù)初始化
     
168.   {
     
169.     blackright[i]=DATACOUNT;
     
170.     blackleft[i]=0;
     
171.     center[i]=0;
     
172.     centerxunizuo[i]=centerxuniyou[i]=DATACOUNT/2;
     
173.     centerszyupan[i]=centerzuoyupan[i]=centeryouyupan[i]=0;
     
174.     right=left=0;
     
175.     rightzuo=leftzuo=0;
     
176.     rightyou=leftyou=0;
     
177.     rightsz=leftsz=0;
     
178.     zuoqishi=youqishi=0;
     
179.   }
     
180.   //計(jì)算前2行的左右邊緣
     
181.   for(i=DATALINE-1;i>DATALINE-3;i--)
     
182.   {
     
183.     for(j=(DATACOUNT/2-10);j<(DATACOUNT-1);j++)//計(jì)算前2行右邊界  起始點(diǎn)靠左一點(diǎn)
     
184.     {
     
185.        if(ImageBuf2[i][j-1]==0&&ImageBuf2[i][j]==1&&ImageBuf2[i][j+1]==1&&blackright[i]==DATACOUNT)//省掉break，
     
186.        {
     
187.          blackright[i]=j;
     
188.        }
     
189.     }
     
190.     for(m=(DATACOUNT/2+10);m>0;m--)        //計(jì)算前2行左邊界  起始點(diǎn)靠右一點(diǎn)
     
191.     {
     
192.       if(ImageBuf2[i][m-1]==1&&ImageBuf2[i][m]==1&&ImageBuf2[i][m+1]==0&&blackleft[i]==0)//省掉break，
     
193.       {
     
194.         blackleft[i]=m;
     
195.       }
     
196.     }   
     
197.     if(blackleft[i]!=0&&blackright[i]!=DATACOUNT)
     
198.      centerxunizuo[i]=(blackleft[i]+blackright[i])/2;//算出虛擬中線，為后面決策作參考
     
199.     else if(blackleft[i]==0&&blackright[i]!=DATACOUNT)
     
200.       centerxunizuo[i]=blackright[i]-bxiankuan[i];
     
201.     else if(blackleft[i]!=0&&blackright[i]==DATACOUNT)
     
202.       centerxunizuo[i]=blackleft[i]+bxiankuan[i];
     
203.   }
     
204. //前2行循環(huán)
     
205. //over
     
206.   rightstart=blackright[DATALINE-2]-3;//根據(jù)前兩行來確定搜索的起始點(diǎn)
     
207.   rightend=blackright[DATALINE-2]+3;
     
208.   leftstart=blackleft[DATALINE-2]+3;
     
209.   leftend=blackleft[DATALINE-2]-3;
     
210.   if(leftend<=0)
     
211.     leftend=0;
     
212.   if(rightend>=DATACOUNT)         
     
213.     rightstart=DATACOUNT;
     
214. //計(jì)算右邊界的有效行數(shù)和右邊界
     
215.   for(i=DATALINE-3;i>=0;i--)//
     
216.   {
     
217.     for(j=rightstart;j<rightend;j++)//右線用0xx011，比較穩(wěn)定可靠
     
218.     {
     
219.        if(rightend>=DATACOUNT)
     
220.          rightend=DATACOUNT;
     
221.        if(ImageBuf2[i][j-4]==0&&ImageBuf2[i][j-1]==0&&ImageBuf2[i][j]==1&&ImageBuf2[i][j+1]==1)  //在范圍內(nèi)找到黑線，這個(gè)地方需要注意
     
222.        {
     
223.          blackright[i]=j;
     
224.          rightstart=j-4;//找到黑線，為下一行的起始賦值
     
225.          rightend=j+4;
     
226.          if(rightstart<=1)         
     
227.            rightstart=1;
     
228.          if(rightend>=DATACOUNT)
     
229.          rightend=DATACOUNT;
     
230.          m1++;//無用，可刪掉
     
231.          break;
     
232.        }
     
233.    }
     
234.    if(j==rightend)         //在范圍內(nèi)沒有找到黑線
     
235.    {
     
236.      rightstart=rightstart-bxiankuan[i];//平移半個(gè)賽道寬重新找黑線  可優(yōu)化
     
237.      rightend=DATACOUNT;//
     
238.      if(rightstart<=1)         
     
239.        rightstart=1;
     
240.      for(j=rightstart;j<rightend;j++)//同樣是0xx011
     
241.      {
     
242.         if(ImageBuf2[i][j-4]==0&&ImageBuf2[i][j-1]==0&&ImageBuf2[i][j]==1&&ImageBuf2[i][j+1]==1) //在范圍內(nèi)找到黑線&&ImageBuf2[i-1][j+1]==1
     
243.         {
     
244.          blackright[i]=j;
     
245.          rightstart=j-4;
     
246.          rightend=j+4;
     
247.          if(rightstart<=1)         
     
248.            rightstart=1;
     
249.          if(rightend>=DATACOUNT)
     
250.          rightend=DATACOUNT;
     
251.          m2++;                                                                                       
     
252.          break;
     
253.         }
     
254.         
     
255.      }
     
256.      if(j==rightend)         //在范圍內(nèi)沒有找到黑線
     
257.         rightstart=rightstart+bxiankuan[i];//還未找到？起始點(diǎn)還原到上次確定的起始點(diǎn)
     
258.    }  
     
259.     for(m=leftstart;m>leftend;m--)//左線用110xx0   以下有上面對(duì)稱，不再注釋
     
260.      {
     
261.         if(ImageBuf2[i][m+4]==0&&ImageBuf2[i][m+1]==0&&ImageBuf2[i][m]==1&&ImageBuf2[i][m-1]==1)
     
262.         {
     
263.           blackleft[i]=m;
     
264.           leftstart=m+4;
     
265.           leftend=m-4;
     
266.           if(leftend<=0)
     
267.             leftend=0;
     
268.           if(leftstart>=DATACOUNT)
     
269.             leftstart=DATACOUNT;
     
270.           n1++;
     
271.           break;
     
272.        }
     
273.      }
     
274.      if(m==leftend)
     
275.      {
     
276.         leftstart=leftstart+bxiankuan[i];//centerxunizuo[i+1];
     
277.         leftend=0;//leftend-6;  
     
278.         if(leftend<=0)
     
279.           leftend=0;
     
280.         if(leftstart>=DATACOUNT)
     
281.            leftstart=DATACOUNT;
     
282.         for(m=leftstart;m>leftend;m--)
     
283.         {
     
284.             if(ImageBuf2[i][m+4]==0&&ImageBuf2[i][m]==1&&ImageBuf2[i][m+1]==0&&ImageBuf2[i][m-1]==1)
     
285.             {
     
286.               blackleft[i]=m;
     
287.               leftstart=m+3;
     
288.               leftend=m-3;
     
289.               if(leftend<=0)
     
290.                 leftend=0;
     
291.               if(leftstart>=DATACOUNT)
     
292.                 leftstart=DATACOUNT;
     
293.               n2++;
     
294.               break;
     
295.             }
     
296.             
     
297.        }
     
298.        if(m==leftend)
     
299.          leftstart=leftstart-bxiankuan[i];
     
300.    }//右邊界循環(huán)結(jié)束
     
301.     if(blackleft[i]!=0&&blackright[i]!=DATACOUNT)
     
302.      centerxunizuo[i]=(blackleft[i]+blackright[i])/2;//計(jì)算虛擬中線
     
303.     else if(blackleft[i]==0&&blackright[i]!=DATACOUNT)
     
304.       centerxunizuo[i]=blackright[i]-bxiankuan[i];
     
305.     else if(blackleft[i]!=0&&blackright[i]==DATACOUNT)
     
306.       centerxunizuo[i]=blackleft[i]+bxiankuan[i];
     
307.   }
     
308. }
     
309. //十字處理函數(shù)，比較麻煩，情況分為正十字(分為遠(yuǎn)處四點(diǎn)都有，近處只有兩點(diǎn)兩種情況)，左斜十字和右斜十字，
     
310. //在實(shí)際操作時(shí)可能會(huì)發(fā)現(xiàn)，各自分類的界限并沒有很明確，可能斜十字的情況在正十字里就補(bǔ)好線了，這是為了
     
311. //有交集，這樣不會(huì)丟失各種情況，不會(huì)有“都不管”的情況
     
312. void shizi_handle()
     
313. {
     
314.     int i,kk,jj,tt,ss;
     
315.     zshizi=0,zxshizi=0,yxshizi=0;
     
316.     shizi_flag=youxiao_shizi=0, jieshu_shizi=0,qishi_shizi=0;
     
317.     xs=ys=fs=es=0,left_sz=0,right_sz=0;
     
318.     zstop=0,ystop=0,zxqishi=0,zzxqishi=0,yzxqishi=0;
     
319.     start_y=0,yous=0,stop_z=0,start_z=0,zuos=0,stop_y=0,zuozs=0,youzs=0;
     
320. /////////////////////////正十字補(bǔ)線///////////////////////
     
321.     //連續(xù)3行以上都是全白，且中點(diǎn)也是白
     
322.     for(is=DATALINE-1;is>=10;is--)//提取特征：兩端都是白色的，搜不到黑線
     
323.     {
     
324.        if(youxiao_shizi==0&&(blackleft[is]==0)&&(blackright[is]==DATACOUNT)&&(blackleft[is-1]==0)&&(blackright[is-1]==DATACOUNT)&&(blackleft[is-2]==0)&&(blackright[is-2]==DATACOUNT)&&
     
325.           ImageBuf2[is][blackleft[is]]==0&&ImageBuf2[is-1][blackleft[is-1]]==0&&ImageBuf2[is][blackright[is]-1]==0&&ImageBuf2[is-1][blackright[is-1]-1]==0&&ImageBuf2[is][DATACOUNT/2]==0)
     
326.              youxiao_shizi=is;//十字開始行
     
327.        if(shizi_flag==0&&youxiao_shizi!=0&&blackleft[is]==0&&blackright[is]==DATACOUNT&&blackleft[is-1]==0&&(blackright[is-1]==DATACOUNT)&&(ImageBuf2[is][blackleft[is]]==0)&&
     
328.           (ImageBuf2[is-1][blackleft[is-1]]==0)&&(ImageBuf2[is][blackright[is]-1]==0)&&(ImageBuf2[is-1][blackright[is-1]-1]==0)&&ImageBuf2[is][DATACOUNT/2]==0&&
     
329.             (ImageBuf2[is-2][blackright[is-2]-1]==1||ImageBuf2[is-2][blackleft[is-2]]==1||blackleft[is-2]>0||blackright[is-2]<=(DATACOUNT-1)))
     
330.        {                                                                         //其實(shí)前面兩條就夠了，后面這兩條為了保險(xiǎn)才加的
     
331.           shizi_flag=is-1; //十字結(jié)束行 在起始行的基礎(chǔ)上繼續(xù)搜索得到
     
332.           break;//跳出，省時(shí)間
     
333.        }
     
334.    }
     
335.    if((youxiao_shizi-shizi_flag)>=1&&(youxiao_shizi-shizi_flag)<=25)//對(duì)白色縱向?qū)挾茸鱿拗?br />
336.    {
     
337.      for(is=DATALINE-1;is>youxiao_shizi;is--)
     
338.      { //左下端特征：邊線存在且大于0；拐點(diǎn)之下由近端到拐點(diǎn)呈遞增趨勢（且比較陡），之上呈遞減趨勢（且比較緩）.
     
339.         if(blackleft[is]>1&&blackleft[is+1]>1&&(blackleft[is]-blackleft[is+1])>=0&&
     
340.            (blackleft[is]-blackleft[is-1])>=1&&(blackleft[is]-blackleft[is-2])>=3&&zstop==0)
     
341.         {
     
342.           zzxqishi=is;//由近到遠(yuǎn) 尋找左下端拐點(diǎn)
     
343.           zstop=1;
     
344.         }
     
345.         //右下端特征：邊線存在且小于DATACOUNT；拐點(diǎn)之下由近端到拐點(diǎn)呈遞減趨勢（且比較陡），之上呈遞增趨勢（且比較緩）.
     
346.         if(blackright[is]<DATACOUNT&&blackright[is+1]<DATACOUNT&&(blackright[is]-blackright[is+1])<=0&&
     
347.            (blackright[is]-blackright[is-1])<=-1&&(blackright[is]-blackright[is-2])<=-3&&ystop==0)
     
348.         {
     
349.           yzxqishi=is;//尋找右下端拐點(diǎn)
     
350.           ystop=1;
     
351.         }
     
352.      }
     
353.      //此處針對(duì)靠下的拐點(diǎn)
     
354.      if(zzxqishi>=yzxqishi&&(zzxqishi<=47))         zxqishi=zzxqishi;//比較后使用最靠近車的行
     
355.      else if (zzxqishi<yzxqishi&&(yzxqishi<=47))   zxqishi=yzxqishi;
     
356.      else if(zzxqishi==0&&yzxqishi!=0) zxqishi=yzxqishi;//一端沒有，用另一端的作為最后的拐點(diǎn)
     
357.      else if(zzxqishi!=0&&yzxqishi==0) zxqishi=zzxqishi;
     
358.      else zxqishi=0;//兩端都沒有，即車體進(jìn)入十字中，此時(shí)沒有最下端的拐點(diǎn)
     
359.     // yupan_fangxiang(zxqishi);//主要為下面服務(wù)的  預(yù)判函數(shù)
     
360.    }
     
361.    if((youxiao_shizi-shizi_flag)>=1&&(youxiao_shizi-shizi_flag)<=25)
     
362.    {//由于存在十字的原因，之前搜到的邊線可能不準(zhǔn)，需要重新搜線，需要確定搜線的起始列，即left_sz，right_sz
     
363.       //最遠(yuǎn)行對(duì)應(yīng)的列坐標(biāo)應(yīng)該位于賽道的中心位置�。。�
     
364.      for(int jdf=0;jdf<DATACOUNT;jdf++)
     
365.        shizibx[jdf]=shizi_flag;//初始化為十字的結(jié)束行
     
366.      jdt=shizi_flag;//初始化為十字的結(jié)束行
     
367.      for(jd=0;jd<DATACOUNT;)//縱向搜線，尋找最遠(yuǎn)端
     
368.      {
     
369.        for(id=shizi_flag;id>0;id--)//從十字的結(jié)束行往上搜索直到第一行
     
370.        {
     
371.          if(ImageBuf2[id+1][jd]==0&&ImageBuf2[id][jd]==1&&ImageBuf2[id-1][jd]==1)
     
372.          {//由下到上縱向搜索，011成立便找到點(diǎn)了
     
373.            shizibx[jd]=id;//將第id行存入數(shù)組中
     
374.            break;
     
375.          }
     
376.        }
     
377.        jd+=10;//此時(shí)只是粗略地找最遠(yuǎn)端，所以不需要每一列都搜索，隔10列可以節(jié)約時(shí)間
     
378.      }
     
379.      for(int jdf=0;jdf<DATACOUNT;jdf++)
     
380.      {//最遠(yuǎn)處的行應(yīng)小于“上一次的最遠(yuǎn)行”�。。�，且不等于結(jié)束行
     
381.        if((shizibx[jdf]<shizibx[jdt])&&shizibx[jdf]!=shizi_flag)
     
382.         jdt=jdf;//獲得最遠(yuǎn)行對(duì)應(yīng)的列坐標(biāo)，如此反復(fù)進(jìn)入幾次就能把最遠(yuǎn)行對(duì)應(yīng)的列坐標(biāo)求出
     
383.      }
     
384.        left_sz=jdt+10;//有了列坐標(biāo)就能推出左右的起始列了。因?yàn)樽钸h(yuǎn)行對(duì)應(yīng)的列坐標(biāo)應(yīng)該位于賽道的中心位置�。。�
     
385.        right_sz=jdt-10;//左右都平移一段距離，留出余量
     
386.      //重新遍歷后面的行
     
387.      for(int ig=youxiao_shizi;ig>=0;ig--)
     
388.      {
     
389.        if(left_sz>blackright[ig])   left_sz=blackright[ig];//越界限制
     
390.        for(int jg=left_sz;jg>=1;jg--)//左線是110，且列大于左下端點(diǎn)的列坐標(biāo)或 其根本沒有下端點(diǎn)
     
391.        {
     
392.          if(ImageBuf2[ig][jg-1]==1&&ImageBuf2[ig][jg]==1&&ImageBuf2[ig][jg+1]==0&&(jg>=blackleft[zzxqishi]||zxqishi==0)) //在范圍內(nèi)找到黑線  
     
393.          {
     
394.             blackleft[ig]=jg;
     
395.             kk++;
     
396.             break;
     
397.          }
     
398.        }
     
399.        if(kk>5&&((blackright[ig+1]-blackleft[ig+1])<(2*bxiankuan[ig+1]+10)))//限制條件，求出的賽道寬度小于直道的寬度加一個(gè)允許波動(dòng)值
     
400.        {//由于賽道不是直線，每5行更新一次left_sz，作為下一次搜線的起始列
     
401.          kk=0;
     
402.          left_sz=blackleft[ig+1]+bxiankuan[ig+1];//根據(jù)上一行的左線虛擬出下一次的起始列
     
403.        }
     
404. 
     
405.        if(right_sz<blackleft[ig])   right_sz=blackleft[ig];//越界限制
     
406.        for(int mg=right_sz;mg<=DATACOUNT-1;mg++)//右線是011，且列小于右下端點(diǎn)的列坐標(biāo)或 其根本沒有下端點(diǎn)
     
407.        {
     
408.          if(ImageBuf2[ig][mg-1]==0&&ImageBuf2[ig][mg]==1&&ImageBuf2[ig][mg+1]==1&&(mg<=blackright[yzxqishi]||zxqishi==0)) //在范圍內(nèi)找到黑線
     
409.          {
     
410.             blackright[ig]=mg;
     
411.             jj++;
     
412.             break;
     
413.          }
     
414.          if(jj>5&&((blackright[ig+1]-blackleft[ig+1])<(2*bxiankuan[ig+1]+10)))
     
415.          {//由于賽道不是直線，每5行更新一次right_sz，作為下一次搜線的起始列
     
416.            jj=0;
     
417.            right_sz=blackright[ig+1]-bxiankuan[ig+1];//根據(jù)上一行的右線虛擬出下一次的起始列
     
418.          }
     
419.        }
     
420.      }
     
421.      for(is=DATALINE-3;is>1;is--)//求出拐點(diǎn)！
     
422.      {
     
423.         as=(blackleft[is]-blackleft[is-2]);//與上兩行做一次差        
     
424.         bs=(blackleft[is]-blackleft[is+2]);//與下兩行做一次差
     
425.         js=blackleft[is]; //為該行的列坐標(biāo)
     
426.         if((bs>=5&&as<=0)&&xs==0&&(absi(as)<=6)&&(is<=shizi_flag)&&(is>=(shizi_flag-18))&&(blackleft[is]>1)&&((ImageBuf2[is+1][js+1]+ImageBuf2[is+1][js]+ImageBuf2[is+1][js-1])<=1))
     
427.           xs=is;//上面  左上拐點(diǎn)的特征：之下為遞增（或直接增），之上為遞增，之下兩行的絕對(duì)值比較小，左上拐點(diǎn)在十字起始行上面，且在一個(gè)范圍內(nèi)，周圍幾個(gè)點(diǎn)的白色多一些
     
428.         if((as>=4&&bs>=0)&&ys==0&&(absi(bs)<=6)&&(is>=xs)&&(is>=youxiao_shizi)&&blackleft[is]>1&&((ImageBuf2[is-1][js+1]+ImageBuf2[is-1][js]+ImageBuf2[is-1][js-1])<=1))
     
429.           ys=is;//下面 左下拐點(diǎn)的特征：之下為遞增，之上為遞減（或直接減），在十字起始行下面
     
430.         cs=(blackright[is]-blackright[is-2]);
     
431.         ds=(blackright[is]-blackright[is+2]);  
     
432.         jz=blackright[is];
     
433.         if((ds<=-5&&cs>=0)&&es==0&&(absi(cs)<=6)&&(is<=shizi_flag)&&(is>=(shizi_flag-18))&&(blackright[is]<=(DATACOUNT-2))&&((ImageBuf2[is+1][jz+1]+ImageBuf2[is+1][jz]+ImageBuf2[is+1][jz-1])<=1))
     
434.           es=is;//上面  右上拐點(diǎn)的特征：之下為遞減（或直接減），之上為遞減，。。。
     
435.         if((ds<=0&&cs<=-3)&&fs==0&&(absi(ds)<=6)&&(is>=es)&&(is>=youxiao_shizi)&&(blackright[is]<=(DATACOUNT-2))&&((ImageBuf2[is-1][jz+1]+ImageBuf2[is-1][jz]+ImageBuf2[is-1][jz-1])<=1))
     
436.           fs=is;//下面  右下拐點(diǎn)的特征：之下為遞減，之上為遞增（或直接增）
     
437.      }
     
438. //有下端拐點(diǎn)的進(jìn)來�。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。�     
     
439.      //下端拐點(diǎn)在十字開始行的下面，且（左線在中線以左   或              下端拐點(diǎn)處的右線在中線以右），
     
440.      if((zxqishi-12)<youxiao_shizi&&((blackleft[zxqishi]<(DATACOUNT/2-10))||blackright[zxqishi]>(DATACOUNT/2+10))&&
     
441.         zxqishi!=0&&(zzxqishi!=0||yzxqishi!=0)&&zxqishi<=(DATACOUNT-3))                             //有拐點(diǎn)的進(jìn)來
     
442.      {//且下端拐點(diǎn)的確存在，且左、右拐點(diǎn)至少存在一個(gè)；限幅
     
443.         if(xs!=0&&ys!=0&&ys<=(zxqishi+10)&&ys>=(zxqishi-10)&&blackleft[xs]>=blackleft[ys])//左側(cè)進(jìn)行連線
     
444.         {//左側(cè)上下拐點(diǎn)都存在，且下拐點(diǎn)在下端點(diǎn)拐點(diǎn)的合理范圍之內(nèi)，左上拐點(diǎn)比左下拐點(diǎn)靠右
     
445.           for(int iz=xs-1;iz<=ys+1;iz++)//一次函數(shù)連線
     
446.           {//為了更加平滑，各讓一步
     
447.              blackleft[iz+1]=(blackleft[iz]-blackleft[ys+1])/(iz-ys-1)+blackleft[iz];//注意順序，上面拐點(diǎn)在左還是在右
     
448.              zshizi=1;//正入十字標(biāo)志置1，為下面使用
     
449.           }
     
450.         }//同樣，不作注釋
     
451.         if(es!=0&&fs!=0&&fs<=(zxqishi+10)&&fs>=(zxqishi-10)&&blackright[fs]>=blackright[es])//右側(cè)進(jìn)行連線
     
452.         {
     
453.           for(int iz=es-1;iz<=fs+1;iz++)
     
454.           {
     
455.              blackright[iz+1]=((blackright[fs+1]-blackright[iz])/(fs+1-iz))+blackright[iz];
     
456.              zshizi=1;//正入十字標(biāo)志置1，為下面使用
     
457.           }
     
458.         }
     
459.         //為了下面求中線的起始行和結(jié)束行而使用
     
460.         if(xs>=es)  szq=es;//僅為標(biāo)準(zhǔn)正十字服務(wù)
     
461.         else        szq=xs;//十字起始行以上為主
     
462.         if(ys>=fs)  szs=ys;
     
463.         else        szs=fs;//十字接收行以下為主
     
464.         if(xs!=0&&ys!=0&&es!=0&&fs!=0)//標(biāo)準(zhǔn)正十字 四拐點(diǎn)都存在
     
465.         {
     
466.           for(i=szq-1;i<=szs+1;i++)//為了平滑，各讓一步
     
467.             if(center[i]==0)//由于前面已經(jīng)補(bǔ)線了，此處只需求中值即可
     
468.                center[i]=(blackright[i]+blackleft[i])/2;
     
469.         }
     
470.         else if(xs!=0&&ys!=0&&(es==0||fs==0))//右側(cè)丟失
     
471.         {
     
472.           for(i=xs-1;i<=ys+1;i++)
     
473.             if(center[i]==0)//中線靠左線加賽道寬度而得
     
474.               center[i]=(bxiankuan[i]+blackleft[i]);
     
475.         }
     
476.         else if(es!=0&&fs!=0&&(xs==0||ys==0))//左側(cè)丟失
     
477.         {  
     
478.           for(i=es-1;i<=fs+1;i++)
     
479.             if(center[i]==0)//中線靠右線減賽道寬度而得
     
480.              center[i]=(blackright[i]-bxiankuan[i]);
     
481.         }
     
482.      }
     
483. //近端處理  無下拐點(diǎn)的進(jìn)來�。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。�！
     
484.     else if(youxiao_shizi>=30&&shizi_flag>=24&&(es!=0&&xs!=0)&&(es>=(xs-13)&&xs>=(es-13)))//如果剛進(jìn)入十字，則只有最上面兩個(gè)拐點(diǎn)，
     
485.     {     //十字比較靠近車                     且兩個(gè)上拐點(diǎn)都存在，es與xs在合理的范圍內(nèi)                              
     
486.       for(int iz=xs-1;iz<DATALINE-1;iz++)//此時(shí)補(bǔ)線利用左拐點(diǎn)與左下角坐標(biāo)值連線，右拐點(diǎn)與右下角坐標(biāo)值連線
     
487.       {//一次函數(shù)連線
     
488.         blackleft[iz+1]=-(blackleft[iz])/(DATALINE-1-iz)+blackleft[iz];//注意順序，上面拐點(diǎn)在左還是在右
     
489.         zshizi=1;//正入十字標(biāo)志置1，為下面使用
     
490.       }
     
491.       for(int iz=es-1;iz<DATALINE-1;iz++)
     
492.       {
     
493.         blackright[iz+1]=((DATACOUNT-blackright[iz])/(49-iz))+blackright[iz];
     
494.         zshizi=1;//正入十字標(biāo)志置1，為下面使用
     
495.       }
     
496.       if(xs>=es)    szq=es;//選取靠上的拐點(diǎn)，作為下面求中線的起始行
     
497.       else          szq=xs;
     
498.       for(i=szq;i<=49;i++)
     
499.         center[i]=(blackright[i]+blackleft[i])/2;
     
500.     }
     
501.     //十字后期處理  主要處理一些不常見的情況，可不看
     
502.     if(zshizi==1)
     
503.     {
     
504.       for(i=DATALINE-1;i>1;i--)
     
505.       {
     
506.         if(blackleft[i]<=1&&blackright[i]<(DATACOUNT-1)&¢er[i]==0)//左線丟失
     
507.         {
     
508.           if(center[i-1]!=0)//給下端補(bǔ)線
     
509.             center[i]=blackright[i]-(blackright[i-1]-center[i-1]+1);
     
510.           else if(center[i+1]!=0)//給上端補(bǔ)線
     
511.             center[i]=blackright[i]-(blackright[i+1]-center[i+1]-1);
     
512.           else
     
513.             center[i]=blackright[i]-bxiankuan[i];
     
514.         }
     
515.         if(blackleft[i]>1&&blackright[i]==DATACOUNT&¢er[i]==0)//右線丟失
     
516.         {
     
517.           if(center[i-1]!=0)//給下端補(bǔ)線
     
518.             center[i]=blackleft[i]+(center[i-1]-blackleft[i-1]+1);
     
519.           else if(center[i+1]!=0)//給上端補(bǔ)線
     
520.             center[i]=blackleft[i]+(center[i+1]-blackleft[i+1]-1);
     
521.           else
     
522.             center[i]=blackleft[i]+bxiankuan[i];
     
523.         }
     
524.       }
     
525.     }
     
526.    }
     
527. //左側(cè)斜十字補(bǔ)線     各標(biāo)志位清零
     
528.    xieshizi_zuoflag=0,xieshizi_youflag=0,youxiaozuoxieshizi=0,youxiaoyouxieshizi=0,xiexs=0,xieys=0,xiees=0,xiefs=0;
     
529.    for(is=DATALINE-3;is>5;is--)
     
530.    {//連續(xù)兩行的左線都存在  下增上減
     
531.      if(blackleft[is]>1&&blackleft[is+1]>1&&(blackleft[is]-blackleft[is+1])>=0&&
     
532.         (blackleft[is]-blackleft[is+2])>=1&&(blackleft[is]-blackleft[is-1])>=1&&(blackleft[is]-blackleft[is-2])>2)
     
533.       {
     
534.         zuoqishi=is;//先尋找左下拐點(diǎn)
     
535.         break;
     
536.       }
     
537.    }
     
538.    if(zuoqishi!=0)//左下拐點(diǎn)存在才能進(jìn)入
     
539.    {
     
540.      for(is=zuoqishi;is>2;is--)//從左下拐點(diǎn)向上尋找
     
541.      {                       //連續(xù)三行左端都沒找到線
     
542.        if(youxiaozuoxieshizi==0&&blackleft[is]==0&&blackleft[is-1]==0&&blackleft[is-2]==0&&
     
543.           ImageBuf2[is][blackleft[is]]==0&&ImageBuf2[is-1][blackleft[is-1]]==0)
     
544.        {//并且連續(xù)兩行左端 二值化圖像 都是白的
     
545.           if(youxiaozuoxieshizi==0)//進(jìn)行遍歷，尋找起始行
     
546.              youxiaozuoxieshizi=is;
     
547.        }                        //連續(xù)兩行沒找到線，且從下到上是白到黑或左線找到了
     
548.        if(youxiaozuoxieshizi!=0&&blackleft[is]==0&&blackleft[is-1]==0&&ImageBuf2[is-1][blackleft[is-1]]==0&&
     
549.           (ImageBuf2[is-2][blackleft[is-2]]==1||blackleft[is-2]!=0))//一般上側(cè)結(jié)束行都是極值，只有分辨圖像是黑是白
     
550.        {
     
551.          xieshizi_zuoflag=is-1;//尋找結(jié)束行
     
552.          break;//找到就跳出，省時(shí)間
     
553.        }
     
554.      }
     
555.    }
     
556. //斜十字補(bǔ)線 左側(cè)  白色區(qū)域在合理范圍內(nèi)，                                               下拐點(diǎn)在白色下面
     
557.     if((youxiaozuoxieshizi-xieshizi_zuoflag)>2&&(youxiaozuoxieshizi-xieshizi_zuoflag)<30&&(zuoqishi-15)<youxiaozuoxieshizi&&
     
558.         (blackleft[zuoqishi]<(DATACOUNT/2+30))&&zuoqishi!=0&&yupan_fangxiang(zuoqishi)==1)
     
559.      {           //                                            右傾較多
     
560.         left_xsz=centerxunizuo[zuoqishi]+25;//根據(jù)預(yù)判得出虛擬中線，
     
561.         for(int ig=zuoqishi;ig>0;ig--)//重新搜線
     
562.         {  
     
563.           if(left_xsz>=blackright[ig])  left_xsz=blackright[ig];
     
564.            for(int jg=left_xsz;jg>=0;jg--)
     
565.            {
     
566.              if(ImageBuf2[ig][jg+1]==0&&ImageBuf2[ig][jg]==1&&ImageBuf2[ig][jg-1]==1&&jg>=blackleft[zuoqishi]) //在范圍內(nèi)找到黑線
     
567.              {
     
568.                 blackleft[ig]=jg;  //更新左線數(shù)組
     
569.                 ss++;
     
570.                 break;
     
571.              }
     
572.            }
     
573.            if(ss>4)
     
574.            {
     
575.              ss=0;
     
576.              left_xsz=blackleft[ig+1]+bxiankuan[ig+1];
     
577.            }
     
578.        }
     
579.        xieshizi_zuoflag=0;
     
580.        for(is=zuoqishi;is>5;is--)//斜十字補(bǔ)線 再判斷 由于前面更新了左線數(shù)組，此時(shí)再次確認(rèn)無誤
     
581.        {
     
582.          if(youxiaozuoxieshizi==0&&blackleft[is]==0&&blackleft[is-1]==0&&blackleft[is-2]==0)
     
583.          {
     
584.            if(youxiaozuoxieshizi==0)
     
585.              youxiaozuoxieshizi=is;//得出起始行
     
586.          }
     
587.          if(youxiaozuoxieshizi!=0&&blackleft[is]==0&&blackleft[is-1]==0&&
     
588.             ImageBuf2[is][blackleft[is]]==0&&ImageBuf2[is-1][blackleft[is-1]]==0&&(ImageBuf2[is-2][blackleft[is-2]]==1||blackleft[is-2]!=0))
     
589.          {
     
590.            xieshizi_zuoflag=is-1;//得出結(jié)束行
     
591.            break;
     
592.          }
     
593.        }
     
594.        for(is=DATALINE-3;is>=0;is--)//十字補(bǔ)線，同正十字
     
595.        {
     
596.          as=absi(blackleft[is]-blackleft[is-2]);         
     
597.          bs=absi(blackleft[is+2]-blackleft[is]);
     
598.          js=blackleft[is]; //下面
     
599.          if((as>5&&bs<=8)&&xieys==0&&(is>=(youxiaozuoxieshizi))&&((ImageBuf2[is-1][js+1]+ImageBuf2[is-1][js]+ImageBuf2[is-1][js-1])<=1))
     
600.            xieys=is;
     
601.          if((bs>5&&as<=6)&&xieys!=0&&(is<=xieshizi_zuoflag)&&(is>=(xieshizi_zuoflag-12))&&blackleft[is]>=blackleft[xieys]&&(blackleft[is]>1)&&((ImageBuf2[is+1][js+1]+ImageBuf2[is+1][js]+ImageBuf2[is+1][js-1])<=1))
     
602.          {
     
603.            xiexs=is;//上面
     
604.            break;
     
605.          }
     
606.        }
     
607.        if(xieys!=0&&xiexs!=0&&xieys>xiexs)
     
608.        {
     
609.          for(int iz=xiexs;iz<=xieys;iz++)//只能補(bǔ)一側(cè)
     
610.          {
     
611.            blackleft[iz+1]=((blackleft[xieys]-blackleft[iz])/(xieys-iz))+blackleft[iz];
     
612.            center[iz+1]=(bxiankuan[iz+1]+blackleft[iz+1]);
     
613.            zxshizi=1;//左斜十字標(biāo)志置1
     
614.          }
     
615.          for(int iz=DATALINE-1;iz>=3;iz--)//把右側(cè)沒搜到線的行的中線根據(jù)左側(cè)補(bǔ)出來
     
616.          {
     
617.            if(blackleft[iz]>=1&&blackright[iz]>=(DATACOUNT-1))
     
618.              center[iz]=blackleft[iz]+bxiankuan[iz];//
     
619.          }
     
620.        }
     
621.      }
     
622. //右側(cè) 斜十字補(bǔ)線/////
     
623.      for(int is=DATALINE-3;is>=5;is--)//斜十字補(bǔ)線   右側(cè) 預(yù)判斷
     
624.      {
     
625.        if(blackright[is]<DATACOUNT&&blackright[is-1]<=DATACOUNT&&blackright[is-2]<=DATACOUNT&&(blackright[is]-blackright[is+1])<=0&&
     
626.           (blackright[is]-blackright[is+2])<=0&&(blackright[is]-blackright[is-1])<=-1&&(blackright[is]-blackright[is-2])<-3)
     
627.        {
     
628.           youqishi=is;//右側(cè)的下端拐點(diǎn)
     
629.           break;
     
630.        }
     
631.      }
     
632.      for(is=youqishi;is>=5;is--)//得出白色區(qū)域
     
633.      {
     
634.         if(blackright[is]==DATACOUNT&&blackright[is-1]==DATACOUNT&&blackright[is-2]==DATACOUNT&&
     
635.            ImageBuf2[is][blackright[is]-1]==0&&ImageBuf2[is-1][blackright[is-1]-1]==0)
     
636.         {
     
637.           if(youxiaoyouxieshizi==0)//開始記錄白點(diǎn)
     
638.             youxiaoyouxieshizi=is;
     
639.         }
     
640.         if(youxiaoyouxieshizi!=0&&xieshizi_youflag==0&&blackright[is]==DATACOUNT&&blackright[is-1]==DATACOUNT&&
     
641.            ImageBuf2[is][blackright[is]-1]==0&&ImageBuf2[is-1][blackright[is-1]-1]==0&&(ImageBuf2[is-2][blackright[is-2]-1]==1||blackright[is-2]!=DATACOUNT))
     
642.         {
     
643.           xieshizi_youflag=is-1;//遇著黑點(diǎn)退出
     
644.           break;
     
645.         }
     
646.      }                                                                                     //右下拐點(diǎn)靠右
     
647.      if((youxiaoyouxieshizi-xieshizi_youflag)>1&&(youxiaoyouxieshizi-xieshizi_youflag)<30&&(blackright[youqishi]>(DATACOUNT/2-30))&&
     
648.         youqishi!=0&&yupan_fangxiang(youqishi)==2)//左傾較多
     
649.      {
     
650.         right_xsz=(centerxunizuo[youqishi]-30);//根據(jù)得出的虛擬中線，來確定再次搜線的起始列
     
651.         for(int ig=youqishi;ig>=0;ig--)//對(duì)右線數(shù)組重新搜索
     
652.         {
     
653.           if(right_xsz<=blackleft[ig])   right_xsz=blackleft[ig];
     
654.           if(right_xsz<1)   right_xsz=1;
     
655.          
     
656.           for(int jg=right_xsz;jg<=DATACOUNT;jg++)
     
657.           {
     
658.             if(ImageBuf2[ig][jg-1]==0&&ImageBuf2[ig][jg]==1&&ImageBuf2[ig][jg+1]==1&&jg<=blackright[youqishi]) //在范圍內(nèi)找到黑線
     
659.             {
     
660.                blackright[ig]=jg;
     
661.                tt++;
     
662.                break;
     
663.             }
     
664.           }
     
665.          if(tt>2)
     
666.           {
     
667.             tt=0;
     
668.             right_xsz=blackright[ig+1]-bxiankuan[ig+1];
     
669.           }
     
670.         }
     
671.         xieshizi_youflag=0;
     
672.         for(is=youqishi;is>=5;is--)//斜十字補(bǔ)線   由于更新了數(shù)組，需要再判斷
     
673.         {
     
674.           if(blackright[is]==DATACOUNT&&blackright[is-1]==DATACOUNT&&blackright[is-2]==DATACOUNT&&
     
675.              ImageBuf2[is][blackright[is]-1]==0&&ImageBuf2[is-2][blackright[is-2]-1]==0)
     
676.           {
     
677.             if(youxiaoyouxieshizi==0)
     
678.               youxiaoyouxieshizi=is;//得出起始行
     
679.           }
     
680.           if(youxiaoyouxieshizi!=0&&xieshizi_youflag==0&&blackright[is]==DATACOUNT&&blackright[is-1]==DATACOUNT&&
     
681.              ImageBuf2[is][blackright[is]-1]==0&&ImageBuf2[is-1][blackright[is-1]-1]==0&&(blackright[is-2]!=DATACOUNT||ImageBuf2[is-2][blackright[is-2]-1]==1))
     
682.           {
     
683.             xieshizi_youflag=is-1;//結(jié)束行
     
684.             break;
     
685.           }
     
686.         }
     
687.         for(is=DATALINE-3;is>=0;is--)//斜十字補(bǔ)線
     
688.         {
     
689.           as=absi(blackright[is]-blackright[is-2]);         
     
690.           bs=absi(blackright[is+1]-blackright[is]);
     
691.           js=blackright[is]; //下面
     
692.           if((as>5&&bs<=6)&&xiefs==0&&(is>=(youxiaoyouxieshizi))&&is<(youqishi+4)&&is>(youqishi-4)&&((ImageBuf2[is-1][js+1]+ImageBuf2[is-1][js]+ImageBuf2[is-1][js-1])<=1))
     
693.             xiefs=is;
     
694.           if((bs>5&&as<=6)&&xiees==0&&xiefs!=0&&(is<=(xieshizi_youflag))&&(is>=(xieshizi_youflag-18))&&(blackright[is]<=(blackright[xiefs]))&&((ImageBuf2[is+1][js+1]+ImageBuf2[is+1][js]+ImageBuf2[is+1][js-1])<=1))
     
695.              xiees=is;//上面(is<=ys)&&(blackleft[i+2]>=2)&&(blackleft[i-3]>=2)youxiao_xieshizi
     
696.         }
     
697.         if(xiefs!=0&&xiees!=0&&xiefs>xiees)//斜十字畸變較大，前后多找些點(diǎn)進(jìn)行補(bǔ)
     
698.         {
     
699.           for(int iz=xiees;iz<(xiefs+3);iz++)//對(duì)右側(cè)補(bǔ)完線并求中心值
     
700.           {
     
701.             blackright[iz+1]=((blackright[xiefs+3]-blackright[iz])/(xiefs+3-iz))+blackright[iz];
     
702.             center[iz+1]=(blackright[iz+1]-bxiankuan[iz+1]);
     
703.             yxshizi=1;//右斜十字置1
     
704.           }
     
705.           for(int iz=49;iz>=1;iz--)
     
706.          {
     
707.            if(blackleft[iz]==0&&blackright[iz]<DATACOUNT)
     
708.              center[iz]=blackright[iz]-bxiankuan[iz];
     
709.            else if((blackleft[iz]==0&&blackright[iz]>=DATACOUNT))
     
710.              center[iz]=DATACOUNT/2;
     
711.          }
     
712.         }
     
713.      }
     
714. }
     
715. /////////////對(duì)尋到的邊情況分類并處理擬合中線////////
     
716. void nihe_center()
     
717. {
     
718.   int i=0;
     
719.   youxiaoyou=0,yici_you=0,youxiaozuobian=0,yici_zuo=0,youxiaoyoubian=0,youxiaozuo=0;
     
720.   youbian_ysy=0,jinduan_ybx=0,zuobian_ysy=0,jinduan_zbx=0,ztuichu=0;
     
721.   for(i=DATALINE-1;i>=0;i--)
     
722.   {
     
723.     if(center[i]==0)
     
724.     {
     
725.        if(blackright[i]>=(DATACOUNT-2)&&blackleft[i]<=1&¢er[i]==0)//兩邊都有取圖像中心
     
726.          center[i]=DATACOUNT/2;
     
727.        else if(blackright[i]>(DATACOUNT-2)&&blackleft[i]!=0&¢er[i]==0&&ImageBuf2[i][blackright[i]-1]==0)//右側(cè)丟線
     
728.        {
     
729.          if(yici_you==0)//右側(cè)開始丟線處
     
730.            yici_you=i;//記錄起始行
     
731.          if(youxiaoyou==0)
     
732.          {
     
733.            for(int bs=i;bs>2;bs--)
     
734.            {//連續(xù)三行的右線為都有值   或連續(xù)兩行是二值化值是黑的
     
735.              if((blackright[bs]!=DATACOUNT&&blackright[bs-1]!=DATACOUNT&&blackright[bs-2]!=DATACOUNT)||
     
736.                 ImageBuf2[bs][blackright[bs]-1]==1&&ImageBuf2[bs-1][blackright[bs-1]-1]==1)
     
737.              {
     
738.                youxiaoyou=bs;//結(jié)束行
     
739.                break;
     
740.              }
     
741.            }
     
742.          }
     
743.          if(youxiaozuobian==0)
     
744.          {
     
745.            for(int bs=i;bs>youxiaoyou-5;bs--)//判斷左線連續(xù)性，并確定不連續(xù)行
     
746.            {
     
747.              if((blackleft[bs]-blackleft[bs-1])>0||blackleft[bs]>=(DATACOUNT-1)||blackleft[bs-1]>=(DATACOUNT-1))
     
748.              {//本來左線是遞增向右的，當(dāng)遇到左線遞減了或跑到右邊沿了就進(jìn)入，
     
749.                youxiaozuobian=bs;
     
750.                break;
     
751.              }
     
752.              else if((blackleft[bs]-blackleft[bs-1])<=0||blackleft[bs]<(DATACOUNT-1)||blackleft[bs-1]<(DATACOUNT-1))
     
753.                youxiaozuobian=bs;//遞增時(shí)一直更新此值
     
754.            }
     
755.          }
     
756.          if(i==49||i==48||(i==47))//如果靠近車，應(yīng)該是近端丟線，先判斷下
     
757.          {
     
758.            if(yici_you>=47&&jinduan_ybx==0)
     
759.            {
     
760.              for(as=(youxiaoyou-2);as>1;as--)
     
761.              {
     
762.                if((blackright[as]-blackright[as-1])>=-1&&blackright[as]<DATACOUNT)
     
763.                {//向上遞減
     
764.                  youbian_ysy=as;
     
765.                  if(youbian_ysy<39&&(youxiaoyou-youbian_ysy)>6)//驗(yàn)證是否確實(shí)是近端丟線  此值應(yīng)離丟線處不遠(yuǎn)
     
766.                  {
     
767.                    for(int as=youxiaoyou;as<=49;as++)//能進(jìn)來，可以確定確實(shí)近端丟線了
     
768.                    {
     
769.                       if(center[as]==0)
     
770.                       {
     
771.                         jinduan_ybx=1;//置位標(biāo)志  //根據(jù)前一行的中線求出該行的中線
     
772.                         center[as]=(blackleft[as-1]+blackright[as-1])/2-blackleft[as-1]+1+blackleft[as];
     
773.                       }
     
774.                    }
     
775.                  }
     
776.                  if(jinduan_ybx==1)  break;//補(bǔ)線完成跳出程序
     
777.                }
     
778.                else break;//若為進(jìn)入急彎，則會(huì)跳出
     
779.              }
     
780.            }
     
781.            if(jinduan_ybx==0&&yici_you>=46&&youxiaoyou<38&&(youxiaozuobian<=yici_you))//如果不是近端丟線，而是進(jìn)入急彎  不
     
782.            {//建議攝像頭的鏡頭在120度以上，90度的容易進(jìn)入此處
     
783.              for(int as=49;as>youxiaoyou;as--)
     
784.              {
     
785.                 if(center[as]==0&&blackleft[as]!=0)
     
786.                 {
     
787.                  center[as]=blackleft[as]+bxiankuan[as]+1;//改變系數(shù)1的大小，可改善進(jìn)入急彎的轉(zhuǎn)向能力
     
788.                 }
     
789.              }
     
790.            }
     
791.         }
     
792.          else  if(center[i]==0&&blackleft[i]!=0)//彎道丟線時(shí)
     
793.           center[i]=center[i+1]-blackleft[i+1]+blackleft[i]+4;//改變系數(shù)4的大小，可改善進(jìn)入急彎的轉(zhuǎn)向能力
     
794.        }
     
795.        else if(blackright[i]<(DATACOUNT-1)&&blackleft[i]==0&¢er[i]==0)//左側(cè)丟線
     
796.        {
     
797.          if(yici_zuo==0)//開始丟線處起始
     
798.            yici_zuo=i;
     
799.          if(youxiaozuo==0)
     
800.          {
     
801.            for(int ks=i;ks>2;ks--)//重新遍歷，確定丟線最上行
     
802.            {
     
803.              if(blackleft[ks]!=0||ImageBuf2[ks][blackleft[ks]+1]==1)
     
804.              {
     
805.                youxiaozuo=ks;
     
806.                break;
     
807.              }
     
808.            }
     
809.          }
     
810.          if(youxiaoyoubian==0)
     
811.          {
     
812.            for(int ks=i;ks>youxiaozuo-5;ks--)//判斷右線連續(xù)性，并確定不連續(xù)行
     
813.            {
     
814.              if((blackright[ks]-blackright[ks-1])<0||blackright[ks]<=0||blackright[ks-1]<=0)
     
815.              {
     
816.                youxiaoyoubian=ks;
     
817.                break;
     
818.              }
     
819.              else if((blackright[ks]-blackright[ks-1])>0||blackright[ks]>0||blackright[ks-1]>0)
     
820.                youxiaoyoubian=ks;
     
821.                
     
822.            }
     
823.          }
     
824.         //重新遍歷最近端的點(diǎn)   判斷是否進(jìn)入急彎，即左側(cè)或右側(cè)一大塊沒尋到線的地方
     
825.          if(i==49||i==48||(i==47))
     
826.          {
     
827.            if(i>=47&&yici_zuo>=47&&jinduan_zbx==0&&ztuichu==0)
     
828.            {
     
829.              for(as=(youxiaozuo-2);as>1;as--)
     
830.              {
     
831.                if((blackleft[as]-blackleft[as-1])<=1&&blackleft[as]>0&&ztuichu==0)
     
832.                {
     
833.                  zuobian_ysy=as;
     
834.                  if(zuobian_ysy<39&&(youxiaozuo-zuobian_ysy)>6)//驗(yàn)證是否確實(shí)是近端丟線
     
835.                  {
     
836.                    for(int as=youxiaozuo;as<=49;as++)
     
837.                    {
     
838.                     if(center[as]==0)
     
839.                     {
     
840.                       jinduan_zbx=1;//置位標(biāo)志
     
841.                        center[as]=blackright[as]-(blackright[as-1]-(blackright[as-1]+blackleft[as-1]+1)/2+1);
     
842.                     }
     
843.                    }
     
844.                  }
     
845.                  if(jinduan_zbx==1)  break;//補(bǔ)線完成跳出程序
     
846.                }
     
847.                else
     
848.                  break;//若為進(jìn)入急彎，則會(huì)跳出
     
849.              }
     
850.            }
     
851.            if(i>47&&jinduan_zbx==0&&yici_zuo>=48&&youxiaozuo<40&&(youxiaoyoubian<=yici_zuo))//如果不是近端丟線，而是進(jìn)入急彎
     
852.            {
     
853.              for(int as=49;as>youxiaozuo;as--)
     
854.                 if(center[as]==0)
     
855.                 {
     
856.                    center[as]=blackright[as]-bxiankuan[as]-4;
     
857.                 }
     
858.            }
     
859.          }
     
860.          else if(blackright[i]!=DATACOUNT&¢er[i]==0)//進(jìn)入彎道
     
861.          {         
     
862.            center[i]=blackright[i]-(blackright[i+1]-center[i+1]+3);//改變系數(shù)3的大小，可改善進(jìn)入急彎的轉(zhuǎn)向能力
     
863.          }
     
864.        }
     
865.        else if(center[i]==0)//其他情況，直接取中值
     
866.        {
     
867.          center[i]=(blackright[i]+blackleft[i])/2;  
     
868.        }
     
869.    }
     
870.   }
     
871. }
     
872. 
     
873. void countvaluerow()
     
874. {
     
875.    s16 i;
     
876.   yx_left=0,yx_right=0;
     
877.    value_flag=0,valuerownum=0;
     
878.    S_xy=0,S_xx=0,x=0,y=0,S_y=0,S_x=0,slope=0;
     
879.    for(i=DATALINE-3;i>=1;i--)
     
880.    {
     
881.      if(center[i]>=DATACOUNT)   center[i]=DATACOUNT;
     
882.      else if(center[i]<=0)      center[i]=0;
     
883.      if(center[i-1]>=DATACOUNT) center[i-1]=DATACOUNT;
     
884.      else if(center[i-1]<=0)    center[i-1]=0;
     
885.      if(center[i-2]>=DATACOUNT) center[i-2]=DATACOUNT;
     
886.      else if(center[i-2]<=0)    center[i-2]=0;
     
887.      if((((absx(center[i]-center[i-1])>5)&&(absx(center[i]-center[i-2])>7))||(center[i]<=0)||(center[i]>=DATACOUNT)||(ImageBuf2[i][center[i]]==1||
     
888.       ImageBuf2[i][center[i]+1]==1||ImageBuf2[i][center[i]-1]==1||ImageBuf2[i][center[i]+2]==1||ImageBuf2[i][center[i]-2]==1))&&value_flag==0&&valuerownum==0)
     
889.      {  
     
890.          valuerownum=i;//中心壞行標(biāo)號(hào)
     
891.          value_yici=i;
     
892.          value_flag=1;
     
893.      }
     
894.      if((center[i]-center[i-1])<=-1)
     
895.        yx_right++;
     
896.      else if((center[i]-center[i-1])>=1)
     
897.        yx_left++;
     
898.      if(value_flag!=0&&valuerownum!=0)
     
899.      {
     
900.        if((yx_right>(yx_right+yx_left)*3/4)||(i>43&&((center[i]-center[i-1])<0&&(center[i]-center[i-2])<0)))
     
901.        {
     
902.          if(((center[i]-center[i-1])>0&&(center[i]-center[i-2])>0)||(center[i]<=0)||(center[i]>=DATACOUNT)||(ImageBuf2[i][center[i]]==1||
     
903.               ImageBuf2[i][center[i]+1]==1||ImageBuf2[i][center[i]-1]==1||ImageBuf2[i][center[i]+2]==1||ImageBuf2[i][center[i]-2]==1))
     
904.          {
     
905.             valuerownum=i;
     
906.             break;
     
907.          }
     
908.        }
     
909.        else if((yx_left>(yx_right+yx_left)*3/4)||(i>43&&((center[i]-center[i-1])>0&&(center[i]-center[i-2])>0)))
     
910.        {
     
911.           if(((center[i]-center[i-1])<0&&(center[i]-center[i-2])<0)||(center[i]<=0)||(center[i]>=DATACOUNT)||(ImageBuf2[i][center[i]]==1||
     
912.               ImageBuf2[i][center[i]+1]==1||ImageBuf2[i][center[i]-1]==1||ImageBuf2[i][center[i]+2]==1||ImageBuf2[i][center[i]-2]==1))
     
913.           {
     
914.              valuerownum=i;
     
915.              break;
     
916.           }
     
917.        }
     
918.        else break;
     
919.      }
     
920.    }
     
921. }
     
922. //作用：計(jì)算有效行          根據(jù)上面黑線的中心偏差來求解有效行
     
923. //改變?nèi)�局變量�?valuerownum
     
924. int wanqishi=0;
     
925. void panduan_youxiao()
     
926. {
     
927.   zuobian_youxiao=0,zuobian_stop=0,youwan_youxiao=0,youwan_flag=0,youwan_center=0,youwan_centerstop=0;
     
928.   youbian_youxiao=0,youbian_stop=0,zuowan_youxiao=0,zuowan_flag=0,zuowan_center=0,zuowan_centerstop=0;
     
929.   wandao_youxiao=0,zuowan=0,youwan=0,youbian_qi=0,zuobian_qi=0;
     
930.   for(int i=wanqishi;i>valuerownum;i--)
     
931.   {
     
932.     if(center[i]<=0||center[i]>=DATACOUNT)
     
933.       break;
     
934.      if((center[i]-center[i-1])<=-1&¢er[i]!=0&¢er[i]!=DATACOUNT&¢er[i-1]!=0&¢er[i-1]!=DATACOUNT)
     
935.        right++;
     
936.      else if((center[i]-center[i-1])>=1&¢er[i]!=0&¢er[i]!=DATACOUNT&¢er[i-1]!=0&¢er[i-1]!=DATACOUNT)
     
937.        left++;
     
938.   }
     
939.   if((right>(left+right+1)/2)&&(zshizi==0&&zxshizi==0&&yxshizi==0))//右彎
     
940.   {
     
941.     for(int xk=wanqishi;xk>=valuerownum;xk--)
     
942.     {
     
943.       if((blackleft[xk]-blackleft[xk-1])<=0&&(blackleft[xk]-blackleft[xk-2])<=0&&zuobian_qi==0)
     
944.         zuobian_qi=xk-2;
     
945.       if((blackleft[xk]-blackleft[xk-1])<=0&&(blackleft[xk]-blackleft[xk-2])<=0&&zuobian_stop==0)
     
946.         zuobian_youxiao=xk-2; //右彎左側(cè)連續(xù)
     
947.       else
     
948.         zuobian_stop=1;
     
949.       if(blackright[xk]==DATACOUNT&&blackright[xk+1]==DATACOUNT&&blackright[xk-1]==DATACOUNT&&youwan_flag==0
     
950.          &&ImageBuf2[xk-1][blackright[xk-1]-1]==0&&ImageBuf2[xk+1][blackright[xk+1]-1]==0)
     
951.       {
     
952.         youwan++;
     
953.         if(youwan>5)
     
954.         {
     
955.           youwan_youxiao=xk;//并且右邊有極點(diǎn)
     
956.           youwan_flag=1;
     
957.         }
     
958.       }
     
959.       else if(blackright[xk]==DATACOUNT&&blackright[xk+1]==DATACOUNT&&blackright[xk-1]!=DATACOUNT&&youwan_flag==0)
     
960.       {
     
961.         youwan_youxiao=xk;//并且右邊有極點(diǎn)
     
962.         youwan_flag=1;
     
963.       }
     
964.      // if(((center[xk]-center[xk-1])>=0)&&(youwan_centerstop==0)&&xk<43)
     
965.       if(((center[xk]-center[xk-1])>0||center[xk-1]>=DATACOUNT)&&(youwan_centerstop==0)&&xk<(wanqishi-4))
     
966.     //  if(((center[xk]-center[xk-1])<=0&¢er[xk-1]<=DATACOUNT)&&(youwan_centerstop==0)&&xk<(wanqishi-4))
     
967.       {
     
968.         youwan_center=xk;//并且平滑后的中心線的方向一致
     
969.         youwan_centerstop=1;
     
970.       }
     
971.      // else if(((center[xk]-center[xk-1])>0||center[xk-1]>=DATACOUNT)&&(youwan_centerstop==0)&&xk<(wanqishi-4))
     
972.         //if(((center[xk]-center[xk-1])>0||center[xk-1]>=DATACOUNT)&&(youwan_centerstop==0)&&xk<(wanqishi-4))
     
973.         
     
974.     }
     
975.     if(youwan_flag==1&&youwan_centerstop&&zuobian_youxiao>4&&(zuobian_youxiao<=youwan_center))
     
976.     {
     
977.       value=youwan_center;//符合以上條件，判定為右彎，重新選擇有效行youwan_youxiao
     
978.       wandao_youxiao=1;
     
979.     }
     
980.   }
     
981.   if((left>(left+right+1)/2)&&(zshizi==0&&zxshizi==0&&yxshizi==0))//左彎
     
982.   {
     
983.     for(int xk=wanqishi;xk>=valuerownum;xk--)
     
984.     {
     
985.       if((blackright[xk]-blackright[xk-1])>=0&&(blackright[xk]-blackright[xk-2])>=0&&youbian_qi==0)
     
986.         youbian_qi=xk-2;
     
987.       if(youbian_qi!=0&&(blackright[xk]-blackright[xk-1])>=0&&(blackright[xk]-blackright[xk-2])>=0&&youbian_stop==0)
     
988.         youbian_youxiao=xk-2; //左彎右側(cè)連續(xù)
     
989.       else
     
990.         youbian_stop=1;
     
991.       if(blackleft[xk]==0&&blackleft[xk+1]==0&&blackleft[xk-1]==0&&zuowan_flag==0)
     
992.       {
     
993.         zuowan++;
     
994.         if(zuowan>5)
     
995.         {
     
996.         zuowan_youxiao=xk;//并且左邊有極點(diǎn)
     
997.         zuowan_flag=1;
     
998.         }
     
999.       }
     
1000.       else if(blackleft[xk]==0&&blackleft[xk+1]==0&&blackleft[xk-1]!=0&&zuowan_flag==0
      
1001.               &&ImageBuf2[xk-1][blackleft[xk-1]]==0&&ImageBuf2[xk+1][blackleft[xk+1]]==0)
      
1002.       {
      
1003.         zuowan_youxiao=xk;//并且左邊有極點(diǎn)
      
1004.         zuowan_flag=1;
      
1005.       }
      
1006.      // if((((center[xk]-center[xk-1])<=0))&&zuowan_centerstop==0&&(xk<43))
      
1007.        if((((center[xk]-center[xk-1])<0)||center[xk]<=0)&&zuowan_centerstop==0&&(xk<43))
      
1008.       // if((((center[xk]-center[xk-1])>=0)&¢er[xk]>=0)&&zuowan_centerstop==0&&(xk<(wanqishi-4)))
      
1009.       {
      
1010.         zuowan_center=xk;//并且平滑后的中心線的方向一致
      
1011.         zuowan_centerstop=1;
      
1012.       }
      
1013.      // else //if((((center[xk]-center[xk-1])<0)||center[xk]<0)&&zuowan_centerstop==0&&(xk<(wanqishi-4)))
      
1014.         
      
1015.     }
      
1016.     if(zuowan_flag==1&&zuowan_centerstop&&youbian_youxiao>4&&(youbian_youxiao<=zuowan_center))
      
1017.     {
      
1018.       
      
1019.       value=zuowan_center;//符合以上條件，判定為左彎，重新選擇有效行zuowan_youxiao
      
1020.       wandao_youxiao=1;
      
1021.     }
      
1022.   }
      
1023. }
      
1024. int ii,jj;
      
1025. void TurnPWM()
      
1026. {
      
1027.    if(valuerownum>=47)//有效行離車太近，可能沖出賽道了
      
1028.    {
      
1029.      wanqishi=45;//虛擬起始點(diǎn)，判斷是否有噪點(diǎn)影響了有效行的讀取
      
1030.      valuerownum=valuerownum-13;
      
1031.      panduan_youxiao();//克服平滑濾波帶來的影響，尋找真正有效點(diǎn)
      
1032.      if(wandao_youxiao==1)//如果是彎道
      
1033.        valuerownum=46; //這樣，數(shù)據(jù)就不會(huì)丟棄了，就會(huì)進(jìn)入下一個(gè)if語句里
      
1034.      else
      
1035.        badframe=1;//不是彎道，舍棄數(shù)據(jù)
      
1036.    }
      
1037.    if(!badframe)//如果數(shù)據(jù)有效
      
1038.    {
      
1039.      CenterEx[1]=CenterEq;
      
1040.      sum=0;ii=0;
      
1041.      if(valuerownum>=35)//最近處，一般是急彎道
      
1042.      {   
      
1043.       
      
1044.         wanqishi=valuerownum+5;//虛擬起始點(diǎn)，判斷是否有噪點(diǎn)影響了有效行的讀取   下同，不做進(jìn)一步注釋
      
1045.         if(wanqishi>46)  wanqishi=46;
      
1046.         valuerownum=valuerownum-10;
      
1047.         panduan_youxiao();//克服平滑濾波帶來的影響，尋找真正有效點(diǎn)
      
1048.         if(wandao_youxiao==1)
      
1049.           valuerownum=value;//有效的話把實(shí)際的有效行求出
      
1050.         else
      
1051.           valuerownum=valuerownum+10;//無效的話，還原原值
      
1052.      }
      
1053.      else if(valuerownum>=17&&valuerownum<35)//有效行在圖像中部
      
1054.      {
      
1055.        wanqishi=valuerownum+9;
      
1056.         valuerownum=valuerownum-8;
      
1057.         panduan_youxiao();
      
1058.         if(wandao_youxiao==1)
      
1059.         {
      
1060.           valuerownum=value;
      
1061.         }
      
1062.         else
      
1063.           valuerownum=valuerownum+8;
      
1064.      }
      
1065.       else  if(valuerownum>=9&&valuerownum<17)//有效行靠前，離車較遠(yuǎn)了
      
1066.       {   
      
1067.         wanqishi=valuerownum+12;
      
1068.         valuerownum=valuerownum-2;
      
1069.         if(valuerownum<=0)  valuerownum=0;      
      
1070.         panduan_youxiao();
      
1071.         if(wandao_youxiao==1)
      
1072.         {
      
1073.           valuerownum=value;
      
1074.         }
      
1075.         else
      
1076.         {
      
1077.           valuerownum=valuerownum+2;
      
1078.         }  
      
1079.      }
      
1080.      else  if(valuerownum>=0&&valuerownum<9)//有效行離車更遠(yuǎn)了，說明是直道或小s
      
1081.      {   
      
1082.         zhidao=1;
      
1083.         wandao=0;
      
1084.      }
      
1085.      for(int i=47;i>(valuerownum+1);i--)//五點(diǎn)平滑濾波，使中線更平滑
      
1086.         center[i]=(center[i+2]+center[i+1]+center[i]+center[i-1]+center[i-2])/5;
      
1087.      for(jj=valuerownum;jj<DATALINE;jj++)//濾波后進(jìn)行加權(quán)平均，
      
1088.      {
      
1089.        sum+=(center[jj]-TurnMidPosition)*CorrectEx[jj];
      
1090.        ii++;      
      
1091.      }
      
1092.      CenterEq=(sum/ii); //得到平均值
      
1093.    }                                                                 
      
1094.    else//無效時(shí)保持原來的數(shù)據(jù)
      
1095.    {      
      
1096.       CenterEq=lasterror;
      
1097.       lasterror=CenterEq;
      
1098.       badframe=0;
      
1099.    }
      
1100.    if(CenterEq>70)//對(duì)加權(quán)平均進(jìn)行限幅，也可不限，數(shù)值根據(jù)自己的小車定，
      
1101.    {              //一般達(dá)不到限幅值，為了防止突發(fā)情況
      
1102.      CenterEq=70;
      
1103.    }
      
1104.     else if(CenterEq<-70)
      
1105.     {
      
1106.       CenterEq=-70;
      
1107.     }
      
1108.     LastTurnPWMOUT=TurnPWMOUT;//新的給舊的
      
1109.     TurnPWMOUT=(-TurnP*CenterEq-TurnD*lasterror) ; //得出轉(zhuǎn)向PWM，但此時(shí)還未加舵機(jī)中值
      
1110.     lasterror=(CenterEq-CenterEx[1]);
      
1111.    
      
1112. }
      
1113. 
      
1114. /***********************************************************
      
1115. 函數(shù)名稱：TurnPWMOut
      
1116. 函數(shù)功能：
      
1117. 入口參數(shù)：NewspeedPWM      當(dāng)前的電機(jī)輸出PWM
      
1118.           LastspeedPWM     上次電機(jī)輸出PWM
      
1119.           PeriodCount      平滑周期
      
1120. ***********************************************************/
      
1121. #define TURNPERIODFAV  (30) //攝像頭一幀33ms，取30，容易整除，也可不用平滑輸出
      
1122. s16 TurnPWMOut(s16 NewturnPWM ,s16 LastturnPWM,s16 PeriodCount)
      
1123. {
      
1124.   s16  turnPWMfav ;
      
1125.   s16  turnOUT ;
      
1126.   turnPWMfav = NewturnPWM - LastturnPWM ;
      
1127.   turnOUT = turnPWMfav *(PeriodCount)/TURNPERIODFAV + LastturnPWM ;
      
1128.   return turnOUT ;
      
1129. }
      
1130. void fasong_erzhihua_bianxian()//發(fā)送到串口調(diào)試助手
      
1131. {
      
1132.   int i,j,cha;
      
1133.   for(i=49;i>=0;i--)
      
1134.   {
      
1135.   ImageBuf2[i][center[i]]='*';
      
1136.   }  
      
1137.   uart_putchar (UART0,'\n' );
      
1138.   uart_putchar (UART0,'0' );
      
1139.   uart_putchar (UART0,'0' );
      
1140.   uart_putchar (UART0,'0' );
      
1141.   for(i=0;i<13;i++)
      
1142.   {
      
1143.     for(j=0;j<10;j++)
      
1144.     {
      
1145.        uart_putchar (UART0,j+0x30 );
      
1146.     }
      
1147.   }
      
1148.   uart_sendStr(UART0,"  ");
      
1149.   uart_sendStr(UART0,"XL:");
      
1150.   if(slope<0)
      
1151.   {
      
1152.     uart_putchar(UART0,'-');
      
1153.     slope=-slope;
      
1154.   }
      
1155. ……………………
      
1156. 
      
1157. …………限于本文篇幅 余下代碼請(qǐng)從51黑下載附件…………