main.c

我们花了很长时间才调出来的程序

/**********************************************************
注意:
1. 清华的板子要把VRL接地，决定AD的参考电压
2. PH4,PH5分别接LM1881的行同步和场同步信号端
3. PAD00接摄像头的视频信号端
4. PWM接线:01舵机，23和45电机,1和3和5作为输出脚
5. 7.2v的镍镉电池的电池电压低于6.5V时尽量不要使用
6. 镍镉电池要坚持"放完充满"的原则

最后修改：2008-8-15
***********************************************************/

/*********宏定义*******************************************/ 

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

//绝对值定义
#define ABS(i) (((i)>=0)?(i):(0-i))
#define true 1
#define false 0
//小车控制
#define CENTER 1450                 //舵机中心位置
#define error (int)(line[magic_row]-17)  //小车偏离中心线程度 
#define BANG_BANG_MAX 180           //bang-bang控制的上线
#define BANG_BANG_MIN 70            //bang-bang控制的底线
//图像处理
#define VALID_ROW 23                //重要，小车导航的有效行数
#define YUZHI 16

/**********变量声明***************************************
typedef unsigned char byte;
typedef unsigned int word;
**********************************************************/
//image
byte x=0,y=0,z=0;//行场信号处理标志
byte image[28][35];
byte imageBuffer[28][35];
byte image_ready=0;
byte last_direction=0;     
byte line[28];   //得出的黑线
byte valid_row=0,magic_row=0;

//speed
byte REAL_SPEED,REAL_SPEED_CURVE;
byte speed;
byte pulse[6]={0,0,0,0,0,0};

/**********延迟程序***********************/
void delay(word time){			//time=1000是对应周期1秒（大约）
    word i,j;
    for (i=0;i<=250;i++)
     for(j=0;j<=time;j++){
       asm nop;
     }
}

/*********PLL设置程序*********************/

/*S12 单片机中有四个不同的时钟，即外部晶振时钟、锁相环时钟、总线时钟和内核时钟。 

开发板采用的是16MHz 的外部晶振，因此外部晶振时钟为16MHz；

默认设置下，锁相环时钟为32MHz，总线时钟为8MHz，内核时钟为16MHz。

锁相环时钟与外部晶振时钟的倍、分频关系由SYNR、REFDV 两寄存器决定。

总线时钟用作片上外围设备的同步，而内核时钟则用作CPU 的同步，它决定了指令执行的速度。

赛车采用摄像头作为寻线传感器，为提高AD转换速度，增加一行视频信号的采样点数，

我们对单片机进行了超频，设置超频后的总线时钟为32MHz, 设置过程为：
REFDV=3;
SYNR=7; //bus clock=16MHz*(SYNR+1)/(REFDV+1)
while(0==CRGFLG_LOCK); //直到VCO使能
CLKSEL=0x80; // PLLSEL=1*/


void PLL_Init(void){        //总线初始 Bus clock=32MHz
    SYNR=7;                   //2*OSCCLK*(SYNR+1)/(REFDV+1)
    REFDV=3;
    while(0==CRGFLG_LOCK);
    CLKSEL=0x80; 
}

/*********RTI初始化***********************/
void RTI_Init(void) {
    HPRIO=0xCC;//set PH as the highest priority interrupt
    PPSH=0x20;
    /*
    Polarity Select Port H
    Rising edge on the associated port H pin 
    sets the associated flag bit in the PIFH register.
    */
    PIEH_PIEH5=1;
    PIEH_PIEH4=0;
}

/*********累加器初始化********************/
void ECT_Init(){
    TIOS=0xFE;   
    TCTL4=0b00000010;     
    TSCR1_TEN = 1; 
    ICPAR_PA0EN = 1; 
}

/*********A/D转换程序*********************/
void AD_Init(void) {//初始化
    ATD0CTL2=0xC0;
    // open AD, quick clear, no wait mode, inhibit outer awake, inhibit interrupt
    ATD0CTL3=0x08;
    // one transform in one sequence, No FIFO, contine to transform under freeze mode
    ATD0CTL4=0x81;
    // 8-bit precision, two clocks, ATDClock=[BusClock*0.5]/[PRS+1] ; PRS=1, divider=4 ;
    //BusClock=8MHZ
    ATD0CTL5=0xA0;
    // right-algned unsigned，single channel，
    //channel 0
    ATD0DIEN=0x00;
    // inhibit digital input
}

/*********串口初始化*********************/
void Sci_Init(){
    SCI0BDL = (byte)((64000000UL/ 2)/9600/16);   
    //SCI baud rate = SCI module clock / (BR x 16)
    SCI0CR1=0;					/*normal,no parity*/
    SCI0CR2=0x2C;       /*RIE=1,TE=1,RE=1*/
}

/*********端口初始化*********************/
void Port_Init(){
    DDRH = 0x00;//接收图像中断，PH4 行，PH5 场
    DDRB = 0xFF;
    PORTA = 0xFF;
    DDRA = 0x0F;
    PORTB = 0x00;
}

/*********PWM控制舵机和直流电机**********/
void PWM_Init(){        
    PWME=0x00;            //禁止PWM输出
    PWMPOL=0x2A;          //极性,决定了PWM的起始电平
    PWMCLK=0xAA;          //clock sa 1,5 ; clock sb 3,7
    PWMPRCLK=0x22;        //bus clock/4 Prescaler Select for Clock A,B
    PWMCAE=0x00;          //左对齐
    PWMCTL=0xF0;          //16位PWM
    PWMSCLA=4;            //Clock SA = Clock A / (2 * PWMSCLA)
    PWMSCLB=4;            //不用说了吧
    PWMPER01=10000;       //PWM01口输出频率100HZ
    PWMPER23=200;         //PWM23、PWM45口输出频率5KHZ 
    PWMPER45=200;
    PWMDTY45=0;
    PWMDTY23=BANG_BANG_MAX;   //快速加速       
    PWMDTY01=CENTER;         //center 向右增大
    PWME=0x2A;            
    //0，1级联，从1输出，控制舵机；
    //2，3和4，5分别级联，3，5分别是输出，控制直流电机
}


/***********发射端程序*******************/
void Sci_Tx(byte text){
    byte temp;
    temp=SCI0SR1;      /*clear flag*/
        while (!(SCI0SR1&0x80));  /* wait for output buffer empty */
    SCI0DRH=0;
    SCI0DRL=text;
}

/*********串口发送视频数据***************/
void Sci_CCD(){
    byte i,j;
    DisableInterrupts;
    for(i=0;i<28;i++) {
      for(j=0;j<35;j++){
        Sci_Tx((byte)image[i][j]);
      }
    }
} 

/*********中值滤波*********************************/
byte get_mid(byte a, byte b, byte c){ 
    byte x;
    if(a>b){ 
      x=b; 
      b=a; 
      a=x; 
    } 
    if(b>c){ 
      x=c; 
      c=b; 
      b=x; 
    } 
    if(a>b){ 
      x=b; 
      b=a; 
      a=x; 
    } 
    return b;   
} 

/*********图像处理与舵机驱动***********************/
void Image_Process(){     
    byte i,j,z=0;
    valid_row=0;
    magic_row=0;
    Sci_CCD(); 
    //矩阵处理
    for(i=0;i<28;i++){  
      for(j=0;j<34;j++){         //粗加工
        if(ABS((image[i][j]-image[i][j+1] ))>=YUZHI){   //比较阈值
          line[i]=j;     //记下列坐标
          break;
        }else{
          line[i]=0xAA;
        }
      }   
      for(j=0;j<34;j++){         //深加工
        if(ABS((image[i][j]-image[i][j+1] ))>=YUZHI){   //比较阈值
          z++;           //记下黑白边沿个数
          if(z>=3){
            line[i]=0xAA;//标记
          }else{
            line[i]=j;     //记下列坐标
          }
        }
      }
      z=0;
    }
    //中值滤波
    for(i=1;i<27;i++){
      if(line[i]!=0xAA){
        line[i]=get_mid(line[i-1],line[i],line[i+1]);
        //Sci_Tx((byte)line[i]);
      }
    }
    //统计有效行  
    for(i=1;i<27;i++){
      if(line[i] != 0xAA){
        valid_row++;
      }
    }
    //Sci_Tx((byte)valid_row);
    //求第一个有效点
    for(i=1;i<27;i++){
      if(line[i] != 0xAA){
        magic_row=i;break;//找到为止
      }
    }
    //Sci_Tx((byte)magic_row);
    //Sci_Tx((byte)line[magic_row]);
}

/*********LED***************************/
void LED(){
    //显示
    if(error<=7 && error>=4) {
      PORTA_BIT0=0;
      PORTA_BIT1=1;
      PORTA_BIT2=1;
      PORTA_BIT3=1;
    }else if(error<=11 && error>=8) {
      PORTA_BIT0=1;
      PORTA_BIT1=0;
      PORTA_BIT2=1;
      PORTA_BIT3=1;
    }else if(error>=12) {
      PORTA_BIT0=0;
      PORTA_BIT1=0;
      PORTA_BIT2=1;
      PORTA_BIT3=1;
    }else if(error>=-7 && error<=-4) {
      PORTA_BIT0=1;
      PORTA_BIT1=1;
      PORTA_BIT2=1;
      PORTA_BIT3=0;
    }else if(error>=-11 && error<=-8) {
      PORTA_BIT0=1;
      PORTA_BIT1=1;
      PORTA_BIT2=0;
      PORTA_BIT3=1;
    }else if(error<=-12) {
      PORTA_BIT0=1;
      PORTA_BIT1=1;
      PORTA_BIT2=0;
      PORTA_BIT3=0;
    }else{
      PORTA_BIT0=1;
      PORTA_BIT1=0;
      PORTA_BIT2=0;
      PORTA_BIT3=1;
    }
}

/*********舵机***********************/
void duoji(void){    //舵机
    //根据line[magic_row]的值决定方向,valid_row控制出界
    if(valid_row<VALID_ROW){
      line[magic_row]=last_direction;
      if(line[magic_row]>17){      //使赛车迅速回到赛道
        line[magic_row]=34;
      }else if(line[magic_row]<17){
        line[magic_row]=0;
      }
    }
    //舵机控制,舵机不对称
    if(error>4){         //right
      PWMDTY01=CENTER+error*((byte)(300/17));   
    }else if(error<-4){    //left
      PWMDTY01=CENTER+error*((byte)(300/17));
    }else{                //straight
      PWMDTY01=CENTER;
    } 
    //限制
    if(PWMDTY01>=CENTER+300){  //limit
      PWMDTY01=CENTER+300;
    }else if(PWMDTY01<=CENTER-300){  //limit
      PWMDTY01=CENTER-300;
    }  
    //显示 
    LED();
    //状态机
    last_direction=line[magic_row];  
}

/*********获得速度***********************/
void get_speed(){ 
    byte i; 
    for(i=0;i<5;i++){ 
      pulse[i]=pulse[i+1]; 
    } 
    pulse[5]=PACN0; 
    speed=pulse[5]-pulse[0];
    //Sci_Tx((byte)speed); 
}  

/*********速度控制***********************/
void bang_bang(byte a){
    if(speed<a) {
       PWMDTY23=BANG_BANG_MAX;
    }
    if(speed>a){
       PWMDTY23=BANG_BANG_MIN;
    }
}

/*********速度调节**********************/
void sudu(int error_index){
    if(ABS(error_index)>=6 && ABS(error_index)<=8){
      bang_bang(REAL_SPEED-1);
    }else if(ABS(error_index)>=9 && ABS(error_index)<=11){
      bang_bang(REAL_SPEED-2);
    }else if(ABS(error_index)>=12){
      bang_bang(REAL_SPEED_CURVE);
    }else{
      bang_bang(REAL_SPEED);
    }
}

/*********电机控制**********************/
void dianji() {
    get_speed();
    sudu(error);
}


/*********中断处理***********************/
#pragma CODE_SEG __NEAR_SEG NON_BANKED     
interrupt 25 void PH_ISR(void) {
    if(PIFH_PIFH5){
      x=0;y=0;z=0;
      PIFH_PIFH5=1;
      PIEH_PIEH4=1;
    }else if(PIFH_PIFH4){
      PIFH_PIFH4=1;
      x++;
      if((x>35)&&(x<289)&&(x%9==0))
      {
        AD_Init() ;
        for(y=0;y<47;y++)
        {
          while(!ATD0STAT1_CCF0);
          if(y>11){
            imageBuffer[z][y-12]= ATD0DR0L;   //Image Buffer
            //B口实时显示AD数据流 ?????
            PORTB = ~ATD0DR0L;
          }
        }
        z++;
        ATD0CTL2=0x00;
      }     
    } 
    if(z==28){
      image_ready=true;
      PIEH_PIEH4=0;
    }
}
#pragma CODE_SEG DEFAULT

/*********拨码开关参数设置***************/
void boma(){
    if(PORTA_BIT7==1){      //激进控制
      PWMDTY23=170;
      REAL_SPEED=0x36;
      REAL_SPEED_CURVE=0x1A;
    }else if(PORTA_BIT6==1){ //稍微激进
      PWMDTY23=160;
      REAL_SPEED=0x1A;
      REAL_SPEED_CURVE=0x0B;
    }else if(PORTA_BIT5==1){ //中等控制
      PWMDTY23=120;
      REAL_SPEED=0x0A;
      REAL_SPEED_CURVE=0x07;
    }else if(PORTA_BIT4==1){ //保守控制，应急处理
      PWMDTY23=110;
      REAL_SPEED=0x09;
      REAL_SPEED_CURVE=0x06;    
    }else{   //最优控制
      PWMDTY23=180;
      REAL_SPEED=0x2A;
      REAL_SPEED_CURVE=0x1A;
    }  
}

/*********主函数区域*********************/
void main(void) { 
    int i,j;
    DisableInterrupts;  //禁止全局中断
    PLL_Init();
    RTI_Init();
    Sci_Init();
    Port_Init();
    PWM_Init();
    ECT_Init();
    boma();        //read configurations
    delay(2000);   //delay some time
    EnableInterrupts;//开启全局中断，程序进入执行状态
    for(;;){
      
      //准确获得一幅图像
      if(image_ready==true) {  
        image_ready=false;
        //拷贝图像缓冲区的数据后进行处理,一个控制周期
        //20ms?????????
        for(i=0;i<28;i++) {
          for(j=0;j<35;j++){
            image[i][j]=imageBuffer[i][j];
          }
        }
        Image_Process();
        duoji();
        dianji();
      } 
    }
}