找回密碼
 立即注冊(cè)

QQ登錄

只需一步，快速開(kāi)始

搜索
查看: 6248|回復(fù): 1
打印 上一主題 下一主題
收起左側(cè)

飛思卡爾智能車(chē)國(guó)賽CCD攝像頭組源程序

[復(fù)制鏈接]
跳轉(zhuǎn)到指定樓層
樓主
ID:287405 發(fā)表于 2018-3-4 12:58 | 只看該作者 回帖獎(jiǎng)勵(lì) |倒序?yàn)g覽 |閱讀模式
飛思卡爾國(guó)賽攝像頭組程序

單片機(jī)源程序如下:
  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ù)自己的小車(chē)自行修改
  10. /*  接口：PWM：PTC1，PTC2，PTC3，PTC4
  11.           舵機(jī)：PTA8
  12.          攝像頭：行中斷：PTD13，場(chǎng)中斷：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  過(guò)大，車(chē)輪旋轉(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;//開(kāi)機(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);//場(chǎng)中斷
  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;//開(kāi)總中斷  
  58.    uart_irq_EN(UART0);//開(kāi)串口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)//由于這一路沒(méi)有使用正交解碼，因此速度脈沖方向由電機(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. }

復(fù)制代碼
  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;//擬合中線(xiàn)
  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ù)近端中線(xiàn)方向判別斷行處應(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;//障礙起始  可惜沒(méi)用上
  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ù)小車(chē)情況調(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) ;          //場(chǎng)中斷
  72.   exti_init(PORTD,13, falling_down) ;         //行中斷
  73. }
  74. void fasongtuxiang()//發(fā)送原始圖像到紅樹(shù)偉業(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. }

  115. void erzhihua()//作用： 將采集的數(shù)據(jù)二值化
  116. {
  117.   u16 i,j;
  118.   for(i=0;i<DATALINE;i++)//由于實(shí)驗(yàn)室光線(xiàn)不均勻，近端與遠(yuǎn)端使用不同的閾值
  119.   {  
  120.     if(i>30)//近端
  121.       yuzhizhi=yuzhi(i)+12;//加大后黑色變多  根據(jù)場(chǎng)地不同調(diào)參數(shù)
  122.     else    //遠(yuǎn)端
  123.       yuzhizhi=yuzhi(i)+19;//根據(jù)場(chǎng)地不同調(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ù)判，與最終中線(xiàn)無(wú)關(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];//一端沒(méi)線(xiàn)，粗略地根據(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;//算出虛擬中線(xiàn)，為后面決策作參考
  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ù)前兩行來(lái)確定搜索的起始點(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++)//右線(xiàn)用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)找到黑線(xiàn)，這個(gè)地方需要注意
  222.        {
  223.          blackright[i]=j;
  224.          rightstart=j-4;//找到黑線(xiàn)，為下一行的起始賦值
  225.          rightend=j+4;
  226.          if(rightstart<=1)         
  227.            rightstart=1;
  228.          if(rightend>=DATACOUNT)
  229.          rightend=DATACOUNT;
  230.          m1++;//無(wú)用，可刪掉
  231.          break;
  232.        }
  233.    }
  234.    if(j==rightend)         //在范圍內(nèi)沒(méi)有找到黑線(xiàn)
  235.    {
  236.      rightstart=rightstart-bxiankuan[i];//平移半個(gè)賽道寬重新找黑線(xiàn)  可優(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)找到黑線(xiàn)&&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)沒(méi)有找到黑線(xiàn)
  257.         rightstart=rightstart+bxiankuan[i];//還未找到？起始點(diǎn)還原到上次確定的起始點(diǎn)
  258.    }  
  259.     for(m=leftstart;m>leftend;m--)//左線(xiàn)用110xx0   以下有上面對(duì)稱(chēng)，不再注釋
  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ì)算虛擬中線(xiàn)
  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)，各自分類(lèi)的界限并沒(méi)有很明確，可能斜十字的情況在正十字里就補(bǔ)好線(xiàn)了，這是為了
  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ǔ)線(xiàn)///////////////////////
  321.     //連續(xù)3行以上都是全白，且中點(diǎn)也是白
  322.     for(is=DATALINE-1;is>=10;is--)//提取特征：兩端都是白色的，搜不到黑線(xiàn)
  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;//十字開(kāi)始行
  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.      { //左下端特征：邊線(xiàn)存在且大于0；拐點(diǎn)之下由近端到拐點(diǎn)呈遞增趨勢(shì)（且比較陡），之上呈遞減趨勢(shì)（且比較緩）.
  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.         //右下端特征：邊線(xiàn)存在且小于DATACOUNT；拐點(diǎn)之下由近端到拐點(diǎn)呈遞減趨勢(shì)（且比較陡），之上呈遞增趨勢(shì)（且比較緩）.
  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;//比較后使用最靠近車(chē)的行
  355.      else if (zzxqishi<yzxqishi&&(yzxqishi<=47))   zxqishi=yzxqishi;
  356.      else if(zzxqishi==0&&yzxqishi!=0) zxqishi=yzxqishi;//一端沒(méi)有，用另一端的作為最后的拐點(diǎn)
  357.      else if(zzxqishi!=0&&yzxqishi==0) zxqishi=zzxqishi;
  358.      else zxqishi=0;//兩端都沒(méi)有，即車(chē)體進(jìn)入十字中，此時(shí)沒(méi)有最下端的拐點(diǎn)
  359.     // yupan_fangxiang(zxqishi);//主要為下面服務(wù)的  預(yù)判函數(shù)
  360.    }
  361.    if((youxiao_shizi-shizi_flag)>=1&&(youxiao_shizi-shizi_flag)<=25)
  362.    {//由于存在十字的原因，之前搜到的邊線(xiàn)可能不準(zhǔn)，需要重新搜線(xiàn)，需要確定搜線(xià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;)//縱向搜線(xiàn)，尋找最遠(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--)//左線(xiàn)是110，且列大于左下端點(diǎn)的列坐標(biāo)或 其根本沒(méi)有下端點(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)找到黑線(xiàn)  
  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.        {//由于賽道不是直線(xiàn)，每5行更新一次left_sz，作為下一次搜線(xiàn)的起始列
  401.          kk=0;
  402.          left_sz=blackleft[ig+1]+bxiankuan[ig+1];//根據(jù)上一行的左線(xiàn)虛擬出下一次的起始列
  403.        }

  405.        if(right_sz<blackleft[ig])   right_sz=blackleft[ig];//越界限制
  406.        for(int mg=right_sz;mg<=DATACOUNT-1;mg++)//右線(xiàn)是011，且列小于右下端點(diǎn)的列坐標(biāo)或 其根本沒(méi)有下端點(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)找到黑線(xiàn)
  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.          {//由于賽道不是直線(xiàn)，每5行更新一次right_sz，作為下一次搜線(xiàn)的起始列
  416.            jj=0;
  417.            right_sz=blackright[ig+1]-bxiankuan[ig+1];//根據(jù)上一行的右線(xiàn)虛擬出下一次的起始列
  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)，周?chē)鷰讉€(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)來(lái)�。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。�！     
  439.      //下端拐點(diǎn)在十字開(kāi)始行的下面，且（左線(xiàn)在中線(xiàn)以左   或              下端拐點(diǎn)處的右線(xiàn)在中線(xià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)來(lái)
  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)行連線(xià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ù)連線(xiàn)
  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)行連線(xià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.         //為了下面求中線(xiàn)的起始行和結(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ǔ)線(xiàn)了，此處只需求中值即可
  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)//中線(xiàn)靠左線(xiàn)加賽道寬度而得
  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)//中線(xiàn)靠右線(xiàn)減賽道寬度而得
  480.              center[i]=(blackright[i]-bxiankuan[i]);
  481.         }
  482.      }
  483. //近端處理  無(wú)下拐點(diǎn)的進(jìn)來(lái)�。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。。�
  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.     {     //十字比較靠近車(chē)                     且兩個(gè)上拐點(diǎn)都存在，es與xs在合理的范圍內(nèi)                              
  486.       for(int iz=xs-1;iz<DATALINE-1;iz++)//此時(shí)補(bǔ)線(xiàn)利用左拐點(diǎn)與左下角坐標(biāo)值連線(xiàn)，右拐點(diǎn)與右下角坐標(biāo)值連線(xiàn)
  487.       {//一次函數(shù)連線(xiàn)
  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)，作為下面求中線(xiàn)的起始行
  497.       else          szq=xs;
  498.       for(i=szq;i<=49;i++)
  499.         center[i]=(blackright[i]+blackleft[i])/2;
  500.     }
  501.     //十字后期處理  主要處理一些不常見(jiàn)的情況，可不看
  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)//左線(xiàn)丟失
  507.         {
  508.           if(center[i-1]!=0)//給下端補(bǔ)線(xiàn)
  509.             center[i]=blackright[i]-(blackright[i-1]-center[i-1]+1);
  510.           else if(center[i+1]!=0)//給上端補(bǔ)線(xiàn)
  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)//右線(xiàn)丟失
  516.         {
  517.           if(center[i-1]!=0)//給下端補(bǔ)線(xiàn)
  518.             center[i]=blackleft[i]+(center[i-1]-blackleft[i-1]+1);
  519.           else if(center[i+1]!=0)//給上端補(bǔ)線(xiàn)
  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ǔ)線(xiàn)     各標(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ù)兩行的左線(xiàn)都存在  下增上減
  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ù)三行左端都沒(méi)找到線(xiàn)
  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ù)兩行沒(méi)找到線(xiàn)，且從下到上是白到黑或左線(xiàn)找到了
  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ǔ)線(xiàn) 左側(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ù)判得出虛擬中線(xiàn)，
  561.         for(int ig=zuoqishi;ig>0;ig--)//重新搜線(xiàn)
  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)找到黑線(xiàn)
  567.              {
  568.                 blackleft[ig]=jg;  //更新左線(xiàn)數(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ǔ)線(xiàn) 再判斷 由于前面更新了左線(xiàn)數(shù)組，此時(shí)再次確認(rèn)無(wú)誤
  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ǔ)線(xiàn)，同正十字
  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è)沒(méi)搜到線(xiàn)的行的中線(xiàn)根據(jù)左側(cè)補(bǔ)出來(lái)
  616.          {
  617.            if(blackleft[iz]>=1&&blackright[iz]>=(DATACOUNT-1))
  618.              center[iz]=blackleft[iz]+bxiankuan[iz];//
  619.          }
  620.        }
  621.      }
  622. //右側(cè) 斜十字補(bǔ)線(xiàn)/////
  623.      for(int is=DATALINE-3;is>=5;is--)//斜十字補(bǔ)線(xiàn)   右側(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)//開(kāi)始記錄白點(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ù)得出的虛擬中線(xiàn)，來(lái)確定再次搜線(xiàn)的起始列
  651.         for(int ig=youqishi;ig>=0;ig--)//對(duì)右線(xiàn)數(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)找到黑線(xiàn)
  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ǔ)線(xiàn)   由于更新了數(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ǔ)線(xiàn)
  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ǔ)完線(xiàn)并求中心值
  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ì)尋到的邊情況分類(lèi)并處理擬合中線(xiàn)////////
  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è)丟線(xiàn)
  728.        {
  729.          if(yici_you==0)//右側(cè)開(kāi)始丟線(xiàn)處
  730.            yici_you=i;//記錄起始行
  731.          if(youxiaoyou==0)
  732.          {
  733.            for(int bs=i;bs>2;bs--)
  734.            {//連續(xù)三行的右線(xiàn)為都有值   或連續(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--)//判斷左線(xiàn)連續(xù)性，并確定不連續(xù)行
  746.            {
  747.              if((blackleft[bs]-blackleft[bs-1])>0||blackleft[bs]>=(DATACOUNT-1)||blackleft[bs-1]>=(DATACOUNT-1))
  748.              {//本來(lái)左線(xiàn)是遞增向右的，當(dāng)遇到左線(xiàn)遞減了或跑到右邊沿了就進(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))//如果靠近車(chē)，應(yīng)該是近端丟線(xiàn)，先判斷下
  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í)是近端丟線(xiàn)  此值應(yīng)離丟線(xiàn)處不遠(yuǎn)
  766.                  {
  767.                    for(int as=youxiaoyou;as<=49;as++)//能進(jìn)來(lái)，可以確定確實(shí)近端丟線(xiàn)了
  768.                    {
  769.                       if(center[as]==0)
  770.                       {
  771.                         jinduan_ybx=1;//置位標(biāo)志  //根據(jù)前一行的中線(xiàn)求出該行的中線(xiàn)
  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ǔ)線(xiàn)完成跳出程序
  777.                }
  778.                else break;//若為進(jìn)入急彎，則會(huì)跳出
  779.              }
  780.            }
  781.            if(jinduan_ybx==0&&yici_you>=46&&youxiaoyou<38&&(youxiaozuobian<=yici_you))//如果不是近端丟線(xiàn)，而是進(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)//彎道丟線(xiàn)時(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è)丟線(xiàn)
  796.        {
  797.          if(yici_zuo==0)//開(kāi)始丟線(xiàn)處起始
  798.            yici_zuo=i;
  799.          if(youxiaozuo==0)
  800.          {
  801.            for(int ks=i;ks>2;ks--)//重新遍歷，確定丟線(xiàn)最上行
  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--)//判斷右線(xiàn)連續(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è)一大塊沒(méi)尋到線(xiàn)的地方
  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í)是近端丟線(xiàn)
  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ǔ)線(xiàn)完成跳出程序
  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))//如果不是近端丟線(xiàn)，而是進(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. }

  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ù)上面黑線(xiàn)的中心偏差來(lái)求解有效行
  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;//并且平滑后的中心線(xiàn)的方向一致
  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;//并且平滑后的中心線(xiàn)的方向一致
  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)//有效行離車(chē)太近，可能沖出賽道了
  1028.    {
  1029.      wanqishi=45;//虛擬起始點(diǎn)，判斷是否有噪點(diǎn)影響了有效行的讀取
  1030.      valuerownum=valuerownum-13;
  1031.      panduan_youxiao();//克服平滑濾波帶來(lái)的影響，尋找真正有效點(diǎn)
  1032.      if(wandao_youxiao==1)//如果是彎道
  1033.        valuerownum=46; //這樣，數(shù)據(jù)就不會(huì)丟棄了，就會(huì)進(jìn)入下一個(gè)if語(yǔ)句里
  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();//克服平滑濾波帶來(lái)的影響，尋找真正有效點(diǎn)
  1048.         if(wandao_youxiao==1)
  1049.           valuerownum=value;//有效的話(huà)把實(shí)際的有效行求出
  1050.         else
  1051.           valuerownum=valuerownum+10;//無(wú)效的話(huà)，還原原值
  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)//有效行靠前，離車(chē)較遠(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)//有效行離車(chē)更遠(yuǎn)了，說(shuō)明是直道或小s
  1081.      {   
  1082.         zhidao=1;
  1083.         wandao=0;
  1084.      }
  1085.      for(int i=47;i>(valuerownum+1);i--)//五點(diǎn)平滑濾波，使中線(xià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//無(wú)效時(shí)保持原來(lái)的數(shù)據(jù)
  1095.    {      
  1096.       CenterEq=lasterror;
  1097.       lasterror=CenterEq;
  1098.       badframe=0;
  1099.    }
  1100.    if(CenterEq>70)//對(duì)加權(quán)平均進(jìn)行限幅，也可不限，數(shù)值根據(jù)自己的小車(chē)定，
  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. }

  1114. /***********************************************************
  1115. 函數(shù)名稱(chēng)：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. ……………………

  1157. …………限于本文篇幅 余下代碼請(qǐng)從51黑下載附件…………
復(fù)制代碼

所有資料51hei提供下載:
國(guó)賽攝像頭程序.rar (12.99 MB, 下載次數(shù): 133)


分享到:  QQ好友和群QQ好友和群 QQ空間QQ空間 騰訊微博騰訊微博 騰訊朋友騰訊朋友
收藏收藏3 分享淘帖 頂頂1 踩踩
回復(fù)

使用道具 舉報(bào)

沙發(fā)
ID:702441 發(fā)表于 2020-3-6 14:40 | 只看該作者
你好，我下載了你的程序，但是沒(méi)有發(fā)現(xiàn)程序打開(kāi)的地方啊，是keil還是什么軟件
回復(fù)

使用道具 舉報(bào)

本版積分規(guī)則

手機(jī)版|小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術(shù)交流QQ群281945664

Powered by 單片機(jī)教程網(wǎng)

快速回復(fù) 返回頂部 返回列表