飛思卡爾智能車智能車源代碼光電組(有注解)

1、智能車源代碼光電組(有注解)#include<hidef.h>/*commondefinesandmacros*/#include<mc9s12dg256.h>/*derivativeinformation*/#include"math.h"#include"PWM.h"#include"ATD.h"#include"LQprintp.h"#pragmaLINK_INFODERIVATIVE"mc9s12dg256b"staticunsignedints_sKd=10;道

2、PID的D值/*用于防止PID溢出*unsignedshorts_sE=5;unsignedchars_sq=1;直/*/*，*分別存放PID彳!*ints_sMp=0;ints_sMi=0;ints_sMd=0;ints_sMp2=0;/*intsL_u3;實際逮度的差值存放前3次理論速度與staticunsignedintmem_num=0;調(diào)試中斷unsignedinti=0;unsignedintj=0;unsignedintt=0;bytead_value13;uchardata13;intsum=0;ucharstart_flag=0;ucharnum=0;ucharlw=0;un

3、signedintper=65530;intSPWM=0;intL_SPWM=0;unsignedintSPmax=1000;intMPWM=0;ucharcurrent_corrd=0;intlast_corrd310=(0,0,0,0,0,0,0,0,0,0),(0,0,0,0,0,0,0,0,0,0),(0,0,0,0,0,0,0,0,0,0);/*/*PID*unsignedcharcp=0;unsignedcharci=0;unsignedcharcd=0;unsignedintsearch_PACN10;unsignedintnp=0;unsignedintsp500;/*/*PI

4、D*staticunsignedintKp=25;staticunsignedintKp2=60;staticunsignedintKi=9;staticunsignedintKd=30;staticunsignedintrKp=100;staticunsignedintrKp2=60;staticunsignedintrKi=0;staticunsignedintrKd=60;unsignedshortE=5;unsignedcharq=1;intMp=0;intMi=0;intMd=0;intMp2=0;/*存儲前20點的數(shù)據(jù)intL_num10=(0,0,0,0,0,0,0,0,0,0)

5、;intsum_corrd=0;intwb=0;白為1;記錄當前狀態(tài)黑為0,/*/*紅外*intcorrd10=(0,0,0,0,0,0,0,0,0,0);intT_corrd=0;intis_white=0;intnumb=0;intP_Speed=0;intL_u3;/*/*/*舵機PID變量/*staticunsignedints_sKp=35;/直道PID的P值一staticunsignedints_sKp2=0;/直道PID的二階P值一staticunsignedints_sKi=0;/直道PID的I值一intcorrd_sate23=(0,0,0,0,0,0,0,0,0,0,0,0

6、,0,0,0,0,0,0,0,0,0,0,0);intSPWM_sate15=(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0);intMPWM_sate11=(0,0,0,0,0,0,0,0,0,0,0);ucharcorrd_time512;ucharSPWM_time512;ucharMPWM_time512;ucharPACN_time512;*voidsetbusclock(void)CLKSEL=0X00;/disengagePLLtosystemPLLCTL_PLLON=1;/turnonPLLSYNR=1;REFDV=1;pllclock=2*osc*(1+SYNR

7、)/(1+REFDV)=32MHz;_asm(nop);/BUSCLOCK=16M_asm(nop);while(!(CRGFLG_LOCK=1);/whenpllissteady,thenuseit;CLKSEL_PLLSEL=1;/engagePLLtosystem;voidDly_ms(intms)一intii,jj;if(ms<1)ms=1;for(ii=0;ii<ms;ii+)for(jj=0;jj<2670;jj+);busclk:16MHz-1msstaticvoidSCI_Init(void)SCI0CR2=0x2c;/enableReceiveFullInt

8、errupt,RXenable,TxenableSCI0BDH=0x00;出口波特率為9600SCI0BDL=0x68;/SCI0BDL=busclk/(16*SCI0BDL)/busclk8MHz,9600bps,SCI0BDL=0x34/busclk16MHz,9600bps,SCI0BDL=0x68/busclk24MHz,9600bps,SCI0BDL=0x9C/busclk32MHz,9600bps,SCI0BDL=0xD0staticvoidIOC_Init(void)PERT=0XFF;/PPST=0XFF;DDRT=0XFe;PBCTL=0X50;/PT0PIN,PACN101

9、6BIT,FALLingedge,NOTINTERRUPT/PACN0=200;/PACN1=0xFF;TCTL4=0x01;/40表示ICx禁止，1表示上升沿2表示下降沿，3表示任何沿/TIE=0x00;/每一位對應(yīng)相應(yīng)通道中斷允許,0表示禁止中斷TIOS=0xfe;/每一位對應(yīng)通道的：0輸入捕捉,1輸出比較unsignedintget_Speed()一intSpeed;Speed=PACN10;PACN10=0;returnSpeed;voids_PID_MPWM()L_u2=L_u1;L_u1=L_u0;L_u0=P_Speed-search_PACN10;Mp=Kp*L_u0;Mi=M

10、i+Ki*L_u0;Md=Kd*(L_u0-2*L_u1+L_u2);Mp2=Kp2*(L_u0-L_u1);if(Ki*L_u0)>E)|(Ki*L_u0)<-E)q=0;elseq=1;MPWM=MPWM+Mp+q*Mi+Md+Mp2;if(MPWM>1000)MPWM=1000;if(MPWM<-1000)MPWM=-1000;Set_MPWM(MPWM);voidr_PID_MPWM()L_u2=L_u1;L_u1=L_u0;L_u0=P_Speed-search_PACN10;Mp=rKp*L_u0;Mi=Mi+rKi*L_u0;Md=rKd*(L_u0-2*

11、L_u1+L_u2);Mp2=rKp2*(L_u0-L_u1);if(rKi*L_u0)>E)|(rKi*L_u0)<-E)q=0;elseq=1;MPWM=MPWM+Mp+q*Mi+Md+Mp2;if(MPWM>1000)MPWM=1000;if(MPWM<-1000)MPWM=-1000;Set_MPWM(MPWM);/*舵機pid控制良*voids_PID_SPWM()sL_u0(current_corrd+last_corrd01)sL_u2=sL_u1;sL_u1=sL_u0;(last_corrd02+last_corrd03)/2;printp("

12、;%c",corrd_timej);一/*計算piD值*if(sL_u0<20&&sL_u0>-20)s_sMp=s_sKp*sL_u0;s_sMi=s_sMi+s_sKi*sL_u0;s_sMd=s_sKd*(sL_u0-2*sL_u1sL_u2);s_sMp2=s_sKp2*(sL_u0-sL_u1);/*/*I項溢出防止*if(s_sKi*sL_u0)>s_sE)|(s_sKi*sL_u0)<-s_sE)s_sq=0;elses_sq=1;/*voidshow_SPWM_time()一一for(j=0;j<512;j+)printp

13、("%c",SPWM_timej);voidshow_MPWM_time()一一for(j=0;j<512;j+)printp("%c",MPWM_timej);SPWM=SPWM+s_sMp+s_sq*s_sMi+s_sMd+s_sMp2;*PWM溢出防止*if(SPWM>70)SPWM=70;if(SPWM<-70)SPWM=-70;/*elsesL_u0=sL_u1;voidshow_PACN_time()一一for(j=0;j<512;j+)printp("%c",PACN_timej);Set_SPW

14、M(SPWM);/*voidmain(void)/*putyourowncodehere*/voidshow_SPWM_data()一一setbusclock();SCI_Init();AD_Init();IOC_Init();Ini_PWM();for(j=0;j<15;j+)printp("%10d",SPWM_satej);EnableInterrupts;voidshow_MPWM_data()voidshow_corrd_data()一一for(j=0;j<11;j+)printp("%10d",MPWM_satej);for(;)

15、Dly_ms(10);*for(j=0;j<10;j+)adc_get(ad_value);for(i=1;i<12;i+)for(j=0;j<23;j+)printp("%6d",corrd_satej);一voidshow_corrd_time()一一for(j=0;j<512;j+)if(ad_valuei>=160)datai=1;num=num+1;elsedatai=0;sum=sum+datai*2*i;/printp("%10d",datai);)if(num=0)(corrdj=-1;)else(corrd

16、j=sum/num;)sum=0;num=0;)is_white=0;for(i=0;i<10;i+)(if(corrdi<0)(is_white+;)一for(j=i;j<10;j+)(T_corrd=corrdj;corrdj=corrdi;corrdi=T_corrd;)if(is_white>6)一numb=0;)sum=0;j=0;for(i=3;i<8;i+)if(corrdi>-1)sum+=corrdi;j+;)if(j!=0)current_corrd=sum/j;numb=j;)L_num9=L_num8;L_num8=L_num7;L_

17、num7=L_num6;L_num6=L_num5;L_num5=L_num4;L_num4=L_num3;L_num3=L_num2;L_num2=L_num1;L_num1=L_num0;L_num0=numb;*wb=L_num1|L_num2|L_num3|L_num4|L_num5|L_num6|L_num7|L_num8|L_num9;if(numb=0)DDRB=0x80;if(search_PACN10>10&&wb>0)P_Speed=20;if(lw=0)SPWM=-70;)elseSPWM=72;)/if(sum_corrd<220&am

18、p;&sum_corrd>-220)Set_MPWM(0);/else/Set_MPWM(700);)elseP_Speed=20;if(lw=0)SPWM=-70;)elseSPWM=72;)Set_MPWM(1000);)/Set_MPWM(0);)elseDDRB=0x00;/printp("%d",current_corrd);/if(PTJ&0x80)Set_MPWM(0);/elseif(wb=0)Set_MPWM(1000);)elseswitch(current_corrd)case 2: SPWM=-70;P_Speed=32;r_P

19、ID_MPWM();lw=0;break;case 3: SPWM=-60;P_Speed=32;r_PID_MPWM();lw=0;break;case 4: SPWM=-50;P_Speed=32;r_PID_MPWM();lw=0;break;case 5: SPWM=-40;P_Speed=32;r_PID_MPWM();lw=0;break;case6:SPWM=-30;P_Speed=32;r_PID_MPWM();lw=0;break;case7:SPWM32;r_PID_MPWM();lw=0;break;=-25;P_Speed=case8:SPWM32;r_PID_MPWM

20、();lw=0;break;=-20;P_Speed=case9:SPWM32;s_PID_MPWM();lw=0;break;=-15;P_Speed=case10:SPWM32;s_PID_MPWM();lw=0;break;=-10;P_Speed=case11:SPWM32;s_PID_MPWM();lw=0;break;=-5;P_Speed=case12:SPWM32;s_PID_MPWM();break;=0;P_Speed=case13:SPWM32;s_PID_MPWM();lw=1;break;=5;P_Speed=case14:SPWM32;s_PID_MPWM();lw

21、=1;break;=10;P_Speed=case15:SPWM32;s_PID_MPWM();lw=1;break;=15;P_Speed=case16:SPWM32;r_PID_MPWM();lw=1;break;=20;P_Speed=case17:SPWM32;r_PID_MPWM();lw=1;break;=25;P_Speed=case18:SPWM32;r_PID_MPWM();lw=1;break;=30;P_Speed=case19:SPWM32;r_PID_MPWM();lw=1;break;=40;P_Speed=case20:SPWM32;r_PID_MPWM();lw

22、=1;break;=50;P_Speed=case21:SPWM32;r_PID_MPWM();lw=1;break;=60;P_Speed=case22:SPWM32;r_PID_MPWM();lw=1;break;=70;P_Speed=default:break;t+;if(t=512)MPWM=0;for(;);)/*waitforever*/*pleasemakesurethatyouneverleavethisfunction*/voidinterrupt20SCI0RX(void)byteresult,temp;DisableInterrupts;temp=SCI0SR1;/*clearflag*/result=SCI0DRL;if(result='s'|result='S')show_SPWM_data();if(result='M'|result='m')show_MPWM_data();if(result='c'|result='C')show_corrd_