第五界飞思卡尔智能车大赛程序(光电组)

我是光电组的，下面是程序，小车可以跑起来，但速度有待提升
#include /* common defines and macros */
#include /* derivative information */
#pragma LINK_INFO DERIVATIVE "mc9s12xs128"
//static unsigned char direction_turn[12]={888,1098,1208,1328,1481,1612,1730,1856,1988,2100,2222,2368};
//Chapter 12
//Periodic Interrupt Timer (S12PIT24B4CV1) Page349
//The PIT module has no external pins.
//PIT 模式没有外部引脚
unsigned char light=0; //激光管检测标志
unsigned short turn_value=0; //转向的PWM数值
unsigned short direction_turn[7]={333,430,560,647,705,780,888}; //转向给定值初始化
short speed_set[7]={250,300,350,400,350,300,250}; //速度给定值
short speed_flag=0; //速度档位标志位
short speed[3]={0,0,0}; //速度检测函数
short pulse_count=0; //编码器脉冲计数值
short speed_expect=0; //理想速度
short kp=2; //比例环节
short ki=0; //积分环节
short kd=1; //微分环节
short ek1=0; //误差1
short ek2=0; //误差2
short ek3=0; //误差3
short speed_add=0; //速度增量
=====================================================================void PLL_Init() //时钟初始化
{
REFDV=0x81; /* PLLCLK=2*OSCCLK*(SYNR+1)/(REFDV+1)*/
SYNR=2; /* 锁相环时钟=2*16*(2+1)/(1+1)=48MHZ */
while(!(CRGFLG&0x08)); /* 总线时钟=48/2=24MHZ */
CLKSEL=0x80;
}
void PWM_Init() //PWM初始化
{
PWME=0x00; //关闭PWM使能
PWMPRCLK=0x66; //A,B时钟均为总线的64分频,375KHZ
//PWMSCLA=0x01; //clockSA=clockA/(2*PWMSCLA) = 1500KHZ
//PWMSCLB=0X01; //clockSB=clockB/(2*PWMSCLB) =1500KHZ
PWMCLK=0x00;
PWMPOL=0xFF; //PWM输出起始电平为高电平
PWMCAE=0x00; //输出左对齐
PWMCTL=0xf0; //通道01，23，45，67级联
PWMPER01=5999; //舵机频率为62.5Hz
PWMDTY01=647; //占空比a=（PWMDTY01+1）/(PWMPER01+1)
PWMPER23=1000; //PWM通道3周期为375HZ
PWMDTY23=0; //占空比a=（PWMDTY01+1）/(PWMPER01+1)占空比50% ~~ 150
PWMPER45=1000; //PWM通道5周期为0.10ms 10KZH 300=0.00010/(1/3000000)
PWMDTY45=300; //占空比a=（PWMDTY01+1）/(PWMPER01+1)
PWMPER67=375; //频率为1000Hz
PWMDTY67=200; //
PWME=0xff; //使能pwm
}
void Pit0_Init() //PIT初始化
{
PITCFLMT_PITE=0; //关PIT使能
PITCE_PCE0=1; //通道0使能
PITMUX_PMUX0=0; //通道0接微时钟0
PITMTLD0=99; //微时钟0值设置为7f
PITLD0=3839; //time-out period = (PITMTLD + 1) * (PITLD + 1) / fBUS.
//时间计算 100*3840/24000000=0.016s
PITINTE_PINTE0=1; //通道0中断时能
PITCFLMT_PITE=1; //PIT使能
}
void ECT_Init()
{
TIOS=0x00; /* OC0路为输出比较，OC1路为输入捕捉 */
TSCR2=0x06; /* 定时器溢出中断禁止，计数器自由运行禁止复位，64分频 */
TSCR1=0x80; /* 定时器使能 */
TIE=0x01; /* 输出比较相应中断使能 */
TCTL4=0x01;
}
void dly_1ms()
{
int i,j;
for(i=0;i<200;i++)
{
for(j=0;j<1000;j++){;}
}
}
void sam_position() //车位检测函数
{
int i=0,j=0;
unsigned

char m=0,n=0;
n=PORTA;
for(i;i<10;i++)
{
m=PORTA;
if(n==m)
j++;
}
if(j>6)
light=n;
}
void check_start() //检测起始线
{
if((light&&4)||(light&&16))
start_flag++;
}
void check_start()
{
}
void turning() //舵机转向函数
{
switch(light)
{
case 1:if(turn_value==direction_turn[1]) //出界判断算法
{
turn_value=direction_turn[0];
speed_expect=speed_set[0];
}
else if(turn_value==direction_turn[0])
{
turn_value=direction_turn[0];
speed_expect=speed_set[0];
}
break;
case 2:if(turn_value==direction_turn[0])
{
turn_value=direction_turn[1];
speed_expect=speed_set[1];
}
else if(turn_value==direction_turn[1])
{
turn_value=direction_turn[1];
speed_expect=speed_set[1];
}
else if(turn_value==direction_turn[2])
{
turn_value=direction_turn[1];
speed_expect=speed_set[1];
}
break;
case 4:if(turn_value==direction_turn[1])
{
turn_value=direction_turn[2];
speed_expect=speed_set[2];
}
else if(turn_value==direction_turn[2])
{
turn_value=direction_turn[2];
speed_expect=speed_set[2];
}
else if(turn_value==direction_turn[3])
{
turn_value=direction_turn[2];
speed_expect=speed_set[2];
}
break;
case 8:if(turn_value==direction_turn[2])
{
turn_value=direction_turn[3];
speed_expect=speed_set[3];
}
else if(turn_value==direction_turn[3])
{
turn_value=direction_turn[3];
speed_expect=speed_set[3];
}
else if(turn_value==direction_turn[4])
{
turn_value=direction_turn[3];
speed_expect=speed_set[3];
}
break;
case 16:if(turn_value==direction_turn[3])
{
turn_value=direction_turn[4];
speed_expect=speed_set[4];
}
else if(turn_value==direction_turn[4])
{
turn_value=direction_turn[4];
speed_expect=speed_set[4];
}
else if(turn_value==direction_turn[5])
{
turn_value=direction_turn[4];
speed_expect=speed_set[4];
}
break;
case 32:if(turn_value==direction_turn[4])
{
turn_value=direction_turn[5];
speed_expect=speed_set[5];
}
else if(turn_value==direction_turn[5])
{
turn_value=direction_turn[5];
speed_expect=speed_set[5];
}
else if(turn_value==direction_turn[6])
{
turn_value=direction_turn[5];
speed_expect=speed_set[5];
}
break;
case 64:if(turn_value==direction_turn[5]) //出界判断算法
{
turn_value=direction_turn[6];
speed_expect=speed_set[6];
}
else if(turn_value==direction_turn[6])
{
turn_value=direction_turn[6];
speed_expect=speed_set[6];
}
break;
default:break;
}
PWMDTY01=turn_value;
}
void check_speed() //速度检测函数
{
ek3=ek2; //计算速度差值
ek2=ek1;
ek1=speed_expect-pulse_count;
speed[2]=speed[1]; //当前速度放在[0],之前放在[1],[2]
speed[1]=speed[0];
speed[0]=pulse_count;
pulse_count=0;
}
void speed_down() //制动函数
{
PWMDTY23=300; //电机反向供电
PWMDTY45=0;
}
void speed_pid() //PID算法
{
speed_add=kp*(ek1-ek2)+ki*ek1+kd*(ek1-2*ek2+ek3); //PID增量式
}
void driver() //驱动电机控制函数
{
//if(((turn_value>705)||(turn_value<560))&&(speed[0]>200)) //

当前速度若远超给定速度
// {
//speed_down();
// }
//else
// {
PWMDTY23=0;
speed_pid();
PWMDTY45=PWMDTY45+speed_add;
// }
if(PWMDTY45>600)
PWMDTY45=600;
}
void main()
{
DisableInterrupts; /* 关中断 */
PLL_Init();
PWM_Init();
Pit0_Init();
ECT_Init();
turn_value=direction_turn[3];
DDRA=0x00;
DDRB=0xFF;
PORTB=0X00;
EnableInterrupts;
for(;;)
{
//sam_position();
//turning();
}
}
#pragma CODE_SEG NON_BANKED
void interrupt 8 Timer0_ISR(void)
{
pulse_count++;
TFLG1_C0F=1; /* TC0端有中断产生 */
}
void interrupt 66 PIT0_ISR(void)
{
sam_position(); //车位检测函数
turning(); //舵机转向函数
check_speed(); //速度检测函数
driver(); // 驱动电机控制函数
PITTF_PTF0=1; /* PIT0端有中断产生,清除标志位 */
}