当前位置:首页 >> 信息与通信 >>

飞思卡尔智能车电磁组 程序


#include <hidef.h> /* common defines and macros */
#include "derivative.h" /* derivative-specific definitions */
#include<mc9s12xs128.h>
#inc

lude<math.h>
#pragma LINK_INFO DERIVATIVE "MC9S12XS128"
#define border 15 //电感采集信号的边界值
#define border_back 160 //背后两个电感的边界值
float caiji_ad[9];//存储七路AD采集量,caiji_ad[0] caiji_ad[8]恒等于零
int a,b,c;
unsigned char table[16]={0x3F,0x06,0x5B,0x4f,0x66,0x6d,0x7d,0x07,0x7f,0x6f,0x77,0x7c,0x58,0x5e,0x79,0x71};
int angle_data,angle_pre; //舵机角度
unsigned int caiji_flag; //采集标志
char out;
char flag_pulse;
char outjudge_flag;
int check_stop;
int upp;
unsigned char cha;
unsigned int position_flag;
float position_q=0,position_qp=0,position_qd=0;
float position_h=0,position_hp=0,position_hd=0;
float position_jian;
float position_jia;
float position;
float position_cha,position_zhong;
float pwm1_p;
float pwm1_diff;
unsigned int jishi_flag;
float diff_q,diff_h;
float Speed_tar,Speed_cur,speed_p;
float pwmtemp,pre_pwmtemp;
float PID_P=200;
float PID_I=300;
float PID_D=0.1;
float error=0;
float error_min=-0.1;
float last_error=0;
float pre_error=0;
float banjing ;
void data_init()
{
unsigned int i;
for(i=1;i<7;i++)
{
caiji_ad[i]=0;
}
position_flag=0;
angle_data=2600;
position=0;
angle_pre=0;
diff_q=0;
diff_h=0;
out=0;
outjudge_flag=0;
pwm1_p=2600;
pwm1_diff=0;
jishi_flag=0;
DDRA=0xff;
DDRB=0x0f;
upp=0;
}

void SET_PLL()
{
CLKSEL=0X00;
PLLCTL=0Xe1;
SYNR=4; //设置总线频率为(SYNR+1)*8MHz
REFDV=1;
PLLCTL=0x60;//怎么设置?
asm NOP; ;//空操作,进行简单的延时,等待锁相环的响应
asm NOP;
asm NOP;
while ((CRGFLG&0x08)==0); //时钟校正同步 无执行语句?
CLKSEL=0x80;//使用PLLCLK oscclk是啥?
}

void initPIT()//定时中断初始化函数 5MS定时中断设置
{
PITCFLMT_PITE=0; //PIT功能关
PITCE_PCE0=1;//定时器通道0使能
PITMUX_PMUX0=0; //通道0
PITMTLD0=200;//200分频,在40MHzBusClock下,为200kHz。即//1/200ms=0.005ms
PITLD0=1000;//16位定时器初值设定。PITTIME*0.005MS =5ms //注意!!!可能影响舵机响应时间
PITINTE_PINTE0=1;//定时器中断通道0中断使能
PITCFLMT_PITE=1;//PIT功能开
}

void AD_Init()
{
ATD0CTL1=0x00; //7:1-外部触发,65:00-8位精度,4:放电,3210:ch
ATD0CTL2=0x40; //AFFC=1,对结果寄存器的访问将自动清除转换完成标志位
ATD0CTL3=0x80; //右对齐无符号,4个ad通道采样, No FIFO, Freeze模式下继续转
ATD0CTL4=0x01; //765:采样时间为4个AD时钟周期,ATDClock=[BusClock*0.5]/[PRS+1]
ATD0CTL5=0x30; //6:0特殊通道禁止,5:1连续转换 ,4:1多通道轮流采样
ATD0DIEN=0x00; //禁止数字输入
}

void IOC_Init()
{
PACTL=0X50;//PT7 PIN,PACN32 16BIT,ALLing edge,NOT INTERRUPT
TCTL3=0xc0;//c-输入捕捉7任何沿有效,
TIE =0x00;//每一位对应相应通道中断允许,0表示禁止中断
TIOS =0x00;//每一位对应通道的: 0输入捕捉,1输出比较
}

void pwm_init()
{
PWMPOL=0x02; //对应通道的极性,及联通到首先输出高电平
PWMCLK=0xff; //clock sa、SB做时钟源、
PWMPRCLK=0x00; //令时钟A为总线频
PWMCAE=0x00; //左对齐输出模式
PWMCTL=0x70; //01,23,45级联
PWMSCLA=10; //clockSA=ClockA/(2*PWMSCLA)
PWMSCLB=10; //clockSB=ClockB/(2*PWMSCLB)
PWMPER01=40000; //设定周期 ,周期是:时钟源周期*(PWMPER) 72hz
PWMPER23=200;
PWMPER45=200;
}

void SCI_Init(void)
{

SCI1BDH=0x01;
SCI1BDL=0x04; //SCI0BDL=busclk/(16*BT) 采用PLLCLK时钟,BUSCLK=PLLCLK/2=16Mhz
//BUSCLK=16MHz,(19200bps,SCI0BDL=0x34);(9600bps,SCI0BDL=0x68);
SCI1CR1=0x00;
SCI1CR2=0x2C; //接收和发送使能,允许接收中断
}

void SCI_SendByte(unsigned char ch)
{
while(!(SCI1SR1&0x80)); //等待 SCI0SR1.7=1 表示上一次发送完毕
SCI1DRL=ch;
}


unsigned int read_ad(unsigned int ch)
{
unsigned int ad=0;
while(!ATD0STAT0_SCF);
switch(ch) {
default:break;
case 1:
ad=ATD0DR8L;
break;
case 2:
ad=ATD0DR7L;
break;
case 3:
ad=ATD0DR6L;
break;
case 4:
ad=ATD0DR5L;
break;
case 5:
ad=ATD0DR4L;
break;
case 6:
ad=ATD0DR3L;
break;
case 7:
ad=ATD0DR2L;
break;
case 8:
ad=ATD0DR1L;
break;
case 9:
ad=ATD0DR0L;
break;
}
return ad;
}




void delay(unsigned int ms)
{

int i;
for(;ms>0;ms--) for(i=5000;i>0;i--);

}

void delay2(unsigned char us)
{

int i;
for(;us>0;us--) for(i=5;i>0;i--);

}


/**************显示子程序*****************/
int L1,L2,L3,L4;
void display(int num1)
{
int x;
L1=(num1%100)%10;
L2=(num1%100)/10;
L3=num1/100;
/*L5=(num2%100)%10;
L6=(num2%100)/10;
L7=num2/100; */


x=PORTB&0xf0;
x|=0x0e;
PORTB=x;//显示L1位
PORTA=table[L1];
delay2(30);
PORTB=x-1;
PORTA=table[L2];
delay2(30);
PORTB=x-3;
PORTA=table[L3];
delay2(30);

/*PORTB=0xef;//显示L5位
PORTA=table[L5];
delay2(50);
PORTB=0xdf;
PORTA=table[L6];
delay2(50);
PORTB=0xbf;
PORTA=table[L7];
delay2(50); */

}


void lvbo_init()
{
while(!ATD0STAT0_SCF);
if(read_ad(9)>(caiji_ad[1]+1)||read_ad(9)<(caiji_ad[1]-1))
caiji_ad[1]=read_ad(9);

if(read_ad(8)>(caiji_ad[2]+1)||read_ad(8)<(caiji_ad[2]-1))
caiji_ad[2]=read_ad(8);

if(read_ad(7)>(caiji_ad[3]+1)||read_ad(7)<(caiji_ad[3]-1))
caiji_ad[3]=read_ad(7);

if(read_ad(6)>(caiji_ad[4]+1)||read_ad(6)<(caiji_ad[4]-1))
caiji_ad[4]=read_ad(6);

if(read_ad(5)>(caiji_ad[5]+1)||read_ad(5)<(caiji_ad[5]-1))
caiji_ad[5]=read_ad(5);

if(read_ad(4)>(caiji_ad[6]+1)||read_ad(4)<(caiji_ad[6]-1))
caiji_ad[6]=read_ad(4);

if(read_ad(3)>(caiji_ad[7]+1)||read_ad(3)<(caiji_ad[7]-1))
caiji_ad[7]=read_ad(3);

if(read_ad(2)>(caiji_ad[8]+1)||read_ad(2)<(caiji_ad[8]-1))
caiji_ad[8]=read_ad(2);

}


void car_position_h()
{
int i,max;

max=1;
for(i=2;i<4;i++) //找出最强的传感器
{

if(caiji_ad[max]<caiji_ad[i])
max=i;
}
if(caiji_ad[1]<50&&caiji_ad[2]<50&&caiji_ad[3]<50) position_h=position_hp;
else
{

if(max==1||(max==2&&(caiji_ad[1]>caiji_ad[3])))
{
diff_h=(1/caiji_ad[1]-1/caiji_ad[2])*10000;
if(diff_h<-200) diff_h=-200;
position_flag=12;
if(diff_h<=66) position_h=18.91*sin(0.003646*diff_h+0.04781)-5;
//else position=-4;

}


if(max==3||(max==2&&(caiji_ad[1]<caiji_ad[3])))
{
diff_h=(1/caiji_ad[2]-1/caiji_ad[3])*10000;
if(diff_h>250) diff_h=250;
position_flag=23;
if(diff_h>=-86) position_h=13.53*sin(0.00483*diff_h-0.05077)+5;
}
}
if(position_h>12) position_h=12;
if(position_h<-12) position_h=-12;

position_hd=position_h-position_hp;
if(position_hd>10||position_hd<-10) position_h=position_hp;
else position_hp=position_h;

}
void car_position_q()
{
int i,max;

max=4;
for(i=5;i<7;i++) //找出最强的传感器
{
if(caiji_ad[max]<caiji_ad[i])
max=i;
}
if(caiji_ad[4]<50&&caiji_ad[5]<50&&caiji_ad[6]<50) position_q=position_qp;
else
{

if(max==4||(max==5&&(caiji_ad[4]>caiji_ad[6])))
{
diff_q=(1/caiji_ad[4]-1/caiji_ad[5])*10000;
if(diff_q<-360) diff_q=-360;

if(diff_q<=66) position_q=25.97*sin(0.002766*diff_q+0.0162)-5;
//else position=-4;

}


if(max==6||(max==5&&(caiji_ad[4]<caiji_ad[6])))
{
diff_q=(1/caiji_ad[5]-1/caiji_ad[6])*10000;
if(diff_q>250) diff_q=250;

if(diff_q>=-70) position_q=12.58*sin(0.005532*diff_q-0.02258)+5;
}
}
if(position_q>12) position_q=12;
if(position_q<-12) position_q=-12;
position_qd=position_q-position_qp;
if(position_qd>10||position_qd<-10) position_q=position_qp;
else position_qp=position_q;

}


void drive()
{

if(position_jian>=-1&&position_jian<=1) //直道
{
if(position_q>=-5&&position_q<=5)
{
a=1;
angle_data=2500;
if((PORTB&0xf0)==0x60) Speed_tar=0.45;
if((PORTB&0xf0)==0x70) Speed_tar=0.5;

if((PORTB&0xf0)==0x80) Speed_tar=0.6;
if((PORTB&0xf0)==0x90) Speed_tar=0.7;
if((PORTB&0xf0)==0xa0) Speed_tar=0.8;
if((PORTB&0xf0)==0xb0) Speed_tar=0.9;
if((PORTB&0xf0)==0xc0) Speed_tar=1.0;
}
else
{
a=2;
angle_data=2600+35*position_q; //大角度回转
if((PORTB&0xf0)==0x60) Speed_tar=0.45;
if((PORTB&0xf0)==0x70) Speed_tar=0.5;

if((PORTB&0xf0)==0x80) Speed_tar=0.6;
if((PORTB&0xf0)==0x90) Speed_tar=0.7;
if((PORTB&0xf0)==0xa0) Speed_tar=0.8;
if((PORTB&0xf0)==0xb0) Speed_tar=0.9;
if((PORTB&0xf0)==0xc0) Speed_tar=1.0;
}

}
if(position_jian<-1||position_jian>1)
{
if((position_q>0&&position_h<0)||(position_q<0&&position_h>0)) //一正一负
{
a=3;
//angle_data=2600+banjing;if((PORTB&0xf0)==0x60) Speed_tar=5;
if((PORTB&0xf0)==0x60) Speed_tar=0.4;
if((PORTB&0xf0)==0x70) Speed_tar=0.45;

if((PORTB&0xf0)==0x80) Speed_tar=0.4;
if((PORTB&0xf0)==0x90) Speed_tar=0.4;
if((PORTB&0xf0)==0xa0) Speed_tar=0.5;
if((PORTB&0xf0)==0xb0) Speed_tar=0.55;
if((PORTB&0xf0)==0xc0) Speed_tar=0.6;
}
if(position_q>0&&position_h>=0) //都在右侧
{
if(position_q>position_h) //外道
{
a=4;
Speed_tar=0.3;
if((PORTB&0xf0)==0x80) Speed_tar=0.45;
if((PORTB&0xf0)==0x90) Speed_tar=0.45;
if((PORTB&0xf0)==0xa0) Speed_tar=0.45;
if((PORTB&0xf0)==0xb0) Speed_tar=0.45;
if((PORTB&0xf0)==0xc0) Speed_tar=0.55;
}

else //内道加速
{
a=5;
if((PORTB&0xf0)==0x60) Speed_tar=0.35;
if((PORTB&0xf0)==0x70) Speed_tar=0.45;

if((PORTB&0xf0)==0x80) Speed_tar=0.45;
if((PORTB&0xf0)==0x90) Speed_tar=0.5;
if((PORTB&0xf0)==0xa0) Speed_tar=0.5;
if((PORTB&0xf0)==0xb0) Speed_tar=0.5;
if((PORTB&0xf0)==0xc0) Speed_tar=0.6;
}
}
if(position_q<=0&&position_h<0) //都在左侧
{
if(position_q>position_h) //内道
{
a=6;
if((PORTB&0xf0)==0x60) Speed_tar=0.35;
if((PORTB&0xf0)==0x70) Speed_tar=0.45;

if((PORTB&0xf0)==0x80) Speed_tar=0.45;
if((PORTB&0xf0)==0x90) Speed_tar=0.5;
if((PORTB&0xf0)==0xa0) Speed_tar=0.5;
if((PORTB&0xf0)==0xb0) Speed_tar=0.5;
if((PORTB&0xf0)==0xc0) Speed_tar=0.6;
}
else //外道
{
a=7;
Speed_tar=0.3;
if((PORTB&0xf0)==0x80) Speed_tar=0.45;
if((PORTB&0xf0)==0x90) Speed_tar=0.45;
if((PORTB&0xf0)==0xa0) Speed_tar=0.45;
if((PORTB&0xf0)==0xb0) Speed_tar=0.45;
if((PORTB&0xf0)==0xc0) Speed_tar=0.55;
}

}
angle_data=2600+banjing;
if(position_q>=-12&&position_q<=-7) angle_data=2200;
if(position_q>=7&&position_q<=12) angle_data=3000;
}
if(angle_data>3000) angle_data=3000;
if(angle_data<2200) angle_data=2200;
angle_pre=angle_data;
}


void PID_init(void) //增量式PID正常处理部分
{

error=Speed_tar-Speed_cur/100.0;
pwmtemp+=(PID_P*(error-last_error)+PID_I*error+PID_D*(error+pre_error-2*last_error));
if(pwmtemp>200) pwmtemp=199;
if(pwmtemp<0) pwmtemp=0;

if(error>=0)
{

PWMDTY23=0;
PWMDTY45=pwmtemp;

}


if(error<0&&error>=error_min)
{

PWMDTY23=0;
PWMDTY45=pwmtemp-50*error_min;

}
if(error<error_min&&error>=-0.2)
{
PWMDTY23=pwmtemp;
PWMDTY45=0;

}
if(error<-0.2&&error>=-2.5)
{
PWMDTY23=150;
PWMDTY45=0;
}
if(error<-2.5)
{
PWMDTY23=199;
PWMDTY45=0;
}
pre_error=last_error;
last_error=error;

}



void main()
{
char d;
DisableInterrupts;
data_init();
SET_PLL();
AD_Init();
initPIT();
IOC_Init();
pwm_init();
SCI_Init();
PWME=0x3f;
PWMDTY01=2600;//2600为中间位置,左极限2200,右极限3000
PWMDTY23=0;
PWMDTY45=0;
pwmtemp=0;
Speed_tar=0;
PUCR_PUPBE=1; //使用拉电阻
//delay(2000);
DDRT=0X00;
EnableInterrupts;

while(1)
{
while((PORTB&0xf0)==0x00)
{
display(caiji_ad[1]);
lvbo_init();
}
while((PORTB&0xf0)==0x10)
{
display(caiji_ad[2]);
lvbo_init();
}
while((PORTB&0xf0)==0x20)
{
display(caiji_ad[3]);
lvbo_init();
}
while((PORTB&0xf0)==0x30)
{
display(caiji_ad[4]);
lvbo_init();
}
while((PORTB&0xf0)==0x40)
{
display(caiji_ad[5]);
lvbo_init();
}
while((PORTB&0xf0)==0x50)
{
display(caiji_ad[6]);
lvbo_init();
}
lvbo_init();
car_position_q();
car_position_h();
position_jian=position_q-position_h+0.27;
position_jia=position_q+position_h;
position_cha=position_jian/2; //gai guo
position_zhong=position_jian-position_cha; //gai guo

if((PORTB&0xf0)==0x60) banjing=(8200*position_zhong)/(position_zhong*position_zhong+100);
if((PORTB&0xf0)==0x70) banjing=(8200*position_zhong)/(position_zhong*position_zhong+100);
if((PORTB&0xf0)==0x80) banjing=(8200*position_zhong)/(position_zhong*position_zhong+100);
if((PORTB&0xf0)==0x90) banjing=(8200*position_zhong)/(position_zhong*position_zhong+100);
if((PORTB&0xf0)==0xa0) banjing=(8200*position_zhong)/(position_zhong*position_zhong+100);
if((PORTB&0xf0)==0xb0) banjing=(8200*position_zhong)/(position_zhong*position_zhong+100);
if((PORTB&0xf0)==0xc0) banjing=(9000*position_zhong)/(position_zhong*position_zhong+100);

position=position_q;
drive();

PWMDTY01=angle_data;

if(flag_pulse==2)
{
flag_pulse=0;
PID_init();

}
if(upp>=1000)
//检测停止
check_stop=1;

while(PTT_PTT0==0&&check_stop==1)
{
while(1)
{

Speed_tar=0;
PWMDTY01=2600;
PID_init();
}
}

}

}



#pragma CODE_SEG __NEAR_SEG NON_BANKED
void interrupt 66 PIT0(void)
{
PACTL_PAEN=0;//16b计数禁止

caiji_flag=1;
flag_pulse++;
upp++;
jishi_flag++;

Speed_cur=PACNT;
//SCI_SendByte(PACNTL);
PACNT = 0X0000;
PACTL_PAEN=1;//16b计数允许

PITTF_PTF0=1;//清中断标志位
}

相关文章:
飞思卡尔智能车电磁组 程序
飞思卡尔智能车电磁组 程序_电子/电路_工程科技_专业资料 暂无评价|0人阅读|0次下载|举报文档飞思卡尔智能车电磁组 程序_电子/电路_工程科技_专业资料。#include ...
第五届飞思卡尔智能车电磁组程序
第五届飞思卡尔智能车电磁组程序_兵器/核科学_工程科技_专业资料。第五届飞思卡尔智能车电磁组获奖程序 MC9S12XS128 单片机、用前置线圈检测磁感线、用无线蓝牙采集...
飞思卡尔智能车电磁组第七届可用程序
飞思卡尔智能车电磁组 程... 6页 1下载券 智能车电磁组完整程序 18页 2下载券飞​思​卡​尔​智​能​车​电​磁​组​第​七​届...
第五届飞思卡尔智能车电磁组获奖程序
飞思卡尔智能车程序 11页 4下载券 飞思卡尔智能车电磁组资... 10页 1下载券...飞​思​卡​尔​比​赛​,​电​磁​组​程​序​,​...
飞思卡尔智能车电磁组程序员成长之路(未完待续)
飞思卡尔智能车电磁组程序员成长之路(未完待续)_电子/电路_工程科技_专业资料。飞思卡尔智能车电磁组程序员成长之路 1. 飞思卡尔智能车小车入门智能汽车电磁组简介:...
飞思卡尔智能车比赛电磁组路径检测设计方案
飞思卡尔智能车比赛电磁组路径检测设计方案_电子/电路_工程科技_专业资料。电磁组竞赛车模 路径检测设计参考方案(竞赛秘书处 2010-1,版本 1.0) 一、 前言 第五届...
飞思卡尔智能车电磁组路径检测设计参考方案
飞思卡尔智能车电磁组路径检测设计参考方案_信息与通信_工程科技_专业资料。飞思卡尔智能车电磁组路径检测设计参考方案 电磁组竞赛车模 路径检测设计参考方案 (竞赛秘书...
[飞思卡尔智能车电磁组]PID调试步骤
飞思卡尔智能车电磁组第... 15页 1下载券喜欢此文档的还喜欢 ...[1+1/(TI*s)+TD*s] [电磁]PID 调试步骤为什么 PID 应用如此广泛、又长久...
飞思卡尔智能车 电磁组 技术报告
飞思卡尔智能车 电磁组 技术报告_机械/仪表_工程科技_专业资料。---...主程序流程图如下: 11 --第六届全国大学生智能汽车邀请赛技术报告---...
飞思卡尔智能车程序
飞思卡尔智能车程序_信息与通信_工程科技_专业资料。全国大学生飞思卡尔智能车程序...飞思卡尔智能车程序 12页 免费 飞思卡尔智能车电磁组第... 15页 1下载券喜欢...
更多相关标签:
飞思卡尔智能车电磁组 | 飞思卡尔智能车 电磁 | 飞思卡尔智能车程序 | 飞思卡尔电磁组程序 | 飞思卡尔智能车大赛 | 飞思卡尔智能车 | 飞思卡尔智能车论坛 | 飞思卡尔智能车官网 |