main.c

第三届飞思卡尔杯智能汽车大赛河北大学工商学院雷霆队

}
//*******舵机调整函数*******
void set_angle(int dutycycle)
{
    if(dutycycle<right_limit)
    {
        PWMDTY01=right_limit;
    } 
    else if(dutycycle>left_limit)
    {
        PWMDTY01=left_limit;
    }
    else
    {
        PWMDTY01=dutycycle;
    }
}
//舵机控制函数
//PID控制
void angle_control(void)
{
    int dutycycle,measure_data;
    measure_data=0;
    for(row=6;row<9;row++)
    {
        measure_data=measure_data+black_x[row];
    }
    measure_data=(char)(measure_data/3);
    dutycycle=center-PID_calculate(34,measure_data);
    set_angle(dutycycle);
}
/*********************************************************************\
**************************图像处理函数********************************
\*********************************************************************/
//*******图像获取函数*******
//*******获取行同步信号的中断******
interrupt 6 void IRQ_ISR()
{
    row_count++;//行计数加1
    if((row_count>ROW_START)&&(row_count%INTERVAL==0)&&(row_image<ROW_MAX))
    {//是所需要的行信号，AD转换后将结果存入一维数组中
        AD_init();//AD模块初始化
        for(line_sample=0;line_sample<LINE_MAX;line_sample++)
        {//LINE_MAX为1行中最大的有效点数
            while(!ATD0STAT1_CCF0); // 等待AD转换结束
            sample_data[line_sample]=SIGNAL_IN; //将结果存入一维数组sample_data[]中
        }
        ATD0CTL2=0x00;//关闭AD
        row_image++;
    }
    if((row_count>ROW_START)&&(row_count%INTERVAL==3)&&(row_image<ROW_MAX+1))
    {//当不是所需要的行信号，将一维数组存入二维数组中,充分利用时间
        for(line_temp=LINE_MIN;line_temp<LINE_MAX;line_temp++)
        {
            image_data[row_image-1][line_temp]=sample_data[line_temp];
        }
    }
}
 // 黑线提取算法
/*void get_black_wire(void) 
{
    unsigned char i;
    for(row=0;row<ROW_MAX;row++)
    {
        for(line=LINE_MIN;line<LINE_MAX-3;line++)
        {
            if(image_data[row][line]>image_data[row][line+3]+VALVE)
            {
                  for(i=3;i<10;i++)
                  {
                      if(image_data[row][line+i]+VALVE<image_data[row][line+i+3])
                      {
                          black_x[row]=line+i/2+2;
                          i=10;
                          line=LINE_MAX;
                      }
                  }
              } 
        }
    }
}*/ 
//跟踪边缘检测法
void get_black_wire(void)
{
    unsigned char i,center_x,left,right;
    row=0;
    for(line=LINE_MIN;line<LINE_MAX-3;line++)
    {
        if(image_data[row][line]>image_data[row][line+3]+VALVE)
        {
            for(i=3;i<10;i++)
            {
                if(image_data[row][line+i]+VALVE<image_data[row][line+i+3])
                {
                    black_x[row]=line+i/2+2;
                    i=10;
                    line=LINE_MAX;
                }
            }
        }
    }
    for(row=1;row<ROW_MAX;row++)
    {
        center_x=black_x[row-1];
        left=center_x;
        right=center_x;
        if(image_data[row][center_x]+VALVE<65)
        {
            for(line=center_x-7;line<LINE_MAX-3;line++)
            {
                if(image_data[row][line]>image_data[row][line+3]+VALVE)
                {
                    for(i=3;i<10;i++)
                    {
                        if(image_data[row][line+i]+VALVE<image_data[row][line+i+3])
                        {
                            black_x[row]=line+i/2+2;
                            i=10;
                            line=LINE_MAX;
                        }
                    }
                }
            }   
        } 
        else
        {
            while(left>LINE_MIN&&right<LINE_MAX)
            {
                left--;
                if(image_data[row][left]+VALVE<65)//右边界
                {
                    for(i=1;i<10;i++)
                    {
                        if(image_data[row][left-i]+VALVE<image_data[row][left-i-3])
                        {
                            black_x[row]=left-i/2-2;
                            i=10;
                            left=LINE_MAX;
                        }
                    }
                    break;
                }
                else
                {
                    right++;
                    if(image_data[row][right]+VALVE<65) //左边界
                    {
                        for(i=1;i<10;i++)
                        {
                            if(image_data[row][right+i]+VALVE<image_data[row][right+i+3])
                            {
                                black_x[row]=right+i/2+2;
                                i=10;
                                right=LINE_MIN;
                            }
                        }
                        break;
                    }
                }
            }
        }
    }
}
/***********************************************************************\
****************************PID算法**************************************
\***********************************************************************/
void PID_init (void)
{
    PID.Kd=3;
    PID.LastError=0;
    PID.PrevError=0; 
}
//SetPoint设定值,NextPoint测量值 
int PID_calculate(int SetPoint,int NextPoint) //增量式
{
    int P=0;
    int D=0;
    int dError,Error;
    Error=SetPoint-NextPoint;
    dError=PID.LastError-PID.PrevError;
    if(abs_sub(Error)<5) 
    {
        PID.Kp=10;
        if(Error>=0)
        {
            D=-2*PID.Kd*dError;
            PORTB=0x00;
        } 
        else
        {
            D=2*PID.Kd*dError;
            PORTB=0x00;
        }
    } 
    else if(abs_sub(Error)<10&&abs_sub(Error)>=5) 
    {
        PID.Kp=12;
        if(Error>=0)
        {
            D=-PID.Kd*dError;
            PORTB=0xEF; 
        } 
        else
        {
            D=PID.Kd*dError;
            PORTB=0xF7;
        }
    } 
    else if(abs_sub(Error)<15&&abs_sub(Error)>=10) 
    {
        PID.Kp=15;
        if(Error>=0)
        {
            PORTB=0xDF; 
        } 
        else
        {
            PORTB=0xFB; 
        }
    } 
    else if(abs_sub(Error)<18&&abs_sub(Error)>=15) 
    {
        PID.Kp=30;
        if(Error>=0)
        {
            PORTB=0xBF; 
        } 
        else
        {
            PORTB=0xFB; 
        }
    } 
    else 
    {
        PID.Kp=35;
        if(Error>=0)
        {
            PORTB=0x7F; 
        } 
        else
        {
            PORTB=0xFE; 
        }
    } 
    P=Error* PID.Kp;
    PID.PrevError = PID.LastError;
    PID.LastError = Error;
    return(P+D);    
}	
/***************************************************************************\
**********************主函数************************************
\***************************************************************************/
void run(void)
{
    start();                
    count=0;
    for(;;)
    {
        get_speed();
        current_speed=current_speed*2+40;
        if(OEFLAG==0)
        { //偶场->奇场
            while(OEFLAG==0);
            row_count=0;
            row_image=0;
            EnableInterrupts;
            while(row_count<ROW_END)
            {
                get_black_wire();
                if(speed_flag==1)
                {
                    speed_control_2(); 
                }                     
                else
                {
                    speed_control_1();
                }
                angle_control();
            }
            DisableInterrupts;
        }
        else
        { //奇场->偶场
            while(OEFLAG==1);
            row_count=0;
            row_image=0;
            EnableInterrupts;
            while(row_count<ROW_END)
            {
                get_black_wire();
                if(speed_flag==1)
                {
                    speed_control_2(); 
                }                     
                else
                {
                    speed_control_1();
                }
                angle_control();
            }
            DisableInterrupts;   
        }
        if(current_speed>speed_max)
        {
            set_speed(speed_min);
        }
        if(current_speed<speed_min)
        {
            set_speed(speed_max);
        }
        start_line();
        if(loop_count==1)
        {
            PID_init();
            if(loop_flag==0)
            {
                if(count==2)
                {
                    stop();
                }
                l_counter++;
                if(l_counter==11)
                {
                    l_counter=0;
                    loop_count=0;
                    count+=1;
                }
            }
            if(loop_flag==1)
            {
                if(count==1)
                {
                    stop();
                }
                l_counter++;
                if(l_counter==11)
                {
                    l_counter=0;
                    loop_count=0;
                    count+=1;
                }
            }
        }
    }
}
void main(void) 
{    
    PWME=0x00;
    Timer_init();
    PLL_init();
    PORT_init();
    ECT_init();
    PWM_init();
    PID_init();
    run();
}