main.c

来自「通过智能车上的CCD摄像头采集道路状况」· C语言 代码 · 共 1,391 行 · 第 1/3 页

#include <hidef.h>      /* common defines and macros */
#include <mc9s12dg128.h>     /* derivative information */
#pragma LINK_INFO DERIVATIVE "mc9s12dg128b"
    

#define SAMPLENUM   165
#define VIDEO_LEFT   20
#define VIDEO_RIGHT   158
#define SAMPLELINE 30
  

#define VIDEO_CENTER  89

#define SERVO_CENTER  3390
#define SERVO_RIGHT  2900 
#define SERVO_LEFT  3880
#define THRESHOLD   169


void BUSCLK_init(void);   
void Comparer(void);
void Init_Capture(void);
void ImageGetting(void);

unsigned char  GetLine(void);
void  Init_servo(void);
void  Init_motor(void);
void ServoControl(void);
void SpeedSample(void);

/////////////////
 void SpeedControl0(void);
  void SpeedControl1(void);
  void SpeedControl2(void);
  void SpeedControl3(void);
//////////////
void DealOneLine(unsigned char LineNum);

 void  bottom  (void);

 void Switch(void) ;

void Brake(void);           
 
 void FarLine(unsigned char row);
      
                                                    
 static unsigned char ren,SkipNum1;
 
 
 static unsigned  char Image[SAMPLELINE][SAMPLENUM];
 
 
 static unsigned char GuideLine[SAMPLELINE] ;
 


 unsigned char DoDealOneLine = 0;
 unsigned char DealLineNum;
   unsigned char stop=1; ///////////// /////////////////////////
 
      unsigned int  Time=400;
      
     unsigned int jishi=0;
 
 static char ImageGood[SAMPLELINE];


 unsigned char  Samplespeed=0;
 
 unsigned int servoScale=SERVO_CENTER;
 unsigned int Last_servoScale=SERVO_CENTER;
  unsigned int hao;
 
 unsigned char Image_good=0;
 signed char top;
 
 unsigned char  startflag=0;
 unsigned int  startNum=0;
 
    unsigned char speedFlag=0;

   unsigned char   choose=0;
 unsigned char Kp = 7;
unsigned char Kd = 7;

unsigned char s1=0;
unsigned char s2=0; 

unsigned char  safe=0;
 
  unsigned char feng=0;
 unsigned char speed=0;

unsigned char SendImage;

unsigned char Begin=0;

unsigned char get=0;

unsigned char know=0;

unsigned char first=0;
 
unsigned char sheng=0;
 
 int angle=0;
 int LastAngle=0;
  int AngleWay=0;
 int errWay1,errWay2,errWay; 
  
 //*************************Declare******************************************// 
  
  
  #pragma CODE_SEG __NEAR_SEG  NON_BANKED
/*******************Field sync interrupt************************/

interrupt void Fieldsync(void) {  //CAPTURE ISR、场中断      // PT6
     
      TFLG1_C6F=1;			 //clear场同步interrupt flag 
       
    hao=0; ren=0;  
    SkipNum1=0;
                     
    TFLG1_C2F=1;		//START ONE FRAME 
     TIE_C2I=1;
     
}
/*********************Row sync interrupt**********************/         // PT2
interrupt void RowSync(void) {    // 行中断
 // register unsigned char i;
  //register unsigned char *p;
 
  register unsigned char DoSampleLine=0;
  TFLG1_C2F=1;//清除行中断标志
   if(hao%80==0) {
     speedFlag=1;
   Samplespeed=PACN32;     //5ms for motor
 PACN32=0;
   }
  hao++;
  
  if(hao<46) {    //46
    DoSampleLine = 0;  //消隐信号  
  } 
  else if(hao<78) {
    SkipNum1++;
    if(SkipNum1==3)    {        
      DoSampleLine = 1;  //0---9     //48+3x
      SkipNum1 = 0;  
    }    
  }
 
  else if(hao<272) {
  
  if((hao-78)%10==0)
        
     DoSampleLine = 1;    //10---29    //78+10x
     
  }
  
  
  if(DoSampleLine==1) {
    
   /* p=&Image[ren][0];
   for (i=SAMPLENUM;i>0;i--) 
     // while(ATD0STAT0_SCF==0);
           
      
     *p++= PORTAD0_PTAD5; */ 
     _asm{
      LDAB  #165
      LDAA  ren
      MUL 
      ADDD  #Image
      XGDX            
      LDAB #165
      

 SAMPLE: LDAA  PORTAD0
         ANDA  #$20
         STAA  1,X+
         NOP
         NOP
         NOP 
         NOP
         NOP
         
       DBNE B,SAMPLE
       
    }    
      
       
                 
    
    DoDealOneLine=1;    
    DealLineNum =ren;
    ren++;
   }   
    
  }
 


 
#pragma CODE_SEG DEFAULT
 // *******************超频至44M****************************//
void BUSCLK_init(void){
  REFDV=0x02;
 SYNR= 0x0A;  //busclk=44M
 while(!(CRGFLG_LOCK)) ;
 CLKSEL=0x80;
 }
 //******************Speed Sample**************************//      // PT7


void SpeedSample(void) {
 PACTL_PAEN=1;
  PACTL_PAMOD=0; //PULSE ACCUMULATOR MODE
  PACTL_PEDGE=0; //COUNT ON RISING EDGE
  
}
//****************************拨码开关*********************************//
void Switch(void) {
  if((PORTB&0x0f)==0)   //时间选择
   Time=250;     //6s
   else  if((PORTB&0x0f)==1)
   Time=350;     //7s
    else  if((PORTB&0x0f)==2)
   Time=400;     //8s
    else  if((PORTB&0x0f)==3)
   Time=450;     //9s
    else  if((PORTB&0x0f)==4)
   Time=500;     //10s
    else  if((PORTB&0x0f)==5)
   Time=550;     //11s
    else  if((PORTB&0x0f)==6)
   Time=600;     //12s
    else  if((PORTB&0x0f)==7)
   Time=650;     //13s
    else  if((PORTB&0x0f)==8)
   Time=700;     //14s
    else  if((PORTB&0x0f)==9)
   Time=750;     //15s
    else  if((PORTB&0x0f)==10)
   Time=800;     //16s
    else  if((PORTB&0x0f)==11)
   Time=850;     //17s
    else  if((PORTB&0x0f)==12)
   Time=900;     //18s
    else  if((PORTB&0x0f)==13)
   Time=950;     //19s
    else  if((PORTB&0x0f)==14)
   Time=1000;     //20s
    else                         //if((PORTB&0x0f)==15)
   Time=1050;     //21s
   //get=(PTS&3);  //速度选择
    if((PTS&3)==0)
       choose=0;
    else if((PTS&3)==1)
    choose=1;
       else if((PTS&3)==2)
    choose=2;
       else 
    choose=3; 
       
}
  
//****************PWM1:OUTPUT OF SERVO PWM SIGNAL****************************//
void Init_servo(void){
  servoScale=SERVO_CENTER;  //SERVO_CENTER= 1510  ,使舵机处于正中间的PWM值
   angle=0;
  LastAngle=0;
  Last_servoScale=SERVO_CENTER;
  PWMSCLB=10;  //SB=44M/2*10=2.2M
 
  PWMCLK|=0X0C; //clock source from clock sb
  PWMPRCLK|=0X00;  //prescale clock SB to 44M
 
  
  
  PWMCTL=0X20; //CONNECT CHANEL2 AND CHANEL3
  PWMPER23=20000; //PRE 110hz
  PWMDTY23=SERVO_CENTER;           //5%      //23.5%   1600     1350
  PWMPOL|=0X08;    //
  PWME|=0X08; //ENABLE PWM3
}


//***************************DC motor initial**********************************//
void Init_motor(void){

  
 
  
  PWMSCLA=200;  //SA=22M/(200*2)=55K
  
   PWMCLK_PCLK1=1; //clock source from clock sA
  PWMPRCLK|=0X01; //prescale clock sA to 1mhz   44m/2=22M
  PWMPER1=100;     //550Hz
  PWMDTY1=80; 
  PWMPOL_PPOL1=1;
  PWME_PWME1=1;        //ENABLE PWM1
  
  
}
 //******************AD初始化**************************//

  void Comparer(void) {
   
 
  PWMPRCLK|=0X01; //prescale clockA to 1mhz   44m/2=22M         //PWM5做比较门限
  
  
  PWMCLK_PCLK5=0; //clock source from clockA
   PWMPOL_PPOL5=1;  
  PWMPER5=100;     //220K
  PWMDTY5=43;       //比较门限为2.15V
  
 
  PWME_PWME5=1;        //ENABLE PWM5
  ////////////////
   PWMPRCLK|=0X00;  //prescale clock SB to 44M
   PWMCLK_PCLK7=0; //clock source from clockB
    PWMPOL_PPOL7=1;  
    PWMPER7=4;       //11M
    PWMDTY7=2;       //50%
     PWME_PWME7=1;        //ENABLE PWM5
}

////////////////////////////////////////////
 void AD_init(void) {
   
  // char  t;
 // ATD0CTL2=0XC0;  //启动转换，禁止中断
   //t=6;
  // while(t>0)
   //t--;
  //ATD0CTL3=0X08;   //转换序列
  //ATD0CTL4=0X80;   //8位AD转换
  ATD0DIEN_IEN5=1;
  //ATD0CTL5=0XA4; 
                 
}
/*******************AD initial************************/
void Init_Capture(void) {
 
 TSCR1_TEN=1; //ENABLE TIMER 

 TCTL3_EDG6A=1; //  capture rising edge first
 TCTL3_EDG6B=0;
 
 TCTL4_EDG2A=1;
 TCTL4_EDG2B=0;
  HPRIO=0XE2;
 
  TIE_C6I=1;  //ENABLE  CAPTURE INTERRUPT
  TIE_C2I=0; 
}
/******************Image sample*************************/
void ImageGetting(void) {

    
 Comparer();
  AD_init();
  Init_Capture();   //START SAMPLING
  for(;;){
  
 
    
    do {
      if(DoDealOneLine==1){
       if (DealLineNum>9) {
        
      
          DealOneLine(DealLineNum);    //寻找起跑线并引导线
          DoDealOneLine = 0;
        
       }
      }  //////////////////采图像并处理
      if(stop<3) {
        if(speedFlag==1){
        if(choose==0)
          SpeedControl0();        //任高速
         else if(choose==1)
          SpeedControl1();       //任低速
         
        else if(choose==2)
          SpeedControl2();       //最高速
        
        else
            SpeedControl3();     //高速
          speedFlag=0;
        } 
      }
      else {
         
        Brake();//速度控制 
      }
    }while(DealLineNum<29) ;   //One frame finished 
 
    TIE_C2I=0;
    DealLineNum=0;  
     
    ServoControl();  //控制舵机   
    ///////////////////////////////stop line
    if((servoScale<3440&&servoScale>3340)&&(Begin==0)){       //直道才识别  3390
    sheng++;
    if(sheng>10){
      sheng=0;
      
     if((top==2)&&(startflag==1)&&(AngleWay<4)&&(safe==0)) {
    
    Begin=1;
   // PORTA=0xff;
       know++;              //起跑线识别次数
    }
    }
    }
     else
     sheng=0 ;
    
    if(Begin==1) {
    first=1;
      
    startNum++ ; 
    //if(startNum>20)
    // PORTA=0; 
    }
    if(first==1)
    jishi++;    //从第一次识别开始计时
    if(stop<2){        
      if(startNum>Time){
        startNum=0;
        startflag=0;
        stop++;
         Begin=0;
      }
    } 
    else if(stop<3){
      if(startNum>15)
        stop++;
    }
    ////////////////////////
   /* if((jishi>(Time+200))&&(know<2)){   //如果在延时的4s后还没有第二次识别，则在剩余时间内不识别
        stop=2;
        safe=1;
    }
    if((jishi>(2*Time+30))&&(safe==1)){
       stop=2;
       safe=0;                      //在延时的两倍时间后再次识别，一次就停
    }
    
    */ 
 
  ///////////////////////////////stop line  
  }
}


//**************************************************//
void DealOneLine(unsigned char LineNum){        //黑为0，白为1
  unsigned char *p;
   unsigned char i;