智能车.c

智能车：核心单片机MC9S12DG128 CCD摄像头对道路信息进行检测 光电传感器与摄像头融合控制方案 保证稳定的条件下追求行使时间最短

 程序源代码
#include <hidef.h>      /* common defines and macros */
#include <mc9s12dg128.h>     /* derivative information */
#pragma LINK_INFO DERIVATIVE "mc9s12dg128b"
/********** Variables  Definition *********************/
uint uiVideo_line_count;			
uint uiVideo_line_flag;			
uint uiVideo_Data[20][32];		 
uint uiVideo_over_flag;			
uint uiVideo_line;            	
uint uiVideo_AD_Data[20][32];
uint uiVideo_Limen[20][6];
uint uiVideo_Ava_Limen[20];
uint uiVideo_Field_uicount;
uint uiVideo_init_flag;
uint uiVideo_Center[20][6];
uint uiVideo_Center_Count;
uint uiVideo_Continuous_Line[20];
uint uiVideo_Continuous_Count;
uint uiLine_First_Place;
uint uiLine_Last_Place;
uint uiRow_First_Place;
uint uiRow_Last_Place;
int  iRow_First;
int  iRow_Last;
int  iLine_First;
int  iLine_Last;
int  iPlace_first;
int  iPlace_last;
uint Video_Place_flag;
uint uiSpeed;
uint uiflag;
uint uiOdd;
uint uiSpeed_Flag;
uint uiSpeed_Tbl[900];
ST_PID pst_pid;
/********PWM Initial***********/
void PWM_init(void)
{   PWMCTL = 0X80;
PWMPOL = 0X02;	  
PWMCAE = 0x00;    
PWMCLK = 0X00;    
PWMPRCLK = 0x36;
PWMSCLA = 0X00;  
PWMSCLB = 0X00;  
PWMPER67 = 30000;     
PWMDTY67 = 6600;	 
PWMPER0 = 200;       
PWMPER1 = 200;
PWMDTY0 = 0;		
PWMDTY1 = 0;
    PWME = 0X83;			
}
/**************AD Initial ****************/
void AD_init(void)    
{
    ATD0CTL2=0xC0; 
    ATD0CTL3=0x08; 
    ATD0CTL4=0x81; 
    ATD0CTL5=0xA0; 
    ATD0DIEN=0x00; 
}
/**************CCD Initial ****************/
void Video_Init(void)
{     uint i, j;										
  	  HPRIO = (unsigned char)(Virq & 0xff);
  	  INTCR |= INTCR_IRQE_MASK;								
	  PIEJ = 0x02;													
	  PPSJ = 0X02;                            
	  INTCR_IRQEN = 0;						           
	  uiVideo_line_count = 0;
	  DDRA = 0X00;
	  DDRB = 0XFF;
	  for(i = 0; i < 20; i++)
	  {      for(j = 0; j < 6; j++)
	      {    uiVideo_Center[i][j] = 255;  
	      }
	      uiVideo_Continuous_Line[i] = 255;
	  }
/**********Get Image Information **********/
void Video_Get_Image(void)
{if(1 == uiVideo_line_flag)
    { Video_transform_Line();
        uiVideo_line_flag = 0;       
 }}
void Video_transform_Line(void)
{   uint i, j;
    uint uiVideo_flag;
    uint uiVideo_count;
    uint uiVideo_count1;
    uint uiVideo_count2;
    uiVideo_flag = 0;
    uiVideo_count = 0;
    uiVideo_count1 = 0;
    uiVideo_count2 = 0;
    uiVideo_Center_Count = 0;
    for(i = 0; i < 32; i++)
    {
        if(uiVideo_Ava_Limen[uiVideo_line] > uiVideo_Data[uiVideo_line][i])
        {uiVideo_AD_Data[uiVideo_line][i] = 0; } 
        else
        {uiVideo_AD_Data[uiVideo_line][i] = 1; }
    }
    for(i = 0; i < 32; i++)
    {
        if((uiVideo_AD_Data[uiVideo_line][i] == 0) &&(uiVideo_flag == 0))
        {       uiVideo_flag = 1;
        		uiVideo_count1 = i;
        		if(31 == i)
        		{
        		    uiVideo_Center[uiVideo_line][uiVideo_Center_Count] = 31; 
        		}} 
        else if((uiVideo_AD_Data[uiVideo_line][i] == 1) &&(uiVideo_flag == 1))
        {
            uiVideo_flag = 0;
            uiVideo_count = 0;
            uiVideo_count2 = i - uiVideo_count1 ;
            for(j = uiVideo_count1; j < i; j++)
            { uiVideo_count = j + uiVideo_count;    }
            uiVideo_Center[uiVideo_line][uiVideo_Center_Count] = uiVideo_count / uiVideo_count2;
            uiVideo_Center_Count++;
        } 
        else if((uiVideo_AD_Data[uiVideo_line][31] == 0) && (uiVideo_flag == 1))
        {   uiVideo_flag = 0;
            uiVideo_count = 0;
            uiVideo_count2 = 32 - uiVideo_count1 ;
            for(j = uiVideo_count1; j < 32; j++)
            {
            		uiVideo_count = j + uiVideo_count;    
            }
            uiVideo_Center[uiVideo_line][uiVideo_Center_Count] = uiVideo_count / uiVideo_count2;
            uiVideo_Center_Count++;
        } 
        else if((uiVideo_AD_Data[uiVideo_line][i] == 1) &&(uiVideo_flag == 0)) 
        {if(31 == i)
            {uiVideo_Center[uiVideo_line][uiVideo_Center_Count] = 255;
            }}}
    if(uiVideo_line > 18) 
    {   uiVideo_over_flag = 1;			
        uiVideo_line = 0; }
}/*----------Process  Image-------------------*/
void Video_Process_Image(void)
{   int i, j,k;
    uint uiLine;
    uint uiTemp[6];
    uint uicount;
    uint uiVideo_Continuous_Last;
    uiLine = 0;
    uicount = 0;
    uiVideo_Continuous_Last = 0;
    for(i = 0; i<6; i++)
    {
        uiTemp[i] = 0 ;  
    }
    for(i = 19; i > 0; i--)
    {
        if(uiVideo_Center[i][0] != 255)
        {
            uiVideo_Continuous_Line[uiVideo_Continuous_Count] = uiVideo_Center[i][0];  
            uiVideo_Continuous_Count++;
            uiLine = i - 1;
            i = 1;  
        } 
        else
        {  uiVideo_Continuous_Line[uiVideo_Continuous_Count] = 255;
        		uiVideo_Continuous_Count++;}
    }
    for(i = uiLine; i >= 0; i--)
    {
        if(uiVideo_Center[i][0] != 255)
        {
            for(j = 0; j < 6; j++)
            {if(uiVideo_Center[i][j] != 255)
                {uicount = j + 1;
                }
            }
            for(j = 0; j < uicount; j++)
            {for(k = uiVideo_Continuous_Count-1; k < 20; k++)
                {if(uiVideo_Continuous_Line[k] != 255)
                    {uiVideo_Continuous_Last = uiVideo_Continuous_Line[k];
                    		k = 20; } }
                if(uiVideo_Center[i][j] > uiVideo_Continuous_Last)
                {if((uiVideo_Center[i][j] - uiVideo_Continuous_Last)<4) 
{uiVideo_Continuous_Line[uiVideo_Continuous_Count]=uiVideo_Center[i][j];
                        j = uicount;
                        uiVideo_Continuous_Count++;    } 
                    else if(j == (uicount - 1) && ((uiVideo_Center[i][j] - uiVideo_Continuous_Last)>=4))
                    { uiVideo_Continuous_Line[uiVideo_Continuous_Count] = 255;
                        uiVideo_Continuous_Count++;} } 
                else
                {if((uiVideo_Continuous_Last - uiVideo_Center[i][j])<4) 
{uiVideo_Continuous_Line[uiVideo_Continuous_Count]=uiVideo_Center[i][j];
                        j = uicount;
                        uiVideo_Continuous_Count++;  }
else if(j == (uicount-1)&&((uiVideo_Center[i][j] - uiVideo_Continuous_Last)>=4))
                    {
                       uiVideo_Continuous_Line[uiVideo_Continuous_Count] = 255;
                        uiVideo_Continuous_Count++;
                    }}}}
                else
        {
            uiVideo_Continuous_Line[uiVideo_Continuous_Count] = 255;
            uiVideo_Continuous_Count++;
        }}}
/*-----------------------Control--------------*/
void Video_Control_Angle(void)
{	int i;
    int iRow_First;
    int iRow_Last;
    int iLine_First;
    int iLine_last;
    for(i = 0; i < 20; i++)
    {  if(uiVideo_Continuous_Line[i] != 255)
        {   iRow_First = i;  
            iLine_First = uiVideo_Continuous_Line[i] - 16;  
            i = 20;
        } }
    for(i = 19; i >= 0; i--)
    {
        if(uiVideo_Continuous_Line[i] != 255)
        {       iRow_Last = i;
        		iLine_last = uiVideo_Continuous_Line[i] - 16;
        		i = -1; } }
    
    PWMDTY67 = 6450 - (iLine_First*38 + ((iLine_last - iLine_First) * 500 /(iRow_Last - iRow_First))) ;
}/**************************************************/
// Main Function
void main(void)
{   
    AD_init();
    PWM_init();
Video_Init();
for(;;){
Video_Get_Image(); 
Video_Process_Image();
Video_Control_Angle();
}