第五屆飛思卡爾電磁車程序速度25

1、#include #include derivative.h #includeprintp.h#define se1max (int*)(0x0800)#define se3max (int*)(0x0802)#define se5max (int*)(0x0804)#define se7max (int*)(0x0806)#define spoint 294#define H1 10#define H2 10#includefunction.h void init(void);int PWM_MIN=8100,PWM_MID=8800,PWM_MAX=9500;int sdata1spoin

2、t,sdata3spoint,sdata5spoint,sdata7spoint;int yspoint;int SE1,SE3,SE5,SE7;int offset1,offset3,offset5,offset7,lastoffset1,lastoffset3,lastoffset5,lastoffset7;int SE1MAX,SE3MAX,SE5MAX,SE7MAX;int servo_pwm,lastservo_pwm;long int sum1,sum3,sum5,sum7;int speed,prespeed,presetspeed,distance,bendspeed=50;i

3、nt print_flag=0,counter=0,startflag=0,stopflag=0,startdelay,startdelaycount;int k1=24,k3=24,k5=24,k7=24; /校正系數(shù)int stateSE73,stateSE53,stateSE13,stateSE33,diff1,diff3,diff7,diff5;void main(void) int n; init(); PWMDTY01 = PWM_MID; /舵機(jī)PWM波脈寬范圍 ATD0CTL5=0x30; /啟動AD PITCE_PCE0=1; /啟動PIT0 PITCE_PCE1=1; /啟

4、動PIT1 PITCE_PCE2=1; /啟動PIT2 PITCE_PCE3=1; /啟動PIT3EnableInterrupts; for(;) if(print_flag=1) n+; if(n=10) n=0; printp( %d ,PWMDTY01); printp(nn); printp(1=%d 3=%d 5=%d 7=%d ,offset1,offset3,offset5,offset7); /*for(n=0;nspoint;n+) printp(%d ,sdata1n); */ printp(SE1=%d %d %d %d ,SE1,SE3,SE5,SE7); /print

5、p(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=%d k3=%d k5=%d k7=%d,k1,k3,k5,k7); /printp(nn); /printp(%d ,servo_pwm); / printp(nn); printp(nn); print_flag=0; #pragma CODE_SEG _NEAR_SEG NON_BANKEDv

6、oid interrupt 12 IOC4interrupt(void) int i,j; TFLG1_C4F=1; TIE_C4I=0; /prespeed=presetspeed; startflag+; /按第一下,顯示舵機(jī)的方向，按第二下，延時2S啟動 for(i=0;i500;i+) for(j=0;j0x0d53;j+); TFLG1_C4F=1; TIE_C4I=1; void interrupt 13 IOC5interrupt(void) int i,j; TFLG1_C5F=1; TIE_C5I=0; if(PTJ_PTJ7=0) bendspeed+=3; else pr

7、esetspeed+=3; if(PTT_PTT2=0) presetspeed=70; for(i=0;i500;i+) for(j=0;j=100) startdelay=1; if(startdelay=1) adjust_speed(); void interrupt 68 PIT2interrupt(void) _asm(MOVB #$04,PITTF); if(PORTA_PA0=0) stopflag+; PITCE_PCE2=0; void interrupt 67 PTI1interrupt(void) /*為下一個循環(huán)作準(zhǔn)備* _asm(MOVB #$02,PITTF);

8、counter=0; sum1=0; sum3=0; sum5=0; sum7=0; PITCE_PCE0=1; ATD0CTL2_ASCIE = 1; ATD0CTL5 = 0x30; print_flag=1;void interrupt 66 PIT0interrupt(void) /采樣時間為2ms int i,temp; PITCE_PCE0=0; /關(guān)閉PIT ATD0CTL2_ASCIE=0; /關(guān)閉AD EnableInterrupts; /允許PIT2,3中斷該程序 /*濾波* filter(sdata1); filter(sdata3); filter(sdata5); f

9、ilter(sdata7); /*求峰峰值* for(i=0;i12); SE3=(int)(sum312); SE5=(int)(sum512); SE7=(int)(sum712); /*找出相應(yīng)的最大值* if(PTT_PTT3=0) if(SE1SE1MAX) SE1MAX=SE1; if(SE3SE3MAX) SE3MAX=SE3; if(SE5SE5MAX) SE5MAX=SE5; if(SE7SE7MAX) SE7MAX=SE7; save2flash(SE1MAX,SE3MAX,SE5MAX,SE7MAX); SE1MAX=*se1max; SE3MAX=*se3max; SE

10、5MAX=*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*(unsigned long int)(SE1MAX)*H1*H1/SE1-H2*H2)*24/k1; offset3=(int)sqrt(100*(unsigned long int)(SE3MAX)*H1*H1/SE3-H2*H2)*24/k3; offset5=(int)sqrt(100*(unsigned long int)(SE5MA

11、X)*H2*H2/SE5-H2*H2)*24/k5; offset7=(int)sqrt(100*(unsigned long int)(SE7MAX)*H2*H2/SE7-H2*H2)*24/k7; if(offset1450) offset1=450; if(offset3450) offset3=450; if(offset5450) offset5=450; if(offset7450) offset7=450; /*求servo_pwm* stateSE10=SE1; stateSE30=SE3; stateSE70=SE7; stateSE50=SE5; diff1=stateSE

12、10-stateSE12; diff3=stateSE30-stateSE32; diff7=stateSE70-stateSE72; diff5=stateSE50-stateSE52; for(i=1;i=0;i-) stateSE1i+1=stateSE1i; stateSE3i+1=stateSE3i; stateSE5i+1=stateSE5i; stateSE7i+1=stateSE7i; if(offset5+offset7400) if(offset7+offset3)(offset5+offset1) servo_pwm=(offset7+offset5-80)+(offse

13、t7-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_MIDPWM_MAX) servo_pwm=PWM_MAX-PWM_MID; if(servo_pwm+PWM_MID=PWM_MAX-PWM_MID+100) servo_pwm=lastservo_pwm; else if(of

14、fset5+offset7)280) /四個電感處在同側(cè) 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; /

15、*求相應(yīng)的PWMDTY01* if(servo_pwm+PWM_MIDPWM_MAX) servo_pwm=PWM_MAX-PWM_MID; PWMDTY01=PWM_MAX; else if(servo_pwm+PWM_MIDPWM_MIN) 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-PW