main.c

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

/*********************************************\
*****************雷霆队智能车程序**************
author:renhaoran
date:2008.03.24--2008.07.22
\*********************************************/
#include <hidef.h>      /* common defines and macros */
#include <mc9s12dg128.h>     /* derivative information */
#pragma LINK_INFO DERIVATIVE "mc9s12dg128b"
/*****************************************************************\
***********************视频变量**************************
\*****************************************************************/
#define SIGNAL_IN ATD0DR0L // 视频信号：右端排列模型,只使用ATD转换的低8位结果寄存器
#define OEFLAG PTT_PTT2 // 奇偶场信号
#define LINE_MIN 12 // 每行的第一个有效点
#define LINE_MAX 77 //每行的采样点数
#define ROW_MAX 10 // 在每幅图片中需要采样的行数
#define ROW_START 50 // 开始从行首采样
#define ROW_END 250 // 采样结束标志
#define INTERVAL 15 // 有效行之间的时间间隔
#define VALVE 20 // 检测黑线轨迹或白线轨迹的阈值
//视频进程使用的变量
unsigned char black_x[ROW_MAX]; // 一维数组
unsigned char row; // 数组中x坐标
unsigned char line; // 数组中y坐标
unsigned int row_count; // 行坐标
unsigned char line_sample; // 在AD中做计数器
unsigned char row_image;//行图像
unsigned char line_temp; // 用于数据传送的临时变量
unsigned char sample_data[LINE_MAX]; // 用于存储进入进入中断的一维数组
volatile unsigned char image_data[ROW_MAX][LINE_MAX]; // 图片数据数组
/*******************************************************************\
********************速度转角变量***************************
\*******************************************************************/
#define left_limit 3250     
#define center 2950 
#define right_limit 2650
//速度变量,最低速,最高速,当前速度,设定速度
#define low_speed 100
#define speed_max 160
#define speed_min 110
unsigned char current_speed,speed,high_speed,curve_speed;
unsigned char speed_flag=0;
unsigned char pulse[5];
unsigned char curvature;
unsigned char s_counter;//速度计数器
//圈数信息
unsigned char loop_count=0;
unsigned char count;
unsigned char l_counter=0;
unsigned char loop_flag=0;
/*******************************************************************\
********************PID变量**************************
\*******************************************************************/
struct CARSTATE
{
    int Kp;
    int Kd;
    int LastError; //上一次误差
    int PrevError; //上两次误差
}PID;
/******************************************************************\
函数声明
\*****************************************************************/
int PID_calculate(int SetPoint,int NextPoint);
void get_black_wire(void);
void angle_control(void);
/*******************************************************************\
**********************初始化*************************
\*******************************************************************/
//********PORT初始化******
/*********************************\
******端口分配*******
A口---波码开关
B口---LED
PWM01---舵机PWM
PWM2---MC33886PWM
PJ0---MC33886控制D1
PJ1---MC33886FS~
PT0---行同步信号
PT1---速度传感器信号
PT2---奇偶场信号
PAD0---摄像头视频信号
\*********************************/
void PORT_init(void)
{
    DDRA=0x00;//A口作为输入
    //LED初始化,输出
    DDRB=0xFF;
    PORTB=0xFF;//灭
    //行同步信号,输入
    DDRT_DDRT0=0;
    //奇偶场信号,输入
    DDRT_DDRT2=0;
    //33886控制信号D1,输出
    DDRJ_DDRJ0=1;
    //33886错误标志信号FS,输入
    DDRJ_DDRJ1=0;
    //行同步信号,外部中断初始化
    INTCR_IRQE=1; // IRQ只选择敏感边沿
    INTCR_IRQEN=1; //外部IRQ使能
}
//********PLL初始化*******
//初始化总线时钟为32MHz
//bus period=16Mhz*(SYNR+1)/(REFDV+1)
void PLL_init(void)
{
    REFDV=3;
    SYNR=7; 
    while(0==CRGFLG_LOCK); // wait for VCO to stablize
    CLKSEL=0x80;// open PLL
}
//*******AD初始化********
void AD_init(void)
{
    // 开启AD, 清除AD转换完成标志, 等待模式继续运行,下降沿触发,关闭外部触发,AD序列完成中断请求关闭,没有AD中断发生
    ATD0CTL2=0xC0;
    // 一个序列一个转换, No FIFO, 冻结模式继续转换
    ATD0CTL3=0x08;
    // 8位精度, 2个AD转换时钟周期, ATDClock=[BusClock*0.5]/[PRS+1] ;ATDClock=8MHZ
    ATD0CTL4=0x81;
    // 8-bit / right justified / unsigned - bits 0-7,扫描模式,只有一个通道采样,模拟输入通道AN0
    ATD0CTL5=0xA0; 
    ATD0DIEN=0x00; // inhibit digital input
}
//********ECT初始化********
//PT0---行同步信号
//PT1---场同步信号
//PT2---奇偶场信号
//PT3---速度传感器信号
void ECT_init(void)
{
    TIOS_IOS1=0; // 设置PT1为输入捕捉
    TCTL4=0b00001100; //设置PT1为任意边沿触发
    ICOVW_NOVW1=1;
    ICPAR_PA1EN=1; // PA1开启
}
//********PWM初始化********
//初始化舵机，但没有初始化马达
void PWM_init(void)
{
    //PJ0接D1口，0：33886开启，1：关闭
    //PJ1接FS口，错误标志位
    PTJ_PTJ0=1; //33886开启
    PWME=0x00; //PWM通道关闭
    PWMCTL_CON01=1; //通道0、1合并为16位通道
    PWMPRCLK=0x33; //时钟A=B=32MHz/8=4MHz
    PWMSCLA=1; //CLOCK SA=CLOCK A/(2*PWMSCLA)=2MHz
    PWMSCLB=1; //CLOCK SB=CLOCK B/(2*PWMSCLB)=2MHz
    PWMCLK=0b00000111; // PWM01使用时钟SA; PWM2使用时钟SB
    PWMPOL=0xff; //所有PWM通道在一个周期开始输出高电平
    PWMCAE=0x00; //所有PWM通道左侧排列输出
    //****************************需要测量一份舵机参数************************************
    //周期脉宽20ms;1ms<占空比脉宽<2ms;静止时1.5ms
    PWMPER01=40000;//设置周期40000;PWM1输出Frequency=SA/40000=50Hz
    PWMDTY01=center;//0°
    PWME_PWME1=1;
    //****************************需要测量一份电机参数************************************
    //周期200,占空比范围0--200
    PWMDTY2=0;
    PWMPER2=200; // 设置周期Frequency=SB/200=10K
}
/*******************************************************************************\
***************************函数声明****************************
\*******************************************************************************/
//*******定时计数器初始化*******
void Timer_init(void)
{
    TSCR2_PR=7;  //prescale factor is 8, bus clock/128=8Mhz/8
    TSCR1_TEN=1;  //timer enable
}
void delay(void)
{
    while (TCNT!=0x0000);
    while (TCNT==0x0000);
}
int abs_sub(int number) //取绝对值
{
    if(number<0)
    {
        number=-number;
    }
    return(number);
} 
unsigned char sub_sub(unsigned char number1,unsigned char number2)
{ //abs子程序，取差值	
    unsigned char difference;
    if(number1>=number2)
    {
        difference=number1-number2;
    }
    else
    {
        difference=number2-number1;
    }
    return(difference);
}
/**********************************************************************\
***********************************特殊算法****************************
\**********************************************************************/
void start_line(void)
{
    for(row=0;row<ROW_MAX;row++)
    {
        for(line=LINE_MIN;line<LINE_MAX-3;line++)
        {
            if(image_data[row][line]+VALVE<image_data[row][line+3])//黑线到白线
            {
                for(line=line+3;line<LINE_MAX-3;line++)
                {
                    if(image_data[row][line]>image_data[row][line+3]+VALVE)//白线到黑线
                    {
                        for(line=line+3;line<LINE_MAX-3;line++)
                        {
                            if(image_data[row][line]+VALVE<image_data[row][line+3])//黑线到白线
                            {
                                for(line=line+3;line<LINE_MAX-3;line++)
                                {
                                    if(image_data[row][line]>image_data[row][line+3]+VALVE)//白线到黑线
                                    {
                                        loop_count=1;
                                        line=LINE_MAX;
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
    } 
}
//******启动赛车函数********
void start(void)
{
    PORTB=0x00;
    delay();
    PORTB=0xff;
    PWME_PWME2=1;
    PTJ_PTJ0=0;
    /*PWMDTY2=20;
    delay();
    PWMDTY2=40;
    delay();
    PWMDTY2=80;
    delay();
    PWMDTY2=160;
    delay();*/
    high_speed=130+(PORTA_BIT6*10+PORTA_BIT5*10+PORTA_BIT4*10+PORTA_BIT3*10+PORTA_BIT2*10+PORTA_BIT1*5+PORTA_BIT0*5);
    curve_speed=180-(PORTA_BIT2*10+PORTA_BIT1*5+PORTA_BIT0*5);
    if(PORTA_BIT7==1)
    {
        loop_flag=1;
    }
}
//******停车处理函数********
//快到终点时加速
void stop(void)
{
    PWMDTY2=0;
    PWME_PWME2=0;
    //33886关闭
    PTJ_PTJ0=1; 
}
/*****************************************************************\
****************************舵机和电机控制函数********************
\*****************************************************************/
//******速度调整函数*******
void get_speed(void)
{
    unsigned char speed_temp;
    s_counter++;
    speed_temp=PACN1;
    current_speed=speed_temp-pulse[s_counter-1];
    pulse[s_counter-1]=speed_temp;
    if(s_counter==5)
    {
        s_counter=0;
    }
}
void set_speed(unsigned char desired_speed)
{
    
    if(desired_speed<current_speed)
    {
        PWMDTY2=desired_speed;
    }
    else
    {
        PWMDTY2=high_speed;
    }
}
//速度控制函数
//当遇到弯道时提前减速，然后使用speed_control_2控制弯道速度，当出弯时在使用speed_control_1
void speed_control_1(void)
{
    unsigned int sum;
    unsigned char average;
    sum=0;
    for(row=0;row<5;row++)
    {
        sum=sum+black_x[row];
    }
    average=(char)(sum/5);
    curvature=0;
    for(row=0;row<5;row++)
    {
        curvature=curvature+sub_sub(black_x[row],average);
        if(curvature>35)
        {
            set_speed(low_speed);
            speed_flag=1;
        }  
        else if(curvature>15&&curvature<=35)
        {
            set_speed(low_speed);
            if(current_speed>100)
            {
                set_speed(0);
            }
            speed_flag=0;
        } 
        else
        {
            set_speed(high_speed);
            if(current_speed>speed_max)
            {
                set_speed(speed_min);
            }
            speed_flag=0;
        }
    } 
}
void speed_control_2(void)
{
    unsigned int sum;
    unsigned char average;
    sum=0;
    for(row=5;row<10;row++)
    {
        sum=sum+black_x[row];
    }
    average=(char)(sum/5);
    curvature=0;
    for(row=5;row<10;row++)
    {
        curvature=curvature+sub_sub(black_x[row],average);
        if(curvature>10)
        {
            PWMDTY2=curve_speed;
            speed_flag=1;
        }  
        else
        {
            speed_flag=0;
        }
    }