恩智浦智能车赛。摄像头处理函数。

这套摄像头处理算法是可以直接使用的。只需要重写一下void servo()//舵机控制函数。
输入的参数是**extern uint8 image_dec[60][80];**数组中保存的摄像头传过来二值化后的数据。
输出是距离赛道的偏差 保存在整形变量：duojijiaodu 中。把这个变量传入舵机打脚的pid算法中即可。
讲道理这样基本就可以用了。如果想跑的更顺畅，更快的话需要调整最下面代码中的

 my_piancha=(int)(mzd2*currentzhongjian_lk - mzd1*B);//0.95,0.32//偏差合成，控制转弯，1.1倍的点+0,9倍的斜率构成总的偏差

通过修改两个参数控制曲率和偏差的融合来实现。
注意：摄像头循迹不会走赛道中线，尤其在拐弯时。比如再拐一个左弯时，在入弯时车模会贴赛道右侧，出弯时会贴赛道左侧。这样可以保证高速过弯且不会冲出赛道。
这些代码是我之前上传文件的一部分。摄像头四轮

使用时，需要依次调用函数：
//摄像头转向控制
lkcongzhongjiansaomiao();
pianchachuli();
servo();

/*
摄像头循迹程序
我们这一届摄像头20CM高度，没法发挥出摄像头的优势，内切一直压线，
这个程序在30CM高度的摄像头中可以跑的3M/s


数组   static int half_width_group【】中保存的是赛道宽的一半，可以在实际中测量然后修改这个数组中的数据。

最终偏差是由中线的差值和曲率融合。pianchachuli（）函数中my_piancha=(int)(mzd2*currentzhongjian_lk - mzd1*B);
这个表达式完成融合，可以通过调整两个参数的大小让小车转弯更顺滑，不压线

servo函数中是控制舵机打脚，其中16.5/70是打角的系数，再乘以上面的my_piancha变量就是实际舵机需要打的角度

*/
#include "saomiao.h"

#include "headfile.h" //白1 黑0

#include "math.h"
int qianzhan=29;
int my_piancha,my_lastzhongjian=40;
int jiao, x,y,linshi_x,linshi_right_heixian,linshi_y,ruyuanhuan_flag=0;
extern uint8 image_dec[60][80];
 int [60],right_heixian[60],left_heixian[60],zhidao_flag;
int lastpiancha_1,lastpiancha_2,lastpiancha_3,lastpiancha_4,duojijiaodu,flag_l=0,flag_r=0,linshi_left_heixian,B,shaobudaozuo_flag[60],shaobudaoyou_flag[60];
int shizidiyidian,shangshizidiyidian,S_Z;
int shizishu,chazhi,yy1,yy2,rushizi;
int duandian,duandianshu,duandianshu1,currentzhongjian_lk;
int xielv_flag;
int qulv_point=0,zhangai_right=0,zhangai_left=0;
int dian1,youshi_kuan_flag=0;
char s_wan_flag,shizi_flag,yuanhuan_flag=0,guai_flag=0;
int qvlv_quanju,qulv_jinduan,qulv_yuandaun;
int qulv_jinduan_right=0,qulv_jinduan_left=0,qulv_yuandaun_right=0,qulv_yuandaun_left=0,qvlv_quanju_right=0,qvlv_quanju_left=0;
int half_zhi[60];
int yy1_pinjun,yy2_pinjun;

static int half_width_group[60]=
{
0,0,0,0,0,0,0,0,8,
9,10,11,12,12,13,15,16,17,18,
19,20,21,22,23,24,25,25,26,28,
29,30,30,31,32,33,34,35,35,36,
37,38,38,39,39,40,40,40,40,40,
40,40,40,40,40,40,40,40,40,40      
};
int regression(int startline,int endline)   //线性回归方程计算斜率      
{
  if(endline>56)
    endline=56;
  int i;
  int sumX=0,sumY=0,avrX=0,avrY=0 ;
   int num=0,B_up1=0,B_up2=0,B_up,B_down;
   for(i=startline;i<=endline;i++)
   {
            num++;
            sumX+=i;
            sumY+=currentzhongjian[i];
   }
         avrX=sumX/num;
         avrY=sumY/num;
         B_up=0;
         B_down=0;
         for(i=startline;i<=endline;i++)
        {
         
            B_up1=(int)(currentzhongjian[i]-avrY);
            B_up2=i-avrX;
            B_up+=(int)(10*(B_up1*B_up2));
            B_up=B_up/100*100;
            B_down+=(int)(10*((i-avrX)*(i-avrX)));
         }
   if(B_down==0) 
   B=0;
   else 
   B=B_up*16/B_down;//16
   return B;
}

int hy;
void huaxian(int x1,int y1,int x2,int y2)//将两个点之间连成一条线段
{
  int i,max,a1,a2;
  a1=x1;
  a2=x2;
  if(a1>a2)
  {max=a1;
  a1=a2;
  a2=max;}
for(i=x1;i>0;i--)
{
  if((x2-x1)!=0)
  {
    hy=(i-x1)*(y2-y1)/(x2-x1)+y1;
   currentzhongjian[i]=hy;
   if(hy<=1||hy>=79)
     break;
  }
}
}
void huaxian2(int xxx1,int yyy1,int xxx2,int yyy2)//两点之间的线段
{
  int i2,max2,a12,a22;
  a12=xxx1;
  a22=xxx2;
  if(a12>a22)
  {
    max2=a12;
  a12=a22;
  a22=max2;}
for(i2=xxx1;i2<59;i2++)
{
  if((xxx2-xxx1)!=0)
  {
    hy=(i2-xxx1)*(yyy2-yyy1)/(xxx2-xxx1)+yyy1;
   currentzhongjian[i2]=hy;
  }
}
}

//*****************************************************舵机控制函数**********************************************************//
void servo()//舵机控制函数
{
jiao= Servo_Center-(int)((duojijiaodu*16.5/70));//舵机的pid算法公式，没有小数，所以要除以70来实现小数
if(jiao>=Servo_Left)
jiao=Servo_Left;
if(jiao<=Servo_Right)
jiao=Servo_Right;
servo_PWM(jiao);
}
//***************************************************扫描函数***************************************************************//
void lkcongzhongjiansaomiao()
{
  int kuan_flag=0,lo=0,s_point=0,S_COUNT=0,fuduandian=0,qulv_yuanhuan=0,chushizi_flag=0;
  memset(shaobudaozuo_flag,0,sizeof(shaobudaozuo_flag));//清零函数
  memset(shaobudaoyou_flag,0,sizeof(shaobudaoyou_flag));
  int zhangai_flag=0,qulv_s_y=0,qulv_s_l=0;
  shangshizidiyidian=0;
  left_heixian[59]=0;
  right_heixian[59]=79; 
  shizishu=0;
  rushizi=0;
  xielv_flag=0;
  shizi_flag=0;
  my_lastzhongjian=39;
  if(currentzhongjian[58])
   currentzhongjian[59]=my_lastzhongjian=currentzhongjian[58];
 else
   currentzhongjian[59]=39;
   for(y=58;y>=0;y--)//扫描完58，整副图像处理完毕
   {
     for(x=my_lastzhongjian;x<=79;x++)//从中间向右扫描
     {
       if(image_dec[y][x]==0)
         {
           
         right_heixian[y]=x;
         shaobudaoyou_flag[y]=1; 
         break;
         }
     }
     for(x=my_lastzhongjian;x>=0;x--)//向左扫描
     { 
       if(image_dec[y][x]==0)
       {
         left_heixian[y]=x;
         shaobudaozuo_flag[y]=1;
         break;
       }  
     }
    if(fuduandian==0)
    {
    if(y>48)//丢边补线，加整个赛道补线
      {
        if(shaobudaoyou_flag[y]==0&&shaobudaozuo_flag[y]==1)//扫不到右
         { 
          right_heixian[y]=left_heixian[y]+2*half_width_group[y];//y为数组中的x值
         }
        else if(shaobudaoyou_flag[y]==1&&shaobudaozuo_flag[y]==0)//扫不到左
         {
           left_heixian[y]=right_heixian[y]-2*half_width_group[y];
         }
       else if(shaobudaoyou_flag[y]==0&&shaobudaozuo_flag[y]==0)//都扫不到
         {
          left_heixian[y]=0;//如果两边都扫不到，直接从中间提取中线
          right_heixian[y]=79;
         }
     }
     else if(y<=48)//更远的补线，
     {
       if(shaobudaoyou_flag[y]==0&&shaobudaozuo_flag[y]==1)//扫不到右
         { 
     
          right_heixian[y]=right_heixian[y+1]+(abs)(left_heixian[y]-left_heixian[y+1])-1;//根据左边这一点与上一点，
                                                                                        //（y+1）为数组中上一次的x坐标，
                                                           //left_heixian[y]-left_heixian[y+1]算左边偏移量，减一为后期补偿
         }
       else if(shaobudaoyou_flag[y]==1&&shaobudaozuo_flag[y]==0)//扫不到左
         {
       
           left_heixian[y]=left_heixian[y+1]-(abs)(right_heixian[y+1]-right_heixian[y])+1;
         }
         
       else if(shaobudaoyou_flag[y]==0&&shaobudaozuo_flag[y]==0)//都扫不到
         {
          left_heixian[y]=0;
          right_heixian[y]=79;
         }
     }
     //*************************************中线处理*********************************************************//
   
    currentzhongjian[y]=(int)((right_heixian[y]+left_heixian[y])/2);  //提取中间线（左边加右边比2）
  
    half_zhi[y]=(int)((right_heixian[y]-left_heixian[y])/2);  //一半赛道值（没用，不改）
    
      //*******************************************************中线滤波防止中线出现毛刺**************************************************/
  
if(currentzhongjian[y]-currentzhongjian[y+1]>4&&y<42&&B<0)
currentzhongjian[y]=currentzhongjian[y+1]+1;
if(currentzhongjian[y]-currentzhongjian[y+1]<-4&&y<42&&B>0)
currentzhongjian[y]=currentzhongjian[y+1]-1;  


/******************************************************************曲率计算**************************************************************/
if(y>30)//曲率近端判断
{
  if((currentzhongjian[y]-currentzhongjian[y+1])>0)
qulv_jinduan_right++;
else
if((currentzhongjian[y]-currentzhongjian[y+1])<0)
qulv_jinduan_left++;
}
if(y<25&&y>=7)//曲率远端判断，远处的一点就不要了
{
  if((currentzhongjian[y]-currentzhongjian[y+1])>0)
qulv_yuandaun_right++;
else if((currentzhongjian[y]-currentzhongjian[y+1])<0)
qulv_yuandaun_left++;
}
if(y<=55&&y>15)//曲率全局判断
{
  if((currentzhongjian[y]-currentzhongjian[y+1])>0)
qvlv_quanju_right++;
else
if((currentzhongjian[y]-currentzhongjian[y+1])<0)
qvlv_quanju_left++;

}

if(currentzhongjian[y]>79)//中线的限制幅度
currentzhongjian[y]=79;

if(currentzhongjian[y]<0)
currentzhongjian[y]=0;

my_lastzhongjian=currentzhongjian[y];//保存中间点坐标
if(y<56)//小s位置判断，小s弯道作直线冲刺
{
if((currentzhongjian[y]-currentzhongjian[y+2])>0)
qulv_s_y++;
else
if((currentzhongjian[y]-currentzhongjian[y+2])<0)
qulv_s_l++;
if(y>36&&abs(currentzhongjian[y]-currentzhongjian[y+2])>0)
qulv_yuanhuan++;
//printf("y:%d\n",qulv_s_y);
//printf("l:%d\n",qulv_s_l);
}
if(y<58&&y>=10)
S_COUNT+=currentzhongjian[y];
// *******************************************小S断点搜索*********************************************//
 if(y>8&&(abs)(right_heixian[y]-currentzhongjian[y])<5||(abs)(left_heixian[y]-currentzhongjian[y])<5)//小S断点判定
  s_point=y;//
else
s_point=0;

  // *******************************************防止扫到跑道外*********************************************//
  if(y<36&&((abs)(right_heixian[y]-currentzhongjian[y])<1||(abs)(left_heixian[y]-currentzhongjian[y])<1))//防止扫到跑道外面
{
  duandian=y;
  fuduandian=y;
  if(y>15)
  {
   duandianshu=y-15;
   if(duandianshu>35)
     duandianshu=35;
  }

 if(y>qianzhan)
  {
     duandianshu1=y-qianzhan;
     if(duandianshu1>25)
       duandianshu1=25;
  }
}
else 
{
  duandian=0;
  duandianshu=0;
  duandianshu1=0; 
}
    }
     }
 if(right_heixian[y]-left_heixian[y]<half_width_group[y]&&right_heixian[y+1]-left_heixian[y+1]<half_width_group[y+1])//防止扫到跑道外面
       break;
//}
//********************************************************y行结束标志*******************************************************

S_Z=S_COUNT/(48);

//********************************************************************曲率计算*******************************************************************
   qvlv_quanju=abs(qvlv_quanju_right-qvlv_quanju_left);//曲率全局
   qulv_jinduan=abs(qulv_jinduan_right-qulv_jinduan_left);//曲率近端
   qulv_yuandaun=abs(qulv_yuandaun_right-qulv_yuandaun_left);//曲率远端
   qvlv_quanju_right=qvlv_quanju_left=qulv_jinduan_right=qulv_jinduan_left=qulv_yuandaun_right=qulv_yuandaun_left=0;


// *******************************************************十字处理*********************************************************//

  if(duandian<16) // *****************************十字
  {
   for(int i=58;i>7;i--)
   { 
     if(i>45&&(shaobudaozuo_flag[i]==1||shaobudaoyou_flag[i]==1))
     rushizi++;
     if((i>9&&i<=40)&&shaobudaozuo_flag[i]==0&&shaobudaoyou_flag[i]==0)
       shizishu++;
     if(i<45&&(shaobudaozuo_flag[i]==1||shaobudaoyou_flag[i]==1)&&shangshizidiyidian==0)
       shangshizidiyidian=i;
   }
   if(shizishu>2&&rushizi>10)//刚入十字
   {
     shizi_flag=1;
     //gpio_set(PTE1,1);
     xielv_flag=1;
    for(int i=49;i>0;i--)
    {      
     if(shaobudaozuo_flag[i]==0&&shaobudaoyou_flag[i]==0)
      {
        shizidiyidian=i;
        break;
      }     
     }
     if(shizidiyidian<34)
       {
      yy1=shizidiyidian+19;
      yy2=shizidiyidian+24;
    }
    else 
    if(shizidiyidian<40)
       {
      yy1=shizidiyidian+13;
      yy2=shizidiyidian+18;
    }
    else if(shizidiyidian<45)
    {
      yy1=shizidiyidian+9;
      yy2=shizidiyidian+13;
    }
    else if(shizidiyidian<49)
    {
    yy1=shizidiyidian+6;
    yy2=shizidiyidian+10;
    }else
      if(shizidiyidian<55)
    {
    yy1=shizidiyidian+2;
    yy2=shizidiyidian+4;
    }
    yy1_pinjun=(currentzhongjian[yy1]+currentzhongjian[yy1+1]+currentzhongjian[yy1-1])/3;
    yy2_pinjun=(currentzhongjian[yy2]+currentzhongjian[yy2+1]+currentzhongjian[yy2-1])/3;
    //if(!ruyuanhuan_flag)
    huaxian(yy1,yy1_pinjun,yy2,yy2_pinjun); 
      
    //huaxian(yy1,currentzhongjian[yy1],yy2,currentzhongjian[yy2]); 
    }
else if(shizishu>4&&rushizi<=10&&shangshizidiyidian>=20)//出十字
  {
    chushizi_flag=1;
  // gpio_set(PTE1,1);
    xielv_flag=1;
    shizi_flag=1;
 //if(!ruyuanhuan_flag)
   huaxian2(shangshizidiyidian-6,currentzhongjian[shangshizidiyidian-6],58,currentzhongjian[58]);

  }
       //  gpio_set(PTE1,0);

  }
  //********************************************************圆环检测*******************************************************
if(shizi_flag&&qulv_yuanhuan<=10&&qulv_jinduan<7&&!chushizi_flag)
{
  
 signed char kkk=0,black_num=0;
  for( kkk=(int)(currentzhongjian[58]+currentzhongjian[57]+currentzhongjian[56])/3;kkk<=79;kkk++)//向右扫描黑块点数
  {
    if(!image_dec[8][kkk]||!image_dec[10][kkk])
    {
    black_num++;
   
    }
    else
      break;
    
  }
   for( kkk=(int)(currentzhongjian[58]+currentzhongjian[57]+currentzhongjian[56])/3;kkk>=0;kkk--)//向左扫描黑块点数
   {
      if(!image_dec[8][kkk]||!image_dec[10][kkk])
    {
    black_num++;
   
    }
    else
      break;
   }
  
  if(black_num>36)//检测到圆环
  {
    yuanhuan_flag++;
    //gpio_set(PTC18,0);//点亮led提示
  }
  else
  {
    yuanhuan_flag=0;
    //gpio_set(PTC18,1);关闭led提示
  }
 
}
else
{
yuanhuan_flag=0;
 //gpio_set(PTC18,1);//关闭led提示
}
  
if(yuanhuan_flag>3)//连续检测到圆环三次以上
{
guai_flag=1;
//gpio_set(PTA14,1);
}

// ***********************************************************小S弯判定*小S弯判定*****************************************************//
if(s_point==0&&qulv_s_y>6&&qulv_s_l>6&&zhidao_flag<20&&!shizi_flag)
{
  s_wan_flag=1;
  //led(LED0, LED_ON);
}
else
{
s_wan_flag=0;
//led(LED0, LED_OFF);
}
 }
//************************************************图像处理结束,下面是偏差处理************************************************/

void pianchachuli()//偏差处理
{
    if(xielv_flag==0)
    regression(15+duandianshu,43+duandianshu);///斜率计算
  dian1=abs((currentzhongjian[52]+currentzhongjian[53]+currentzhongjian[54])/3-39);
//lins=currentzhongjian[30];
currentzhongjian_lk=(currentzhongjian[qianzhan+duandianshu1]-39);//点

if(B>19)
B=19;
if(B<-19)
B=-19;

float mzd1=0.11;  //曲率与中线合成的权值
float mzd2=0.75;
my_piancha=(int)(mzd2*currentzhongjian_lk - mzd1*B);//0.95,0.32//偏差合成，控制转弯，1.1倍的点+0,9倍的斜率构成总的偏差

if(!ruyuanhuan_flag&&ruyuanhuan_flag<10)//拐50ms
{
  ruyuanhuan_flag++;
if(my_piancha<-4&&shizi_flag)//右边进的圆环
my_piancha+=13;
else
if(my_piancha>4&&shizi_flag)//左边进的圆环
my_piancha-=13;
}
else
ruyuanhuan_flag=0;


lastpiancha_4=lastpiancha_3;
lastpiancha_3=lastpiancha_2;
lastpiancha_2=lastpiancha_1;
lastpiancha_1=duojijiaodu;
duojijiaodu=my_piancha; 


}