当前位置：文档之家› 智能车程序

智能车程序

#include /* common defines and macros */
#include "derivative.h" /* derivative-specific definitions */
#include
#include
#include
#pragma LINK_INFO DERIVATIVE "mc9s12xs128"
float Kp=3;
float Ki=0.8;
float Kd=0;
int MID1;
int changzhi_speed;//长直道速度
int changwan_speed;//长弯道速度
int zhidao_speed;
int wandao_speed;
int chongchu_speed;
int xiaceng_speed;//下层激光巡线时的速度
unsigned int Object_speed;//电机理想速度
unsigned int pulse_speed;//电机当前速度
int error=0;//理想速度与当前速度的差值
int pre_error=0;//PID控制的速度差值
int pre_d_error=0;//PID控制的速度上一次差值
signed int d_error,dd_error;
int pk=0;//速度的PID值
int atd_value[15]; //远距离激光数组
int add_value[8];//近距离激光数组
int dweight;
int tweight;
int vweight;//黑线个数
int Sensor;//舵机转角
int po_hei;//未处理前黑线的个数
int tdweight;
int tddweight;//用于下层激光巡线
int tvd_weight;//下层黑线个数
int i;
int last_turn;//用于上排激光
int last_turn1;//用于下排激光
int count=0;//用于上下排激光转换
int count1=0;//用于长直道统计
int count2=0;//用于长弯道统计
int DEG;
int DEG1=20;
int DEG2=23;
int DEG3=10;
int DEG4=10;
unsigned char flag;//上下排激光切换的标志位
unsigned char flag1;//长直道标志位
int chuangkou;//窗口处理，用于滤波
int j,m,k,l;//起跑线识别用
int exit,exit_flag,stop_times=2;//停车用变量
unsigned char stop_stop=0xff,stop_stop1=0xff;//上坡不停用变量
void delay()

{
int i,j;
for(i=40;i>0;i--)
for(j=20;j>0;j--);
}
void delay2s() //延时2秒发车

{
int i,j;
for(i=5000;i>0;i--)
for(j=1700;j>0;j--);
}
void PIT_Init(void) //定时10ms中断初始化函数

{
PITCFLMT_PITE=0;//定时中断通道0关
PITCE_PCE0=1;//定时器通道0使能
PITMTLD0=240-1;//8位定时器初值设定。240分频
PITMUX=0x00;
PITLD0=1000-1;//16位定时器初值设定
PITINTE_PINTE0=1;//定时器中断通道0中断使能
PITCFLMT_PITE=1;//定时器通道0使能
}
void PLL_Init(void) //锁相环初始化，总线24M
{
CLKSEL=0x00;
PLLCTL=0xe1;
REFDV=1;
SYNR=2;
PLLCTL=0x60;
asm NOP;
asm NOP;
asm NOP;
while((CRGFLG&0x08)==0);
CLKSEL=0x80;
}
void TIM_Init(void)//脉冲累计初始化

{
PACTL=0x50;//下降沿触发
PACNT=0x0000;//清0
}
void Servo_Init(void) //舵机PWM初始化

{
PWME_PWME1=0;//关PWM1通道
PWMPRCLK=0x33;//A=B=24M/8=3M
PWMSCLA=1;//SA=3M/2=1.5M
PWMSCLB=1;//SB=3M/2=1.5M
PWMCLK_PCLK1=1;//SA作为时钟源
PWMCTL_CON01=1;//0,1级联
PWMPOL_PPOL1=1;//输出先高后低
PWMCAE_CAE1=0;//通道1左对齐
PWMCNT01=0;//计数器自动清零
PWMPER01=30000;//周期为20ms

PWMDTY01=2180;//高电平1.5ms，范围1675-2675
PWME_PWME1=1;//开PWM1通道
}
void Forward_Init(void)//电机PWM初始化

{
PWME_PWME3=0;
PWME_PWME5=0;
PWMCTL_CON23=1;//2,3级联
PWMCTL_CON45=1;//4,5级联
PWMPOL_PPOL3=1;
PWMPOL_PPOL5=1;
PWMCLK_PCLK3=1;//SB作为时钟源
PWMCLK_PCLK5=1;//SA作为时钟源
PWMPOL_PPOL3=1;
PWMPOL_PPOL5=1;
PWMCLK_PCLK3=1;
PWMCLK_PCLK5=1;
PWMCAE_CAE3=0;
PWMCAE_CAE5=0;
PWMCNT23=0;
PWMCNT45=0;
PWMPER23=750;//电机频率为2KHZ
PWMPER45=750;
PWMDTY23=0;//占空比为30%
PWMDTY45=0;
PWME_PWME3=1;
PWME_PWME5=1;
}
void Io_Init(void)

{
//DDRA=0x00;//A口作为输入口，用于检测路径
DDRA_DDRA1=0;
DDRA_DDRA2=0;
DDRA_DDRA3=0;
DDRA_DDRA4=0;
DDRA_DDRA5=0;
DDRA_DDRA6=0;
DDRA_DDRA7=0;
DDRB=0xff;//B口作为输出口，控制激光管分时发光
DDRM=0xff;//M口作为输出口，用于指示，调试用
ECLKCTL_NECLK=1;
DDRE=0x00;//E口作为输入口，用于拨码开关
}
void check_start(void)
{
/**************起跑线******************************/
for(i=0;i<2;i++)
{
if(add_value[i]==1)
{
for(j=i;j<3;j++)
{
if(add_value[j]==0)
{
for(k=j;k<5;k++)
{
if(add_value[k]==1)
{
for(l=k;l<6;l++)
{
if(add_value[l]==0)
{
for(m=l;m<8;m++)
{
if(add_value[m]==1)
exit_flag=1;
}
}
}
}
}
}
}
}
}
if(exit_flag==1)
{
exit_flag=0;
stop_stop1++;
}
else if(exit_flag==0)

{

if(stop_stop1!=0xff)

{
stop_stop=stop_stop1;
stop_stop1=0xff;
}
}
if(stop_stop<=100&&stop_stop>=1)

{
stop_stop=0xff;
exit++;
if(exit>stop_times)
exit=stop_times;
}
}
void shache(void)

{
//PTM_PTM5=1;//左边红灯亮
PWMDTY23=0;
PWMDTY45=150;
while(pulse_speed>10);
// PTM_PTM5=0;//红灯灭
}
void stop()

{
//PTM_PTM3=1;//右边红灯亮
shache();
PWME_PWME3=0;
PWME_PWME5=0;
}
void PID(void)

{
error=Object_speed-pulse_speed; //理想速度与实际速度之差
d_error=error-pre_error;//当前速度差与上一次速度差之差
dd_error=d_error-pre_d_

error;
pre_error=error;//存储当前偏差
pre_d_error=d_error;
pk+=(int)(Kp*d_error+Ki*error+Kd*dd_error);
if(pk<=0)
pk=0;
else if(pk>=750)
pk=750;
}
void driver(void)

{
if(exit==2)

{
exit=0;
//PTM_PTM3=1;//右红灯亮
stop();

}
PWMDTY23=pk;
PWMDTY45=0;
}
void Tiaosu(void)//车速调节

{
PID();
}
void saomiao(void)

{
PORTB=0;
delay();
if(PORTE_PE3==1)
atd_value[0]=0;
else
{
atd_value[0]=1;
}
if(PORTA_PA2==1)
atd_value[6]=0;
else
{
atd_value[6]=1;
}
if(PORTA_PA5==1)
add_value[0]=0;
else
{
add_value[0]=1;
}
PORTB=3;
delay();
if(PORTA_PA1==1)
atd_value[3]=0;
else
{
atd_value[3]=1;
}
if(PORTA_PA3==1)
atd_value[9]=0;
else
{
atd_value[9]=1;
}
if(PORTA_PA6==1)
add_value[3]=0;
else
{
add_value[3]=1;
}
PORTB=1;
delay();
if(PORTE_PE3==1)
atd_value[1]=0;
else
{
atd_value[1]=1;
}
if(PORTA_PA4==1)
atd_value[12]=0;
else
{
atd_value[12]=1;
}
if(PORTA_PA7==1)
add_value[5]=0;
else
{
add_value[5]=1;
}
PORTB=4;
delay();
if(PORTA_PA1==1)
atd_value[4]=0;
else
{
atd_value[4]=1;
}
if(PORTA_PA3==1)
atd_value[10]=0;
else
{
atd_value[10]=1;
}
if(PORTA_PA5==1)
add_value[1]=0;
else
{
add_value[1]=1;
}
PORTB=6;
delay();
if(PORTA_PA2==1)
atd_value[7]=0;
else
{
atd_value[7]=1;
}
if(PORTA_PA4==1)
atd_value[13]=0;
else
{
atd_value[13]=1;
}
if(PORTA_PA6==1)
add_value[4]=0;
else
{
add_value[4]=1;
}
PORTB=2;
delay();
if(PORTE_PE3==1)
atd_value[2]=0;
else
{
atd_value[2]=1;
}
if(PORTA_PA3==1)
atd_value[11]=0;
else
{
atd_value[11]=1;

}
if(PORTA_PA7==1)
add_value[6]=0;
else
{
add_value[6]=1;
}
PORTB=7;
delay();
if(PORTA_PA2==1)
atd_value[8]=0;
else
{
atd_value[8]=1;
}
if(PORTA_PA4==1)
atd_value[14]=0;
else
{
atd_value[14]=1;
}
if(PORTA_PA5==1)
add_value[2]=0;
else
{
add_value[2]=1;
}
PORTB=5;
delay();
if(PORTA_PA1==1)
atd_value[5]=0;
else
{
atd_value[5]=1;
}
if(PORTA_PA7==1)
add_value[7]=0;
else
{
add_value[7]=1;
}
}
void chuli(void)

{
check_start();//起跑线检测
/***************窗口处理************************/
for(i=0;i<15;i++)

{
if((ichuangkou+8))
atd_value[i]=0;
}
/* if(dweight==13||dweight==12)
{
for(i=7;i<15;i++)
atd_value[i]=0;
}
if(dweight==-13||dweight==-12)
{
for(i=0;i<8;i++)
atd_value[i]=0;
} */
for(i=0;

i<15;i++)

{
if(atd_value[i]==1)
po_hei++;
}
if(po_hei>4) //有5个点为黑时，启用下层激光

{
flag=1; //下层激光管巡线标志位
chuangkou=8;
PTM_PTM4=1;//绿灯亮
}
if(flag==1) //下层激光巡线

{
for(i=0;i<8;i++)
{
if(add_value[i]==1)
tvd_weight++;
}
if(tvd_weight==0||tvd_weight>2)

{
tddweight=last_turn1;
}
else
{
tdweight=add_value[0]*7+add_value[1]*5+add_value[2]*3+add_value[3]+
add_value[4]*(-1)+add_value[5]*(-3)+add_value[6]*(-5)+add_value[7]*(-7);
tddweight=tdweight/tvd_weight;
}
Sensor=2180+(signed int)tddweight*30;
if(Sensor>2390)
Sensor=2390;
if(Sensor<1970)
Sensor=1970;
PWMDTY01=Sensor;
//Object_speed=xiaceng_speed;
po_hei=0;//黑线计数清零
tvd_weight=0;//下层计数清零
last_turn1=tddweight;
}


/***************统计***********************/
for(i=0;i<15;i++)

{
if(atd_value[i]==1)
vweight++; //检测到的个数
}
if(vweight>0&&vweight<3)

{
count++;
if(count>=25)//连续有25个点是证明十字路口已过，启用上层激光
{ flag=0;//下层激光转为上层激光
PTM_PTM4=0;//绿灯灭
}
}
else
count=0;
if(flag==0) //上层激光巡线

{
if(vweight==0||vweight>3) //空白时保持
{ dweight=last_turn;
if(dweight>0)
chuangkou=(13-dweight)/2;
else if(dweight<0)
chuangkou=(15-dweight)/2;
else
chuangkou=(14-dweight)/2;
po_hei=0;
}
else
{
tweight=atd_value[0]*(13)+atd_value[1]*(11)+atd_value[2]*(9)+
atd_value[3]*(7)+atd_value[4]*(5)+atd_value[5]*(3)+atd_value[6]+
atd_value[7]*(0)+atd_value[8]*(-1)+atd_value[9]*(-3)+atd_value[10]*(-5)+
atd_value[11]*(-7)+atd_value[12]*(-9)+atd_value[13]*(-11)+atd_value[14]*(-13);
dweight=tweight/vweight;
last_turn=dweight;
if(dweight>0)
chuangkou=(13-dweight)/2;
else if(dweight<0)
chuangkou=(15-dweight)/2;
else
chuangkou=(14-dweight)/2;
po_hei=0;
}
/****************处理***********************/
if(abs(dweight)<3)

{
//Object_speed=zhidao_speed;
DEG=DEG1;
MID1=2180;
}
else if(abs(dweight)<9)

{
//Object_speed=wandao_speed;
DEG=DEG2;
if(dweight<0)
MID1=2164;
else
MID1=2196;
}
else if(abs(dweight)<13)

{
//Object_speed=wandao_speed;
DEG=DEG3;
if(dweight<0)
MID1=2060;
else
MID1=2300;
}
else
{


//Object_speed=chongchu_speed;
DEG=DEG4;
if(dweight<0)
MID1=2060;
else
MID1=2300;
}
/*if(abs(dweight)<3)
count1++;
else
count1=0;
if(count1>=50)
{
Object_speed=changzhi_speed;
PTM_PTM0=1;//第一个红灯亮
} else
PTM_PTM0=0;//第一个红灯灭
if(abs(dweight)>2&&abs(dweight)<10)
count2++;
else
count2=0;
if(count2>70) {
PTM_PTM1=1;//第二个红灯亮
Object_speed=changwan_speed;
}
else
PTM_PTM1=0;//第二个红灯灭 */
Sensor=MID1+(signed int)dweight*DEG;
if(Sensor>2430)
Sensor=2430;
if(Sensor<1930)
Sensor=1930;
PWMDTY01=Sensor;
}
vweight=0;
}
void main(void) {
/* put your own code here */
PLL_Init();
PIT_Init();//定时10ms初始化
TIM_Init();//脉冲累计初始化
Servo_Init();//舵机16位PWM控制
Forward_Init();//电机初始化
Io_Init();//IO口初始化
//changzhi_speed=27;
//changwan_speed=27;
//zhidao_speed=27;
//wandao_speed=27;
//chongchu_speed=27;
//xiaceng_speed=27;
//PTM_PTM0=0;//右边红灯灭
//PTM_PTM1=0;//右边红灯灭
//PTM_PTM2=0;//右边红灯灭
//PTM_PTM3=0;//右边红灯灭
//PTM_PTM4=0;//右边红灯灭
//PTM_PTM5=0;//右边红灯灭
//PTM_PTM6=0;//绿灯灭
//PTM_PTM7=0;//左边红灯灭
switch(PORTE&0x14)

{
case 0x14:{
Object_speed=22;
break;
}
case 0x04:{
Object_speed=26;
break;
}
case 0x10:{
Object_speed=30;
break;
}
case 0x00:{
Object_speed=35;
break;
}
default:break;
}
delay2s();//延时2秒发车
EnableInterrupts;

for(;;) {
_FEED_COP(); /* feeds the dog */
saomiao();
chuli();
Tiaosu();
driver();
} /* loop forever */
/* please make sure that you never leave main */
}
/***********************测速***********************/
#pragma CODE_SEG NON_BANKED
void interrupt 66 PIT0(void)

{
PITTF_PTF0=1;
pulse_speed=PACNT;
PACNT=0x0000;

}
#pragma CODE_SEG DEFAULT

相关主题
文本预览
相关文档 最新文档