设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 电子技术分类讨论区 › 资料共享 › 飞思卡尔智能车国赛CCD摄像头组源程序
返回列表 发新帖
查看: 5931|回复: 1
打印 上一主题 下一主题
收起左侧

飞思卡尔智能车国赛CCD摄像头组源程序

[复制链接]
跳转到指定楼层
楼主
ID:287405 发表于 2018-3-4 12:58 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
飞思卡尔国赛摄像头组程序

单片机源程序如下:
  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. }

复制代码
  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. }

  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.        }

  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. }

  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. }

  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. ……………………

  1157. …………限于本文篇幅 余下代码请从51黑下载附件…………
复制代码

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


分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏3 分享淘帖 顶顶1 踩踩
回复

使用道具 举报

沙发
ID:702441 发表于 2020-3-6 14:40 | 只看该作者
你好，我下载了你的程序，但是没有发现程序打开的地方啊，是keil还是什么软件
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表