ccd.c

第三届飞思卡尔智能车竞赛华东赛一等奖的程序

#include "CCD.h"
#include "utile.h"
#include <mc9s12dg128.h>

#define BOTTOM_MIN 85
#define TOP_MIN 115
#define BLACK 150         //大于140
#define WHITE 180 
#define VALVE 45     
#define MAX 10

#define k_warp 686

int AVR=0,path[LINE],path_record[LINE],line_index,line_cnt,img_cnt,cnt,head,head_record,half,tail,tail_record,s_mid,RT_angle,rec_cnt,angle_record;
int warp,length,true_length,length_record,warp_record,startpoint_count,Flag_startpoint,img_cnt1,cross_start_cnt,fhead;
char img1[DOT*LINE],delta[MAX],img1_ready,img2_ready,img_type;
char state,StartPointDectected,CrossDectected;
extern int  RT_speed,I_dest_speed,k2,kl;
int warp_cm,warp_center,length_cm;

//img2[DOT*LINE],##################################################################### CCD初始化
void CCD_Init(void)
{
   
  ATD1CTL2=0xC0;      //快速清除
  ATD1CTL3=0x08;      //转换序列长度1
  ATD1CTL4=0x80;      //8位AD转换,采样时间为2个时钟周期
  ATD1CTL5=0xA7;      //右对齐,无符数据连续采样
  
  DDRE  &=~0x02;      //PE1 as input,falling edge triger,interrupt enable
  INTCR &=~0x40;
  
  TIOS  &=~0x01;
  TCTL4 &=~0x01;       //capture on falling edge
  TCTL4 |=0x02;
  TFLG1 |=0x01;
  TIE   |=0x01;
  
  line_index=line_cnt=0;
  img1_ready=img2_ready=0;
  img_cnt=0;
  cnt=0;
  
  angle_record=0;
  warp_record=0;
 
  head=0;
  head_record=0;
  tail=LINE-1;
  tail_record=LINE-1;
  path[head]=path[tail]=0;
  path_record[head_record]=path_record[tail_record]=0;
  state='L';
  startpoint_count=0;
  cross_start_cnt=0;
  Flag_startpoint=0;
  img_cnt1=0;
  fhead=10;
}
//##################################################################### CCD初始化

void CCD_exit(void)
{
  ATD1CTL2 =0x00;
  INTCR   &=~0x40;
  TIE     &=~0x01;
}


//##################################################################### CCD场中断
interrupt void CCD_field_ISR(void)
{
  if(img1_ready==0) 
     img_type=1;
 
  else 
  {
    TFLG1|=0x01;
    return;
  }
  
  //###############################################################
 if(Flag_startpoint==1) 
 {
    img_cnt1++;
    
    if(img_cnt1>=80) 
    {
      Flag_startpoint=0;
     
      img_cnt1=0; 
    }            
 } 
 //###############################################################  
  
  INTCR|=0x40;
  TFLG1|=0x01;
}
//##################################################################### CCD场中断


//#####################################################################黑线提取算法
interrupt void CCD_line_ISR(void)
{
  int i,valid_index;
  unsigned int t;
  
  t=TCNT;
  i=line_cnt/7;
  valid_index=i*(3*i+12)+(i+2)*(line_cnt%7);

  
  if(line_index-DROP==valid_index)
  {
    if(img_type==1)
    {
         // while(TCNT-t<53);
         for(i=545;i>0;i--);
          for(i=0;i<DOT;i++)//13个指令周期，95列
          {
            img1[cnt++]=ATD1DR0L;
            asm("NOP"); 
            
            
            
          }
    }
   
    if(++line_cnt>=LINE)
    {
      INTCR&=~0x40;
      line_cnt=line_index=cnt=0;
      img_cnt++;
      if(img_type==1) img1_ready=1;
        return;
    }
  }
  line_index++;
}

//#############################################################################

int black_search1(int x)                                         //对连续的5行进行黑点搜索（针对img1)
{
    int black_cnt,j;
    black_cnt=0;
    if (img1[x]<=35) 
       black_cnt++;
    for(j=1;j<=4;j++) 
    {
       if(img1[x-j*DOT]<=35||img1[x+j*DOT]<=35) 
       {
          black_cnt++;
       } 
       else
          break;
       
    }
    
    if(black_cnt>0)
        return 1;
    else 
        return 0;
    
}




//#############################################################################



int black_line_center1(void)
{
  int i,j,t,tmp,count,min_index,min,p,q,left_black_index,right_black_index;
  StartPointDectected=0;
  CrossDectected=0;
  
  for(i=LINE-1;i>0;i--)
  {
    min=1000;
    count=0;
    
    for(j=1;j<DOT-1;j++)
    {
      t=DOT*i+j;
      tmp=img1[t]+img1[t-DOT]+img1[t-2*DOT];
           
      if(min>tmp)
      {
        min=tmp;
        min_index=j;
        count=0;
      }
      else if(min==tmp) 
        count++;
    }
    
    if(min<BOTTOM_MIN) 
        break;
  }
  
  min_index+=count>>1;
  
  if(i>=2*LINE/3)
  {
    
     tail=i;
     tail_record=i; 
    
     
  }
  
  else 
  return 0;
  
  
  path[tail]=min_index;
  path_record[tail_record]=path[tail];
  
  for(i--;i>0;i--) 
  {
    p=min_index-3;
    q=min_index+3;
    
    if(min_index<3) 
       p=0;
    if(min_index>=DOT-3) 
       q=DOT-1;
    
    t=DOT*i+min_index;
    min=img1[t]+img1[t+DOT]+img1[t-DOT]-5;
    count=0;
    
    for(j=p;j<=q;j++)
    {
      t=DOT*i+j;
      tmp=img1[t]+img1[t+DOT]+img1[t-DOT];
          if(min>tmp)
      {
        min=tmp;
        min_index=j;
        count=0;
      }
      else if(min==tmp) count++;
    }
    
    if(min<TOP_MIN+15-i/3) 
       path[i]=min_index+(count>>1);
    else 
       break;
    
    if(path[i]<=3*DOT/4 && path[i]>=DOT/4&&i>=9&&i<=31)
    {
      
      t=DOT*i+path[i];
      if(img1[t-14]<=35) 
       {
          left_black_index=t-14; 
          right_black_index=t+14;
          
          if(black_search1(left_black_index)&&black_search1(right_black_index)&&black_search1(left_black_index-1)&&black_search1(right_black_index+1))
          {
             left_black_index=DOT*i+2;
             right_black_index=DOT*(i+1)-2;
             
             if(((!black_search1(left_black_index)) && (!black_search1(left_black_index-1)))||((!black_search1(right_black_index))&&(!black_search1(right_black_index+1))))
                if(!black_search1(t-4)&&!black_search1(t+4))
                 StartPointDectected=1;
                else
                  CrossDectected=1;
          } 
          else
             continue;   
       }
    }
  }
  
  head=i;
  if(head>=0&&tail>0)
  head_record=head;
  else 
    head_record=head_record;
  path[head]=2*path[head+1]-path[head+2];
  path_record[head_record]=path[head];
  return tail-head+1;
}


//#####################################################################黑线提取算法


void CCD_process(void)               //图象处理程序
{
  int i,avr,offset,k1;
       
  if(img1_ready)
  {
    length=black_line_center1();
    true_length=84*length/26;
    if(length>0)length_record=length;
    else length_record=length_record;    //提取黑线
    img1_ready=0;                    //释放图象存储空间
  }
 
  else 
    return;                      //两幅图象都未采集完，异常，返回

 if(StartPointDectected&&CrossDectected)
     StartPointDectected=0;
 
 if(StartPointDectected&&(Flag_startpoint==0)) 
  {
     startpoint_count++;
     
     if((startpoint_count>=1)&&(Flag_startpoint==0)) 
     {
        Flag_startpoint=1; 
        cross_start_cnt++;
        PTM=cross_start_cnt;
      
     }
  } 
  else
     startpoint_count=0;

//起点、十字处理
  if(StartPointDectected || CrossDectected) 
  {                 //遇到起点或十字
    RT_angle=0;     //转角不变
    state='L';
   
  } 
//看不见处理
  else if(length==0)    //黑线长度为0
  { 
      
     if(warp_record>0)     //右转，则右转最大
       RT_angle=RT_angle+5/2;
     else if(warp_record<0)
      RT_angle=RT_angle-5/2;
     else
     RT_angle=RT_angle; 
     state='O';  
     PORTB=0x00; 


  } 
    
    
  
//L、S、I处理
  else
  {
          
  //线性放大
     for(i=head;i<=tail;i++) {
        
        path[i]=(path[i]-DOT/2)*(3*LINE-2*i)/LINE;   //path[i]=(path[i]-DOT/2)*(5*LINE-3*i)/(2*LINE); 
        path_record[i]=path[i];
        
       
       
     }
                                                                      //并且path数组成为与视场中心偏差的数据
     warp=path[head]-path[tail];       //头部中心与尾部中心的差值     //14 测试：各个赛道warp的值
     warp_center=path[head];
     
           
  if(state=='L' && warp<VALVE && warp>-VALVE&&length>=LINE-3)
          state='L';
  else if(state=='L' && (warp>=VALVE || warp<=-VALVE|| length<=35)) 
  {       
         state='I'; 
         
          
  }
  else if(state=='I' && ((warp>=VALVE || warp<=-VALVE) && length>30 || length<=35) ) 
          state='I';
  else if(state=='I' && warp<VALVE && warp>-VALVE && length>=LINE-3) 
         state='L';
  else if(state=='O' && warp<VALVE && warp>-VALVE&& length>=LINE-3) 
          state='L';     //O TO L  
  else if(state=='O'&& (warp>=VALVE || warp<=-VALVE|| length<=35))  
         state='I';   //O TO I
       

        
    
   
   
        if(state=='L')
        { 
        
           avr=0;
           for(i=head;i<=tail;i++) 
              avr+=path[i];
              avr/=length;           //avr为平均偏差
       
           RT_angle=4*warp/length+1*avr/6;      //L 转角设置
           PORTB=0xAA;                          //L 指示灯全灭
        }
        
        
        else if(state=='I') 
        {                                       
          avr=path[tail];                      //平均偏差为头尾偏差
         
          PORTB=0xf0;
          if(warp>47)       offset=2;
          else if(warp<-47) offset=-2;
          else offset=0; 
          
          if(warp<=0 && avr>3 || warp>=0 && avr<-3)          //内线
          {
            k1=k2-1; 
           PORTB=0b01111111;
           
          }
          else if(warp<=0 && avr<=-18 || warp>=0 && avr>=18)  //外线
          {
            k1=k2+3;        
          PORTB=0b00011111;
          }
          else                                          //中线
          {
            k1=k2;          
           PORTB=0b00111111;
          }
          
          half=(3*head+tail)*1/4;
         
         
       RT_angle=k1*(path[half]-path[tail])/length+2*(avr-offset)/7;                                                                         
    }
  warp_record=warp;  
}



//RT_angle取值离散化，公差为5     OK
  if(RT_angle>37) RT_angle=40;
  else  if(RT_angle<-37) RT_angle=-40;
//  if(RT_angle>-4&&RT_angle<4)  RT_angle=0;


    
  angle_record=RT_angle;   //转角存储
  rotate(-RT_angle);        //RT_angle 为 degree

}