main.c

基于摄像头的巡线小车的源代码

/*********************************************************\

projectname:smartcar 

verson:0.9

copyright(c)2009 by Jim

date:2009.3.29

\*********************************************************/
#include <hidef.h>      /* common defines and macros */
#include <MC9S12XS128.h>     /* derivative information */
#include "smartcar.h"
#pragma LINK_INFO DERIVATIVE "mc9s12xs128"  





/*********************************************************\

define functions

\*********************************************************/


#pragma CODE_SEG  __NEAR_SEG NON_BANKED
/***************************************************/


/*******************sci interrupt************************/
/*void interrupt 20 SCI_INT(void) { 

  register data;
  DisableInterrupts;
  
  
  send_image ();
  while(!SCI0SR1_RDRF); 
  data= SCI0DRL;
  
  
   
     
  //EnableInterrupts; 
  
  

} */

/*******************Field sync interrupt************************/
void interrupt 9 Fieldsync(void) {  //CAPTURE ISR、场中断     

  
  last_speed=cur_speed;
  Get_speed();            //speed feedback
  //Speed_feedback_ctl();
                        
  onSamplenum=0;
  abandon=0;
  z=0;
  
  if (PingOrPong==PING)         
    PingOrPong=PONG;		 //END OF ONE FRAME   
  else
    PingOrPong=PING;         
  
  
         
  TIE_C2I=1;   //开行中断
 
  TFLG1_C1F=1; //清场标志位
  
   //accumu++;
  /*if (accumu==6) {
    send_image();  
    PORTB=0x0f;
    Delay(5000);
    PORTB=0xf0;
  }*/
  


}



/*********************Row sync interrupt************************/   
void interrupt 10 Rowsync(void) {    // 行中断
  register byte i;
  register byte *p;
  register byte onSampling=0;  //0 此行不采样，1 此行采样
 

  if(onSamplenum>=231)
  {
    //Sampling=0;
    TIE_C2I=0;  //关行中断
  }
  else if(onSamplenum>=20)
  {
    if(onSamplenum<=90)
    {
      if(onSamplenum%5==0)
      onSampling=1;
    } 
    else if(onSamplenum%7==0)
    onSampling=1; 
  
    
    if(onSampling==1) 
    {
      ATD0CTL5=0X20;
      
      if (PingOrPong==PING)
      {   
        p=&ImagePing[z][0];
      }
      else
      {    
        p=&ImagePong[z][0];
      }
      /*for (i=0;i<col_max;i++) 
      {
        *p++=(PT0AD0_PT0AD01&0b00000010);
        Delay(10);
      }*/ 
      for (i=0;i<col_max;i++) 
      {
        while(ATD0STAT0_SCF==0);
        *p++= ATD0DR0L;
      } 
      //PORTB_BIT1=0; //采集完后输出一个低电平，这些信号在拔掉BDM后也能正常出来的
      ATD0CTL5=0X00;
      z++; 
    }
    
  }
  onSamplenum++;
   
   
  TFLG1_C2F=1;  //清除行中断标志
}





#pragma CODE_SEG DEFAULT		 
/************************************************************************/
/************************************************************************/


void Overall_ini (void){

  DDRA = 0x00; 
  DDRB = 0xff;

  //Set_PLL();
  PWM_ini();
  Capture_ini();
  SCI_ini();
  
  Image_ini();

}
/*********************************************************/
 void delayms(int ms)
{   
   int ii,jj;
   if (ms<1) ms=1;
   for(ii=0;ii<ms;ii++)
     for(jj=0;jj<3338;jj++);    //40MHz--1ms      
}

void SetBusCLK_40M(void)
{   
    CLKSEL=0X00;				//disengage PLL to system
    PLLCTL_PLLON=1;			//turn on PLL
    SYNR =0xc0 | 0x04;                        
    REFDV=0xc0 | 0x01; 
    POSTDIV=0x00;       //pllclock=2*osc*(1+SYNR)/(1+REFDV)=80MHz;
    _asm(nop);          //BUS CLOCK=40M
    _asm(nop);
    while(!(CRGFLG_LOCK==1));	  //when pll is steady ,then use it;
    CLKSEL_PLLSEL =1;		        //engage PLL to system; 
}
void SetBusCLK_48M(void)
{   
    CLKSEL=0X00;				//disengage PLL to system
    PLLCTL_PLLON=1;			//turn on PLL
    SYNR =0xc0 | 0x05;                        
    REFDV=0xc0 | 0x01; 
    POSTDIV=0x00;       //pllclock=2*osc*(1+SYNR)/(1+REFDV)=80MHz;
    _asm(nop);          //BUS CLOCK=48M
    _asm(nop);
    while(!(CRGFLG_LOCK==1));	  //when pll is steady ,then use it;
    CLKSEL_PLLSEL =1;		        //engage PLL to system; 
}
void SetBusCLK_64M(void)
{   
    CLKSEL=0X00;				//disengage PLL to system
    PLLCTL_PLLON=1;			//turn on PLL
    SYNR =0xc0 | 0x07;                        
    REFDV=0xc0 | 0x01; 
    POSTDIV=0x00;       //pllclock=2*osc*(1+SYNR)/(1+REFDV)=80MHz;
    _asm(nop);          //BUS CLOCK=64M
    _asm(nop);
    while(!(CRGFLG_LOCK==1));	  //when pll is steady ,then use it;
    CLKSEL_PLLSEL =1;		        //engage PLL to system; 
}
void SetBusCLK_80M(void)
{   
    CLKSEL=0X00;				//disengage PLL to system
    PLLCTL_PLLON=1;			//turn on PLL
    SYNR =0xc0 | 0x09;                        
    REFDV=0xc0 | 0x01; 
    POSTDIV=0x00;       //pllclock=2*osc*(1+SYNR)/(1+REFDV)=80MHz;
    _asm(nop);          //BUS CLOCK=80M
    _asm(nop);
    while(!(CRGFLG_LOCK==1));	  //when pll is steady ,then use it;
    CLKSEL_PLLSEL =1;		        //engage PLL to system; 
}
void Set_PLL (void){
  REFDV = 1;  // set the REFDV register 16M*2*(4+1)/(1+1)=80M 
  SYNR =4;    // set the SYNR register to give us a 80 MHz PLL-clock.
  asm nop    // nops required for PLL stability.
  asm nop 
  asm nop
  asm nop
  while ((CRGFLG&0x08)==0); // wait here till the PLL is locked. 
  CLKSEL|=0x80;             // switch the bus clock to the PLL.
                            // bus clock=PLL clock/2=40MHz  
}

/*********************************************************/
void PWM_ini (void){
  servoOUT =SERVO_CENTER;
  
  PWME = 0x00;
  PWMCTL_CON01 = 1;           // PWM01 合并 16 bit
  PWMPRCLK = 0x33;            // A=B=40M/8=5M
  PWMSCLA = 100;              // SA=A/2/100=25k
  PWMSCLB = 10;               // SB=B/2/10=250k
  PWMCLK = 0b00011100;        // PWM0,1-A; PWM2,3-SB; PWM4-SA
  PWMPOL = 0xff;              // 位极性＝1 Duty＝High Time
  PWMCAE = 0x00;              // 对齐方式－左对齐 
  
  

//舵机初始化:
  PWMPER01 = 0xc350;            // 25000 = 0x61a8;channel 01 frequnce=200hz	 50000=0xc350
  PWMDTY01 = servoOUT;            // 舵机DTY 初始值
  

//电机初始化:  
  
  PWMPER2 = 50;    // 驱动电机 Frequency=SB/25=5K
  PWMPER3 = 50;    // 驱动电机 Frequency=SB/25=5K
  PWMDTY2 = 20;   //speed
  PWMDTY3 = 0x00;   //speed initial

//PWMenable: 
  PWME = 0x0f;             
}

/*********************************************************/
 
void Capture_ini (void){
  
  //speed feedback capture
  TIOS_IOS7=0;   //speed feedback capture input
  //image capture
  TIOS_IOS1=0;   //INPUT CAPTURE ,VS INPUT
  TIOS_IOS2=0;   //INPUT CAPTURE ,HS INPUT
  
  TSCR1_TEN=1;   //ENABLE TIMER 
  
  TCTL4_EDG1A=1; //  capture rising edge first
  TCTL4_EDG1B=0;
 
  TCTL4_EDG2A=1;
  TCTL4_EDG2B=0;
 /*****************************/ 
  PACTL_PAEN =1; // PAenable
  PACTL_PAMOD=0; // event trigger
  PACTL_PEDGE=1; // capture rising edge 
}

/*********************************************************/

void SCI_ini (void) {
  SCI0CR2 = 0X2C; //发送与接受方使能
  
  SCI0BDH = 0X00; 
  SCI0BDL = 130;  //bus clock=40M,baud=19200
} 
   

/*********************************************************/
 void AD_Init(void) 
{  
  
  ATD0CTL1=0x00;   //7:1-外部触发,65:00-8位精度,4:放电,3210:ch
  ATD0CTL2=0x40;   //禁止外部触发, 中断禁止    
  ATD0CTL3=0xa0;   //右对齐无符号,每次转换4个序列, No FIFO, Freeze模式下继续转    
  ATD0CTL4=0x01;   //765:采样时间为4个AD时钟周期,ATDClock=[BusClock*0.5]/[PRS+1]
  ATD0CTL5=0x30;   //6:0特殊通道禁止,5:1连续转换 ,4:1多通道轮流采样
  ATD0DIEN=0x00;   //禁止数字输入 
} 
void ADC_ini(void){ 
  ATD0CTL1=0x00;
  ATD0CTL2=0X40;         //DISABLE INTERRUPT 
  for(z=0;z<20;z++) asm("nop");   //warm up 
  
  ATD0CTL3=0X88; 
  ATD0CTL4=0X03; 
  ATD0DIEN=0X00;
}

/*********************************************************/
void Image_ini (void){
  PingOrPong=PING; //SAMPLING PING FIRST ;
  ADC_ini();
  //ATD0DIEN=0XFF;
}

/*********************************************************/
void Get_speed (void) {
 
  cur_speed=PACNT;
  PACNT=0;
}
/*********************************************************/

void CCD_get (void) {
  //register byte hang;
  //register byte *p;
  
  
  if (PingOrPong==PING) 
    ImageReady=&ImagePong[0][0];
 
  else if (PingOrPong==PONG) 
    ImageReady=&ImagePing[0][0];
  
  
  for(y=0;y<10;y++){
    DealFarLine(y);  
  }
  
  for(y=10;y<row_max;y++){
    DealNearLine(y);
  }
  
  
  
  
  
  
  
  /*if(abandon==0) {
    if (GuideLine[top]>VIDEO_CENTER+28)//-right +left
    {
      curve=-(top+4);
      PORTB=0b11111110;
    }

    else if (GuideLine[top]<VIDEO_CENTER-28)
    {
      curve=(top+4); 
      PORTB=0b01111111; 
    }
    else if (GuideLine[top]>VIDEO_CENTER+20)
    {
      curve=-(top+3);
      PORTB=0b11111101;
    }
    else if (GuideLine[top]<VIDEO_CENTER-20)
    {
      curve=(top+3);
      PORTB=0b10111111;
    }
    else if (GuideLine[top]>VIDEO_CENTER+9)
    {
      curve=-(top+2); 
      PORTB=0b11111011;
    }
    else if (GuideLine[top]<VIDEO_CENTER-9)
    {
      curve=(top+2);
      PORTB=0b11011111;
    }
    else if (GuideLine[top]>VIDEO_CENTER+3)
    {
      curve=-(top+1);
      PORTB=0b11110111;
    }
    else if (GuideLine[top]<VIDEO_CENTER-3)
    {
      curve=(top+1);
      PORTB=0b11101111;
    }
    else	 
    {
      curve=0;
      PORTB=0b11111111;
    } 
  }
   
  else if(abandon==1){
  curve=last_curve;
  }*/
  
  
  
  PWM_PID_servo();
  
}

/*********************************************************/

/*void Get_line (void){
  register byte *p;
  //register byte i
  err_line=0;
  
  if (PingOrPong==PING) 
  {
    p=&ImagePong[0][0];
  } 
  else if (PingOrPong==PONG) 
  {
    p=&ImagePing[0][0];
  }
  
  for(y=0;y<row_max;y++) {
    for(x=0;(*(p+(y*col_max)+x)!=0)&&x<(col_max-1);x++);
    line_left=x;
    for(x=col_max-1;(*(p+(y*col_max)+x)!=0)&&x>0;x--);
    line_right=x;
    
    line_width=line_right-line_left;
    if(line_width>1) 
      GuideLine[y]=(line_right+line_left)/2; 
    else 
      err_line++;
  }
  
  if(err_line>=5) abandon=1;
  
} */



byte Get_line (void){
  byte *p;
  byte value;
  volatile byte k;
  
  //register byte i
  //err_line[]=(0);
  p=ImageReady;
  
  
  for(k=2;k<row_max;k++){
    if(GuideLine[k]<=VIDEO_LEFT||GuideLine[k]>=VIDEO_RIGHT)
      err_line[k]=0;
    else                                                
      err_line[k]=1;   //先找出扫描过程提取失败的点     1表示好的点
  }
  
  k=34;
  
  while(err_line[k]==0&&err_line[k-1]==0&&err_line[k-2]==0&&k>8) k--;
  
  if (k<10) {
    return 1 ;              //全场都失败返回1
  } 
  
  
  
  k=33;
      
  do{
    if (err_line[k]==0)
      GuideLine[k]=2*GuideLine[k+1]-GuideLine[k+2];
    else {
      if(GuideLine[k+1]-GuideLine[k+2]<0)
        value= GuideLine[k+2]-GuideLine[k+1];
      else
        value=GuideLine[k+1]-GuideLine[k+2];
      
       //////
      if(GuideLine[k]<(GuideLine[k+1]-value-4)||GuideLine[k]>(GuideLine[k+1]+value+4))  
        GuideLine[k]=2*GuideLine[k+1]-GuideLine[k+2];    //线的连续行纠正
    }
    
    k--; 
  } while (k>1 &&GuideLine[k+1]>VIDEO_LEFT&&GuideLine[k+1]<VIDEO_RIGHT);
    
  top=k+1;   //top>=2
  if(GuideLine[top]<=VIDEO_LEFT||GuideLine[top]>=VIDEO_RIGHT)
    GuideLine[top++]=0;
         //Find the top  
         

  last_curve=curve;
  
  
  if(top<12) {
    curve=VIDEO_CENTER-((GuideLine[top]+GuideLine[top+1]+GuideLine[top+2]+GuideLine[top+3]+GuideLine[top+4]+GuideLine[top+5])/6);
  
  }
  
  //速度参数
  if(top<8) {
    AngleWay= GuideLine[top+1]-GuideLine[top+9];
    if(AngleWay<0)
      AngleWay=0-AngleWay;      
  }
 /////////////////////////////////////////// 
  if(top<12) {
    errWay1=(GuideLine[top]+GuideLine[top+1]+GuideLine[top+2])/3-(GuideLine[top+17]+GuideLine[top+18])/2;