第五届飞思卡尔电磁车程序速度25.docx
《第五届飞思卡尔电磁车程序速度25.docx》由会员分享，可在线阅读，更多相关《第五届飞思卡尔电磁车程序速度25.docx（11页珍藏版）》请在冰豆网上搜索。
第五届飞思卡尔电磁车程序速度25
#include
#include"derivative.h"
#include"printp.h"
#definese1max（int*）（0x0800）
#definese3max（int*）（0x0802）
#definese5max（int*）（0x0804）
#definese7max（int*）（0x0806）
#definespoint294
#defineH110
#defineH210
#include"function.h"
voidinit（void）;
intPWM_MIN=8100,PWM_MID=8800,PWM_MAX=9500;
intsdata1[spoint],sdata3[spoint],sdata5[spoint],sdata7[spoint];
inty[spoint];
intSE1,SE3,SE5,SE7;
intoffset1,offset3,offset5,offset7,lastoffset1,lastoffset3,lastoffset5,lastoffset7;
intSE1MAX,SE3MAX,SE5MAX,SE7MAX;
intservo_pwm,lastservo_pwm;
longintsum1,sum3,sum5,sum7;
intspeed,prespeed,presetspeed,distance,bendspeed=50;
intprint_flag=0,counter=0,startflag=0,stopflag=0,startdelay,startdelaycount;
intk1=24,k3=24,k5=24,k7=24;//校正系数
intstateSE7[3],stateSE5[3],stateSE1[3],stateSE3[3],diff1,diff3,diff7,diff5;
voidmain（void）
{
intn;
init（）;
PWMDTY01=PWM_MID;//舵机PWM波脉宽范围
ATD0CTL5=0x30;//启动AD
PITCE_PCE0=1;//启动PIT0
PITCE_PCE1=1;//启动PIT1
PITCE_PCE2=1;//启动PIT2
PITCE_PCE3=1;//启动PIT3
EnableInterrupts;
for（;;）
{
if（print_flag==1）
{
n++;
if（n==10）
{
n=0;
printp（"%d",PWMDTY01）;
printp（"\n\n"）;
printp（"1=%d3=%d5=%d7=%d",offset1,offset3,offset5,offset7）;
/*for（n=0;n{
printp（"%d",sdata1[n]）;
}*/
printp（"SE1=%d%d%d%d",SE1,SE3,SE5,SE7）;
//printp（"diff1=%d%d%d%d",diff1,diff3,diff5,diff7）;
//printp（"\n"）;
printp（"SEMAX=%d%d%d%d",SE1MAX,SE3MAX,SE5MAX,SE7MAX）;
printp（"speed=%d",speed）;
//printp（"k1=%dk3=%dk5=%dk7=%d",k1,k3,k5,k7）;
//printp（"\n\n"）;
//printp（"%d",servo_pwm）;
//printp（"\n\n"）;
printp（"\n\n"）;
print_flag=0;
}
}
}
}
#pragmaCODE_SEG__NEAR_SEGNON_BANKED
voidinterrupt12IOC4interrupt（void）
{
inti,j;
TFLG1_C4F=1;
TIE_C4I=0;
//prespeed=presetspeed;
startflag++;//按第一下,显示舵机的方向，按第二下，延时2S启动
for（i=0;i<500;i++）
for（j=0;j<0x0d53;j++）;
TFLG1_C4F=1;
TIE_C4I=1;
}
voidinterrupt13IOC5interrupt（void）
{
inti,j;
TFLG1_C5F=1;
TIE_C5I=0;
if（PTJ_PTJ7==0）
{
bendspeed+=3;
}
else
presetspeed+=3;
if（PTT_PTT2==0）
{
presetspeed=70;
}
for（i=0;i<500;i++）
for（j=0;j<0x0d53;j++）;
TFLG1_C5F=1;
TIE_C5I=1;
}
voidinterrupt22ADinterrupt（void）
{
sdata1[counter]=ATD0DR0;
sdata3[counter]=ATD0DR1;
sdata5[counter]=ATD0DR2;
sdata7[counter]=ATD0DR3;
counter++;
}
voidinterrupt69PIT3interrupt（void）
{
_asm（MOVB#$08,PITTF）;
speed=PACNT;
PACNT=0;
if（startflag==2）
{
if（startdelay==0）
{
startdelaycount++;
if（startdelaycount>=100）
{
startdelay=1;
}
}
if（startdelay==1）
{
adjust_speed（）;
}
}
}
voidinterrupt68PIT2interrupt（void）
{
_asm（MOVB#$04,PITTF）;
if（PORTA_PA0==0）
{
stopflag++;
PITCE_PCE2=0;
}
}
voidinterrupt67PTI1interrupt（void）
{
//***为下一个循环作准备****
_asm（MOVB#$02,PITTF）;
counter=0;
sum1=0;
sum3=0;
sum5=0;
sum7=0;
PITCE_PCE0=1;
ATD0CTL2_ASCIE=1;
ATD0CTL5=0x30;
print_flag=1;
}
voidinterrupt66PIT0interrupt（void）//采样时间为2ms
{
inti,temp;
PITCE_PCE0=0;//关闭PIT
ATD0CTL2_ASCIE=0;//关闭AD
EnableInterrupts;//允许PIT2,3中断该程序
//***滤波****
filter（sdata1）;
filter（sdata3）;
filter（sdata5）;
filter（sdata7）;
//***求峰峰值***
for（i=0;i{
sum1+=sdata1[i];
sum3+=sdata3[i];
sum5+=sdata5[i];
sum7+=sdata7[i];
}
SE1=（int）（sum1>>12）;
SE3=（int）（sum3>>12）;
SE5=（int）（sum5>>12）;
SE7=（int）（sum7>>12）;
//***找出相应的最大值***
if（PTT_PTT3==0）
{
if（SE1>SE1MAX）
{
SE1MAX=SE1;
}
if（SE3>SE3MAX）
{
SE3MAX=SE3;
}
if（SE5>SE5MAX）
{
SE5MAX=SE5;
}
if（SE7>SE7MAX）
{
SE7MAX=SE7;
}
save2flash（SE1MAX,SE3MAX,SE5MAX,SE7MAX）;
}
SE1MAX=*se1max;
SE3MAX=*se3max;
SE5MAX=*se5max;
SE7MAX=*se7max;
if（SE1==0）
SE1=1;
if（SE3==0）
SE3=1;
if（SE5==0）
SE5=1;
if（SE7==0）
SE7=1;
//***求偏移*****
offset1=（int）sqrt（100*（unsignedlongint）（（SE1MAX）*H1*H1/SE1-H2*H2））*24/k1;
offset3=（int）sqrt（100*（unsignedlongint）（（SE3MAX）*H1*H1/SE3-H2*H2））*24/k3;
offset5=（int）sqrt（100*（unsignedlongint）（（SE5MAX）*H2*H2/SE5-H2*H2））*24/k5;
offset7=（int）sqrt（100*（unsignedlongint）（（SE7MAX）*H2*H2/SE7-H2*H2））*24/k7;
if（offset1>450）
offset1=450;
if（offset3>450）
offset3=450;
if（offset5>450）
offset5=450;
if（offset7>450）
offset7=450;
//***求servo_pwm***
stateSE1[0]=SE1;
stateSE3[0]=SE3;
stateSE7[0]=SE7;
stateSE5[0]=SE5;
diff1=stateSE1[0]-stateSE1[2];
diff3=stateSE3[0]-stateSE3[2];
diff7=stateSE7[0]-stateSE7[2];
diff5=stateSE5[0]-stateSE5[2];
for（i=1;i>=0;i--）
{
stateSE1[i+1]=stateSE1[i];
stateSE3[i+1]=stateSE3[i];
stateSE5[i+1]=stateSE5[i];
stateSE7[i+1]=stateSE7[i];
}
if（offset5+offset7>400）
{
if（（offset7+offset3）>（offset5+offset1））
{
servo_pwm=（（offset7+offset5-80）+（（offset7-lastoffset7）-（offset5-lastoffset5）））*（（PWM_MAX-PWM_MID）/200）;
}
else
{
servo_pwm=（（-offset7-offset5+80）+（（offset7-lastoffset7）-（offset5-lastoffset5）））*（（PWM_MAX-PWM_MID）/200）;
}
if（servo_pwm+PWM_MID>PWM_MAX）
servo_pwm=PWM_MAX-PWM_MID;
if（servo_pwm+PWM_MIDservo_pwm=PWM_MIN-PWM_MID;
if（abs（servo_pwm-lastservo_pwm）>=PWM_MAX-PWM_MID+100）
servo_pwm=lastservo_pwm;
}
elseif（（offset5+offset7）>280）//四个电感处在同侧
{
servo_pwm=（（offset7-offset5）+（（offset7-lastoffset7）-（offset5-lastoffset5）））*（（PWM_MAX-PWM_MID）/100）;
}
else
{
servo_pwm=（（offset7-offset5）*3/2+（offset1-offset5+offset7-offset3）*4+（（offset7-lastoffset7）-（offset5-lastoffset5））*2）*（（PWM_MAX-PWM_MID）/350）+（（diff3-diff7）+（diff5-diff1））*5;
}
//***求相应的PWMDTY01***
if（servo_pwm+PWM_MID>PWM_MAX）
{
servo_pwm=PWM_MAX-PWM_MID;
PWMDTY01=PWM_MAX;
}
elseif（servo_pwm+PWM_MID{
servo_pwm=PWM_MIN-PWM_MID;
PWMDTY01=PWM_MIN;
}
else
PWMDTY01=PWM_MID+servo_pwm;
lastservo_pwm=servo_pwm;
lastoffset7=offset7;
lastoffset5=offset5;
prespeed=presetspeed-abs（PWMDTY01-PWM_MID）/（（PWM_MAX-PWM_MID）/（presetspeed-bendspeed））;
//***显示控制***
if（startflag==0）
{
if（PTJ_PTJ7==0）
display_speed（bendspeed）;
else
display_speed（presetspeed）;
}
else
display_position（）;
//***停车控制***
if（SCI0SR1_RDRF==1）
{
temp=SCI0DRL;
PWMDTY01=PWM_MID;
PWMDTY23=0;
PWMDTY67=0;
}
}
升级会员 