标题: 飞思卡尔智能车国赛CCD摄像头组源程序 [打印本页]

---

作者: Jonelll    时间: 2018-3-4 12:58
标题: 飞思卡尔智能车国赛CCD摄像头组源程序
飞思卡尔国赛摄像头组程序

单片机源程序如下:

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 产生中断的计数次数
   
7. extern u32 LPT_INT_count;
   
8. s16 count;
   
9. #define duoji_center 2400//*舵机中值，根据自己的小车自行修改
   
10. /*  接口：PWM：PTC1，PTC2，PTC3，PTC4
    
11.           舵机：PTA8
    
12.          摄像头：行中断：PTD13，场中断：PTC8，PCLK：PTD12，数据端：PTD0~PTD7
    
13. 带有//*的是需要修改的参数  关于下面的P、I、D最好先清零，即SpeedKP=0,SpeedKI=0,TurnP=0,TurnD=0*/
    
14. //PID参数
    
15. float TurnP=4.4,TurnD=0.6;//*转向PD
    
16. float SpeedKP =6;//*速度P  过大，车轮旋转一前转一后转，表现为走走停停
    
17. float SpeedKI =1;//*速度I
    
18. float g_Speedgoal=500;//*目标速度 即编码器的目标脉冲值
    
19. //时间标志位
    
20. extern u8 TIME1flag_100ms,flag_1ms ;
    
21. extern u8 TIME1flag_1s   ;  //PT1口1s定时标志位
    
22. int start_flag=0,stop_jiasu=0;//开机静止跑道2秒标志
    
23. s32 ATimeCount=0;//100ms进入pid标志
    
24. s32 TimeCount ;//1ms中断标志
    
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); //程序运行灯
    
48.   // oled_init();
    
49.    CCD_init ();  //摄像头初始化
    
50.    pit_init_ms(PIT0, 1);  //初始化PIT0，定时时间为： 1ms
    
51.    pit_init_ms(PIT1, 100);//初始化PIT1，定时时间为： 100ms
    
52.    set_irq_priority (90,3);//行中断
    
53.    set_irq_priority (89,2);//场中断
    
54.    set_irq_priority (84,1);//PIT0中断 优先级最高
    
55.    FTM_init() ; //FTM初始化
    
56.    lptmr_counter_init(LPT0_ALT2,INT_COUNT,2,LPT_Rising);//PTC5脚 脉冲累加计数器
    
57.    EnableInterrupts;//开总中断  
    
58.    uart_irq_EN(UART0);//开串口0中断
    
59.    while(1)
    
60.     {
    
61.      if(flag_1ms==1)//运行run函数
    
62.      {
    
63.        flag_1ms=0;
    
64.        run();
    
65.      }
    
66.    // display_suducaiji();//速度显示函数
    
67.   }
    
68. }
    
69. void run()//在isr.c中的1ms中断中调用  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);//电机输出
    
84.   }
    
85.    else if(TimeCount == 2)//速度控制
    
86.    {
    
87.      ATimeCount ++ ;
    
88.      if(ATimeCount >= 20)
    
89.      {
    
90.        ATimeCount=0;
    
91.        sudu_xianshi=1;//采集完速度的标志，用于向上位机发送速度
    
92.        count =get_counter_value(); //读取LPTMR0_CNR之前需要先对这个寄存器赋值！！！
    
93.        lptmr_counter_clean(); //清空脉冲计数器计算值（马上清空，这样才能保证计数值准确）
    
94.        g_nLeftMotorPulseSigma=LPT_INT_count*INT_COUNT+count;//每LPT_INT_count个脉冲产生一次中断，LPT_INT_count为中断的次数，
    
95.        if(PWMout>0)//由于这一路没有使用正交解码，因此速度脉冲方向由电机方向判定
    
96.          g_nLeftMotorPulseSigma=-g_nLeftMotorPulseSigma;
    
97.        LPT_INT_count=0;//对中断次数清零 //count为不足产生一次中断的值
    
98.        lm=g_nLeftMotorPulseSigma;//获得左、右轮速度用于上位机显示
    
99.        rm=g_nRightMotorPulseSigma;
    
100.        SpeedPID() ; //速度PI调节
     
101.        SpeedPeriodCount = 0 ;
     
102.      }
     
103.    }
     
104.   else if(TimeCount == 3)//方向控制
     
105.   {
     
106.      FTM_CnV_REG(FTMx[FTM1], CH0) =MotorTurnPWM+duoji_center;//方向控制  要加上舵机中值
     
107.      LastTurnPWMOUT = TurnPWMOUT ;
     
108.      TurnPeriodCount = 0 ;
     
109.   }  
     
110.   else if(TimeCount == 4)
     
111.   {
     
112.     if(TIME1flag_1s == 1)//程序运行灯
     
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;//根据近端中线方向判别断行处应该具有的方向
    
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;//十字 有点复杂
    
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;//斜十字标志
    
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;//转向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. //加权平均参数，一般是远大近小，间隔均匀，根据小车情况调
    
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()//发送原始图像到红树伟业上位机
    
75. {
    
76.   int i,j;
    
77.   for(i=0;i<DATALINE;i++)//采样行数 50
    
78.   {        
    
79.     for(j=32;j<lie;j++)//采样点数 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)//作用： 求绝对值  浮点型
    
89. {
    
90.   if(i<0) return (-i);
    
91.   else  return i;
    
92. }
    
93. s32 absi(s32 i)//作用： 求绝对值  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++)//采样点数  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()//作用： 将采集的数据二值化
     
116. {
     
117.   u16 i,j;
     
118.   for(i=0;i<DATALINE;i++)//由于实验室光线不均匀，近端与远端使用不同的阈值
     
119.   {  
     
120.     if(i>30)//近端
     
121.       yuzhizhi=yuzhi(i)+12;//加大后黑色变多  根据场地不同调参数
     
122.     else    //远端
     
123.       yuzhizhi=yuzhi(i)+19;//根据场地不同调参数
     
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)//只是预判，与最终中线无关！！！
     
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;//中心预判
     
141.      else if(blackright[is]>(DATACOUNT-2)&&blackleft[is]!=0&&(is>0))
     
142.        centerzuoyupan[is]=blackleft[is]+bxiankuan[is];//一端没线，粗略地根据赛道进行平移
     
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++)//对图像处理参数初始化
     
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.   //计算前2行的左右边缘
     
181.   for(i=DATALINE-1;i>DATALINE-3;i--)
     
182.   {
     
183.     for(j=(DATACOUNT/2-10);j<(DATACOUNT-1);j++)//计算前2行右边界  起始点靠左一点
     
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--)        //计算前2行左边界  起始点靠右一点
     
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行循环
     
205. //over
     
206.   rightstart=blackright[DATALINE-2]-3;//根据前两行来确定搜索的起始点
     
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. //计算右边界的有效行数和右边界
     
215.   for(i=DATALINE-3;i>=0;i--)//
     
216.   {
     
217.     for(j=rightstart;j<rightend;j++)//右线用0xx011，比较稳定可靠
     
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)  //在范围内找到黑线，这个地方需要注意
     
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)         //在范围内没有找到黑线
     
235.    {
     
236.      rightstart=rightstart-bxiankuan[i];//平移半个赛道宽重新找黑线  可优化
     
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) //在范围内找到黑线&&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)         //在范围内没有找到黑线
     
257.         rightstart=rightstart+bxiankuan[i];//还未找到？起始点还原到上次确定的起始点
     
258.    }  
     
259.     for(m=leftstart;m>leftend;m--)//左线用110xx0   以下有上面对称，不再注释
     
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.    }//右边界循环结束
     
301.     if(blackleft[i]!=0&&blackright[i]!=DATACOUNT)
     
302.      centerxunizuo[i]=(blackleft[i]+blackright[i])/2;//计算虚拟中线
     
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. //十字处理函数，比较麻烦，情况分为正十字(分为远处四点都有，近处只有两点两种情况)，左斜十字和右斜十字，
     
310. //在实际操作时可能会发现，各自分类的界限并没有很明确，可能斜十字的情况在正十字里就补好线了，这是为了
     
311. //有交集，这样不会丢失各种情况，不会有“都不管”的情况
     
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. /////////////////////////正十字补线///////////////////////
     
321.     //连续3行以上都是全白，且中点也是白
     
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.        {                                                                         //其实前面两条就够了，后面这两条为了保险才加的
     
331.           shizi_flag=is-1; //十字结束行 在起始行的基础上继续搜索得到
     
332.           break;//跳出，省时间
     
333.        }
     
334.    }
     
335.    if((youxiao_shizi-shizi_flag)>=1&&(youxiao_shizi-shizi_flag)<=25)//对白色纵向宽度做限制
     
336.    {
     
337.      for(is=DATALINE-1;is>youxiao_shizi;is--)
     
338.      { //左下端特征：边线存在且大于0；拐点之下由近端到拐点呈递增趋势（且比较陡），之上呈递减趋势（且比较缓）.
     
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;//由近到远 寻找左下端拐点
     
343.           zstop=1;
     
344.         }
     
345.         //右下端特征：边线存在且小于DATACOUNT；拐点之下由近端到拐点呈递减趋势（且比较陡），之上呈递增趋势（且比较缓）.
     
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;//寻找右下端拐点
     
350.           ystop=1;
     
351.         }
     
352.      }
     
353.      //此处针对靠下的拐点
     
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;//一端没有，用另一端的作为最后的拐点
     
357.      else if(zzxqishi!=0&&yzxqishi==0) zxqishi=zzxqishi;
     
358.      else zxqishi=0;//两端都没有，即车体进入十字中，此时没有最下端的拐点
     
359.     // yupan_fangxiang(zxqishi);//主要为下面服务的  预判函数
     
360.    }
     
361.    if((youxiao_shizi-shizi_flag)>=1&&(youxiao_shizi-shizi_flag)<=25)
     
362.    {//由于存在十字的原因，之前搜到的边线可能不准，需要重新搜线，需要确定搜线的起始列，即left_sz，right_sz
     
363.       //最远行对应的列坐标应该位于赛道的中心位置！！！
     
364.      for(int jdf=0;jdf<DATACOUNT;jdf++)
     
365.        shizibx[jdf]=shizi_flag;//初始化为十字的结束行
     
366.      jdt=shizi_flag;//初始化为十字的结束行
     
367.      for(jd=0;jd<DATACOUNT;)//纵向搜线，寻找最远端
     
368.      {
     
369.        for(id=shizi_flag;id>0;id--)//从十字的结束行往上搜索直到第一行
     
370.        {
     
371.          if(ImageBuf2[id+1][jd]==0&&ImageBuf2[id][jd]==1&&ImageBuf2[id-1][jd]==1)
     
372.          {//由下到上纵向搜索，011成立便找到点了
     
373.            shizibx[jd]=id;//将第id行存入数组中
     
374.            break;
     
375.          }
     
376.        }
     
377.        jd+=10;//此时只是粗略地找最远端，所以不需要每一列都搜索，隔10列可以节约时间
     
378.      }
     
379.      for(int jdf=0;jdf<DATACOUNT;jdf++)
     
380.      {//最远处的行应小于“上一次的最远行”！！！，且不等于结束行
     
381.        if((shizibx[jdf]<shizibx[jdt])&&shizibx[jdf]!=shizi_flag)
     
382.         jdt=jdf;//获得最远行对应的列坐标，如此反复进入几次就能把最远行对应的列坐标求出
     
383.      }
     
384.        left_sz=jdt+10;//有了列坐标就能推出左右的起始列了。因为最远行对应的列坐标应该位于赛道的中心位置！！！
     
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，且列大于左下端点的列坐标或 其根本没有下端点
     
391.        {
     
392.          if(ImageBuf2[ig][jg-1]==1&&ImageBuf2[ig][jg]==1&&ImageBuf2[ig][jg+1]==0&&(jg>=blackleft[zzxqishi]||zxqishi==0)) //在范围内找到黑线  
     
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)))//限制条件，求出的赛道宽度小于直道的宽度加一个允许波动值
     
400.        {//由于赛道不是直线，每5行更新一次left_sz，作为下一次搜线的起始列
     
401.          kk=0;
     
402.          left_sz=blackleft[ig+1]+bxiankuan[ig+1];//根据上一行的左线虚拟出下一次的起始列
     
403.        }
     
404. 
     
405.        if(right_sz<blackleft[ig])   right_sz=blackleft[ig];//越界限制
     
406.        for(int mg=right_sz;mg<=DATACOUNT-1;mg++)//右线是011，且列小于右下端点的列坐标或 其根本没有下端点
     
407.        {
     
408.          if(ImageBuf2[ig][mg-1]==0&&ImageBuf2[ig][mg]==1&&ImageBuf2[ig][mg+1]==1&&(mg<=blackright[yzxqishi]||zxqishi==0)) //在范围内找到黑线
     
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];//根据上一行的右线虚拟出下一次的起始列
     
418.          }
     
419.        }
     
420.      }
     
421.      for(is=DATALINE-3;is>1;is--)//求出拐点！
     
422.      {
     
423.         as=(blackleft[is]-blackleft[is-2]);//与上两行做一次差        
     
424.         bs=(blackleft[is]-blackleft[is+2]);//与下两行做一次差
     
425.         js=blackleft[is]; //为该行的列坐标
     
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;//上面  左上拐点的特征：之下为递增（或直接增），之上为递增，之下两行的绝对值比较小，左上拐点在十字起始行上面，且在一个范围内，周围几个点的白色多一些
     
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;//下面 左下拐点的特征：之下为递增，之上为递减（或直接减），在十字起始行下面
     
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;//上面  右上拐点的特征：之下为递减（或直接减），之上为递减，。。。
     
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;//下面  右下拐点的特征：之下为递减，之上为递增（或直接增）
     
437.      }
     
438. //有下端拐点的进来！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！     
     
439.      //下端拐点在十字开始行的下面，且（左线在中线以左   或              下端拐点处的右线在中线以右），
     
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))                             //有拐点的进来
     
442.      {//且下端拐点的确存在，且左、右拐点至少存在一个；限幅
     
443.         if(xs!=0&&ys!=0&&ys<=(zxqishi+10)&&ys>=(zxqishi-10)&&blackleft[xs]>=blackleft[ys])//左侧进行连线
     
444.         {//左侧上下拐点都存在，且下拐点在下端点拐点的合理范围之内，左上拐点比左下拐点靠右
     
445.           for(int iz=xs-1;iz<=ys+1;iz++)//一次函数连线
     
446.           {//为了更加平滑，各让一步
     
447.              blackleft[iz+1]=(blackleft[iz]-blackleft[ys+1])/(iz-ys-1)+blackleft[iz];//注意顺序，上面拐点在左还是在右
     
448.              zshizi=1;//正入十字标志置1，为下面使用
     
449.           }
     
450.         }//同样，不作注释
     
451.         if(es!=0&&fs!=0&&fs<=(zxqishi+10)&&fs>=(zxqishi-10)&&blackright[fs]>=blackright[es])//右侧进行连线
     
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;//正入十字标志置1，为下面使用
     
457.           }
     
458.         }
     
459.         //为了下面求中线的起始行和结束行而使用
     
460.         if(xs>=es)  szq=es;//仅为标准正十字服务
     
461.         else        szq=xs;//十字起始行以上为主
     
462.         if(ys>=fs)  szs=ys;
     
463.         else        szs=fs;//十字接收行以下为主
     
464.         if(xs!=0&&ys!=0&&es!=0&&fs!=0)//标准正十字 四拐点都存在
     
465.         {
     
466.           for(i=szq-1;i<=szs+1;i++)//为了平滑，各让一步
     
467.             if(center[i]==0)//由于前面已经补线了，此处只需求中值即可
     
468.                center[i]=(blackright[i]+blackleft[i])/2;
     
469.         }
     
470.         else if(xs!=0&&ys!=0&&(es==0||fs==0))//右侧丢失
     
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))//左侧丢失
     
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. //近端处理  无下拐点的进来！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！
     
484.     else if(youxiao_shizi>=30&&shizi_flag>=24&&(es!=0&&xs!=0)&&(es>=(xs-13)&&xs>=(es-13)))//如果刚进入十字，则只有最上面两个拐点，
     
485.     {     //十字比较靠近车                     且两个上拐点都存在，es与xs在合理的范围内                              
     
486.       for(int iz=xs-1;iz<DATALINE-1;iz++)//此时补线利用左拐点与左下角坐标值连线，右拐点与右下角坐标值连线
     
487.       {//一次函数连线
     
488.         blackleft[iz+1]=-(blackleft[iz])/(DATALINE-1-iz)+blackleft[iz];//注意顺序，上面拐点在左还是在右
     
489.         zshizi=1;//正入十字标志置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;//正入十字标志置1，为下面使用
     
495.       }
     
496.       if(xs>=es)    szq=es;//选取靠上的拐点，作为下面求中线的起始行
     
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)//给下端补线
     
509.             center[i]=blackright[i]-(blackright[i-1]-center[i-1]+1);
     
510.           else if(center[i+1]!=0)//给上端补线
     
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)//给下端补线
     
518.             center[i]=blackleft[i]+(center[i-1]-blackleft[i-1]+1);
     
519.           else if(center[i+1]!=0)//给上端补线
     
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. //左侧斜十字补线     各标志位清零
     
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.    {//连续两行的左线都存在  下增上减
     
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;//先寻找左下拐点
     
535.         break;
     
536.       }
     
537.    }
     
538.    if(zuoqishi!=0)//左下拐点存在才能进入
     
539.    {
     
540.      for(is=zuoqishi;is>2;is--)//从左下拐点向上寻找
     
541.      {                       //连续三行左端都没找到线
     
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.        {//并且连续两行左端 二值化图像 都是白的
     
545.           if(youxiaozuoxieshizi==0)//进行遍历，寻找起始行
     
546.              youxiaozuoxieshizi=is;
     
547.        }                        //连续两行没找到线，且从下到上是白到黑或左线找到了
     
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))//一般上侧结束行都是极值，只有分辨图像是黑是白
     
550.        {
     
551.          xieshizi_zuoflag=is-1;//寻找结束行
     
552.          break;//找到就跳出，省时间
     
553.        }
     
554.      }
     
555.    }
     
556. //斜十字补线 左侧  白色区域在合理范围内，                                               下拐点在白色下面
     
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;//根据预判得出虚拟中线，
     
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]) //在范围内找到黑线
     
567.              {
     
568.                 blackleft[ig]=jg;  //更新左线数组
     
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--)//斜十字补线 再判断 由于前面更新了左线数组，此时再次确认无误
     
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;//得出结束行
     
591.            break;
     
592.          }
     
593.        }
     
594.        for(is=DATALINE-3;is>=0;is--)//十字补线，同正十字
     
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++)//只能补一侧
     
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;//左斜十字标志置1
     
614.          }
     
615.          for(int iz=DATALINE-1;iz>=3;iz--)//把右侧没搜到线的行的中线根据左侧补出来
     
616.          {
     
617.            if(blackleft[iz]>=1&&blackright[iz]>=(DATACOUNT-1))
     
618.              center[iz]=blackleft[iz]+bxiankuan[iz];//
     
619.          }
     
620.        }
     
621.      }
     
622. //右侧 斜十字补线/////
     
623.      for(int is=DATALINE-3;is>=5;is--)//斜十字补线   右侧 预判断
     
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;//右侧的下端拐点
     
629.           break;
     
630.        }
     
631.      }
     
632.      for(is=youqishi;is>=5;is--)//得出白色区域
     
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)//开始记录白点
     
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;//遇着黑点退出
     
644.           break;
     
645.         }
     
646.      }                                                                                     //右下拐点靠右
     
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);//根据得出的虚拟中线，来确定再次搜线的起始列
     
651.         for(int ig=youqishi;ig>=0;ig--)//对右线数组重新搜索
     
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]) //在范围内找到黑线
     
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--)//斜十字补线   由于更新了数组，需要再判断
     
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;//结束行
     
684.             break;
     
685.           }
     
686.         }
     
687.         for(is=DATALINE-3;is>=0;is--)//斜十字补线
     
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)//斜十字畸变较大，前后多找些点进行补
     
698.         {
     
699.           for(int iz=xiees;iz<(xiefs+3);iz++)//对右侧补完线并求中心值
     
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. /////////////对寻到的边情况分类并处理拟合中线////////
     
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)//右侧丢线
     
728.        {
     
729.          if(yici_you==0)//右侧开始丢线处
     
730.            yici_you=i;//记录起始行
     
731.          if(youxiaoyou==0)
     
732.          {
     
733.            for(int bs=i;bs>2;bs--)
     
734.            {//连续三行的右线为都有值   或连续两行是二值化值是黑的
     
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;//结束行
     
739.                break;
     
740.              }
     
741.            }
     
742.          }
     
743.          if(youxiaozuobian==0)
     
744.          {
     
745.            for(int bs=i;bs>youxiaoyou-5;bs--)//判断左线连续性，并确定不连续行
     
746.            {
     
747.              if((blackleft[bs]-blackleft[bs-1])>0||blackleft[bs]>=(DATACOUNT-1)||blackleft[bs-1]>=(DATACOUNT-1))
     
748.              {//本来左线是递增向右的，当遇到左线递减了或跑到右边沿了就进入，
     
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;//递增时一直更新此值
     
754.            }
     
755.          }
     
756.          if(i==49||i==48||(i==47))//如果靠近车，应该是近端丢线，先判断下
     
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)//验证是否确实是近端丢线  此值应离丢线处不远
     
766.                  {
     
767.                    for(int as=youxiaoyou;as<=49;as++)//能进来，可以确定确实近端丢线了
     
768.                    {
     
769.                       if(center[as]==0)
     
770.                       {
     
771.                         jinduan_ybx=1;//置位标志  //根据前一行的中线求出该行的中线
     
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;//补线完成跳出程序
     
777.                }
     
778.                else break;//若为进入急弯，则会跳出
     
779.              }
     
780.            }
     
781.            if(jinduan_ybx==0&&yici_you>=46&&youxiaoyou<38&&(youxiaozuobian<=yici_you))//如果不是近端丢线，而是进入急弯  不
     
782.            {//建议摄像头的镜头在120度以上，90度的容易进入此处
     
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;//改变系数1的大小，可改善进入急弯的转向能力
     
788.                 }
     
789.              }
     
790.            }
     
791.         }
     
792.          else  if(center[i]==0&&blackleft[i]!=0)//弯道丢线时
     
793.           center[i]=center[i+1]-blackleft[i+1]+blackleft[i]+4;//改变系数4的大小，可改善进入急弯的转向能力
     
794.        }
     
795.        else if(blackright[i]<(DATACOUNT-1)&&blackleft[i]==0&¢er[i]==0)//左侧丢线
     
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--)//判断右线连续性，并确定不连续行
     
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.         //重新遍历最近端的点   判断是否进入急弯，即左侧或右侧一大块没寻到线的地方
     
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)//验证是否确实是近端丢线
     
835.                  {
     
836.                    for(int as=youxiaozuo;as<=49;as++)
     
837.                    {
     
838.                     if(center[as]==0)
     
839.                     {
     
840.                       jinduan_zbx=1;//置位标志
     
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;//补线完成跳出程序
     
846.                }
     
847.                else
     
848.                  break;//若为进入急弯，则会跳出
     
849.              }
     
850.            }
     
851.            if(i>47&&jinduan_zbx==0&&yici_zuo>=48&&youxiaozuo<40&&(youxiaoyoubian<=yici_zuo))//如果不是近端丢线，而是进入急弯
     
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)//进入弯道
     
861.          {         
     
862.            center[i]=blackright[i]-(blackright[i+1]-center[i+1]+3);//改变系数3的大小，可改善进入急弯的转向能力
     
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;//中心坏行标号
     
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. //作用：计算有效行          根据上面黑线的中心偏差来求解有效行
     
923. //改变全局变量： 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; //右弯左侧连续
     
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;//并且右边有极点
     
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;//并且右边有极点
     
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; //左弯右侧连续
     
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;//并且左边有极点
     
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;//并且左边有极点
      
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;//虚拟起始点，判断是否有噪点影响了有效行的读取
      
1030.      valuerownum=valuerownum-13;
      
1031.      panduan_youxiao();//克服平滑滤波带来的影响，寻找真正有效点
      
1032.      if(wandao_youxiao==1)//如果是弯道
      
1033.        valuerownum=46; //这样，数据就不会丢弃了，就会进入下一个if语句里
      
1034.      else
      
1035.        badframe=1;//不是弯道，舍弃数据
      
1036.    }
      
1037.    if(!badframe)//如果数据有效
      
1038.    {
      
1039.      CenterEx[1]=CenterEq;
      
1040.      sum=0;ii=0;
      
1041.      if(valuerownum>=35)//最近处，一般是急弯道
      
1042.      {   
      
1043.       
      
1044.         wanqishi=valuerownum+5;//虚拟起始点，判断是否有噪点影响了有效行的读取   下同，不做进一步注释
      
1045.         if(wanqishi>46)  wanqishi=46;
      
1046.         valuerownum=valuerownum-10;
      
1047.         panduan_youxiao();//克服平滑滤波带来的影响，寻找真正有效点
      
1048.         if(wandao_youxiao==1)
      
1049.           valuerownum=value;//有效的话把实际的有效行求出
      
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)//有效行靠前，离车较远了
      
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)//有效行离车更远了，说明是直道或小s
      
1081.      {   
      
1082.         zhidao=1;
      
1083.         wandao=0;
      
1084.      }
      
1085.      for(int i=47;i>(valuerownum+1);i--)//五点平滑滤波，使中线更平滑
      
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++)//滤波后进行加权平均，
      
1088.      {
      
1089.        sum+=(center[jj]-TurnMidPosition)*CorrectEx[jj];
      
1090.        ii++;      
      
1091.      }
      
1092.      CenterEq=(sum/ii); //得到平均值
      
1093.    }                                                                 
      
1094.    else//无效时保持原来的数据
      
1095.    {      
      
1096.       CenterEq=lasterror;
      
1097.       lasterror=CenterEq;
      
1098.       badframe=0;
      
1099.    }
      
1100.    if(CenterEq>70)//对加权平均进行限幅，也可不限，数值根据自己的小车定，
      
1101.    {              //一般达不到限幅值，为了防止突发情况
      
1102.      CenterEq=70;
      
1103.    }
      
1104.     else if(CenterEq<-70)
      
1105.     {
      
1106.       CenterEq=-70;
      
1107.     }
      
1108.     LastTurnPWMOUT=TurnPWMOUT;//新的给旧的
      
1109.     TurnPWMOUT=(-TurnP*CenterEq-TurnD*lasterror) ; //得出转向PWM，但此时还未加舵机中值
      
1110.     lasterror=(CenterEq-CenterEx[1]);
      
1111.    
      
1112. }
      
1113. 
      
1114. /***********************************************************
      
1115. 函数名称：TurnPWMOut
      
1116. 函数功能：
      
1117. 入口参数：NewspeedPWM      当前的电机输出PWM
      
1118.           LastspeedPWM     上次电机输出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()//发送到串口调试助手
      
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. …………限于本文篇幅 余下代码请从51黑下载附件…………
      

复制代码

所有资料51hei提供下载:
国赛摄像头程序.rar (12.99 MB, 下载次数: 133)

2018-3-4 12:57 上传

点击文件名下载附件
国赛摄像头组程序
下载积分: 黑币 -5

---

作者: dwwd    时间: 2020-3-6 14:40
你好，我下载了你的程序，但是没有发现程序打开的地方啊，是keil还是什么软件

---

欢迎光临 (http://www.51hei.com/bbs/)

Powered by Discuz! X3.1