智能車電磁組完整程序

1、#include <hidef.h> /* common defines and macros */#include "derivative.h" /* derivative-specific definitions */int left1=0;int left2=0;int right1=0;int right2=0;int AR_LEFT=0;/left2-right2int AR_RIGHT=0;int CR=0;/左邊相加減右邊相加int preCR=0;int ppreCR=0;int mkp=0;int mki=0;int mkd=0;int ide

2、al_speed=0; /設(shè)定速度int speed=0;int s_ideal06=75,80,42,42,42,42; /普通道、長直道、普通到彎、長直道到彎、彎內(nèi)、偏離黑線int s_ideal16=70,75,42,42,42,42;int s_ideal26=62,70,42,40,41,40;int s_ideal36=54,66,42,40,41,40;int table_mkp06=30,30,30,30,30,30; /ni 16.31 ,shun 15.16int table_mkp16=25,25,25,25,25,25; int table_mkp26=5,4,4,20

3、,20,20;int table_mkp36=4,4,4,10,8,9; /穩(wěn)定速度int table_mki06=0,0,20,20,20,20;int table_mki16=0,0,20,20,20,20;int table_mki26=0,0,0,10,10,20;int table_mki36=0,0,0,0,0,0;int table_mkd06=0,0,0,0,0,0;int table_mkd16=0,0,0,0,0,0;int table_mkd26=0;int table_mkd36=0,0,0,0,0,0;int s_table6;int b_mkp6=0;int b_m

4、ki6=0;int b_mkd6=0;int table_rkp07=5,3,2,550,550,550,8;/普通道中間、長直道低速、長直道高速、普到彎、直到彎、彎、普通道兩邊int table_rkp17=7,5,4,450,450,400,9;int table_rkp27=6,3,2,150,150,150,9;int table_rkp37=5,3,2,150,150,150,9;int table_rkd07=0,0,0,400,400,400,100;int table_rkd17=0,0,0,500,500,500,100;int table_rkd27=0,0,0,200,3

5、00,400,100;int table_rkd37=0,0,0,200,300,400,100;int b_rkp7=0;int b_rkd7=0;int rkp=0;int rkd=0;int f=0;/ pwmDTY要加的值int pref=0;int Pulse_count=0; /脈沖數(shù)int ganhuang=0;unsigned int ting=0;int i=0; int Flag_Chute=0; /道路標(biāo)志int GeneralCtn=0;int CurveCtn=0;int ChuteCtn=0;int WANCtn=0;int Flag_gaosu=0;unsigne

6、d char Flag_Pwm;/知道轉(zhuǎn)彎道標(biāo)志 int flag=0; /*/PLL超頻到40MHZ*/ void PLL_Init(void) CLKSEL=0X00; PLLCTL_PLLON=1; REFDV=0X80|0X01; SYNR=0X40|0X04; POSTDIV=0X00; asm nop; asm nop; while(!(CRGFLG_LOCK=1); CLKSEL_PLLSEL=1;/延時函數(shù) cnt*1ms;void delay(unsigned int cnt) unsigned int loop_i,loop_j; for(loop_i=0;loop_i&l

7、t;cnt;loop_i+) loop_j=0x1300; while(loop_j-); /*/計數(shù)程序/*/void PACBInit() /PT7 獲得脈沖值 PACTL=0X40; /PT7 PIN,PACN32 16BIT,Rising edge,NOT INTERRUPT TCTL3=0x40; /c-輸入捕捉7 上升沿有效, TIE_C7I=0; /通道7 禁止中斷 TIOS_IOS7=0; /每一位對應(yīng)通道的: 0 輸入捕捉,1 輸出比較 PACNT = 0; void RTI_init(void) /RTI 產(chǎn)生 10ms 的中斷定時 asm sei; /關(guān)閉中斷 RTICT

8、L=0xC7; /中斷周期設(shè)置10ms 中斷一次(或者讓RTICTL=0x59<為10.24ms 定時>) CRGINT_RTIE=1; /實時中斷有效，一旦RTIF=1 則發(fā)出中斷請求 asm cli; /開放中斷 /舵機(jī)初始化void PWM_rudder_init(void) PWME_PWME3=0; PWME_PWME2=0; PWMPRCLK_PCKB=2;/CLOCKB=BUS/4=10MHz PWMSCLB=2;/CLOCCSB=10/(2*2)=2.5MHz PWMCTL_CON23=1;/組合PWM23 PWMCLK_PCLK3=1;/PWM3使用SB PWMP

9、ER23=50000;/寫PWM23的周期寄存器，周期是20ms PWMPOL_PPOL3=1;/極性為正 PWMCAE_CAE3=0;/左對齊 PWME_PWME3=1;/使能PWM23 /電機(jī)初始化void PWM_init_motor(void) /電機(jī)初始化 PWME_PWME0=0; PWME_PWME1=0; PWMPRCLK_PCKA=2; /Clock A=40M/4=10M PWMPOL_PPOL1=1;/通道1 正極性輸出 PWMCLK_PCLK1=0;/通道1 選擇A 時鐘 PWMCAE_CAE1=0;/左對齊 PWMCTL_CON01=1; PWMPER01=1000;

10、/輸出頻率=10M/1000=10Khz PWMDTY01=0;/通道1 占空比為100/250 PWME_PWME1=1;/通道1 使能 PWME_PWME4=0; PWME_PWME5=0; PWMPRCLK_PCKA=1; /Clock A=40M/2=20M PWMPOL_PPOL5=1;/通道5 正極性輸出 PWMCLK_PCLK5=1;/通道5 選擇SA 時鐘 PWMSCLA=1; /ClockSB=20M/(2*1)=10M PWMCAE_CAE5=0;/左對齊 PWMCTL_CON45=1; PWMPER45=1000;/輸出頻率=10M/1000=10Khz PWMDTY45

11、=0;/初始通占空比0 PWME_PWME5=1;/通道5 使能 void AD_Init(void) ATD0CTL1=0x20; /選擇AD通道為外部觸發(fā),10位精度,采樣前不放電 ATD0CTL2=0x40; /標(biāo)志位自動清零，禁止外部觸發(fā), 禁止中斷 ATD0CTL3=0xA0; /右對齊無符號,每次轉(zhuǎn)換4個序列, No FIFO, Freeze模式下繼續(xù)轉(zhuǎn) ATD0CTL4=0x09; /采樣時間為4個AD時鐘周期,PRS=9,ATDClock=40/(2*(9+1)2MHz ATD0CTL5=0x30; /特殊通道禁止,連續(xù)轉(zhuǎn)換4個通道 ,多通道轉(zhuǎn)換，起始通道為0轉(zhuǎn)換 ATD0DI

12、EN=0x00; /禁止數(shù)字輸入/*/檢測起跑線*/void Checkstart() asm sei; TIOS_IOS0=0; /輸入捕捉 TSCR1=0X80; TSCR2=0X07; TCTL4=0X01;/上升沿捕捉 TIE=0X01; /允許硬件中斷 asm cli; /撥碼開關(guān)void boman() if(PORTA_PA0=1) b_rkp0=table_rkp00; b_rkp1=table_rkp01; b_rkp2=table_rkp02; b_rkp3=table_rkp03; b_rkp4=table_rkp04; b_rkp5=table_rkp05; b_rkp

13、6=table_rkp06; b_rkd0=table_rkd00; b_rkd1=table_rkd01; b_rkd2=table_rkd02; b_rkd3=table_rkd03; b_rkd4=table_rkd04; b_rkd5=table_rkd05; b_rkd6=table_rkd06; b_mkp0=table_mkp00; b_mkp1=table_mkp01; b_mkp2=table_mkp02; b_mkp3=table_mkp03; b_mkp4=table_mkp04; b_mkp5=table_mkp05; b_mki0=table_mki00; b_mki

14、1=table_mki01; b_mki2=table_mki02; b_mki3=table_mki03; b_mki4=table_mki04; b_mki5=table_mki05; b_mkd0=table_mkd00; b_mkd1=table_mkd01; b_mkd2=table_mkd02; b_mkd3=table_mkd03; b_mkd4=table_mkd04; b_mkd5=table_mkd05; s_table0=s_ideal00; s_table1=s_ideal01; s_table2=s_ideal02; s_table3=s_ideal03; s_tab

15、le4=s_ideal04; s_table5=s_ideal05; if(PORTA_PA1=1) b_rkp0=table_rkp10; b_rkp1=table_rkp11; b_rkp2=table_rkp12; b_rkp3=table_rkp13; b_rkp4=table_rkp14; b_rkp5=table_rkp15; b_rkp6=table_rkp16; b_rkd0=table_rkd10; b_rkd1=table_rkd11; b_rkd2=table_rkd12; b_rkd3=table_rkd13; b_rkd4=table_rkd14; b_rkd5=ta

16、ble_rkd15; b_rkd6=table_rkd16; b_mkp0=table_mkp10; b_mkp1=table_mkp11; b_mkp2=table_mkp12; b_mkp3=table_mkp13; b_mkp4=table_mkp14; b_mkp5=table_mkp15; b_mki0=table_mki10; b_mki1=table_mki11; b_mki2=table_mki12; b_mki3=table_mki13; b_mki4=table_mki14; b_mki5=table_mki15; b_mkd0=table_mkd10; b_mkd1=ta

17、ble_mkd11; b_mkd2=table_mkd12; b_mkd3=table_mkd13; b_mkd4=table_mkd14; b_mkd5=table_mkd15; s_table0=s_ideal10; s_table1=s_ideal11; s_table2=s_ideal12; s_table3=s_ideal13; s_table4=s_ideal14; s_table5=s_ideal15; if(PORTA_PA2=1) b_rkp0=table_rkp20; b_rkp1=table_rkp21; b_rkp2=table_rkp22; b_rkp3=table_

18、rkp23; b_rkp4=table_rkp24; b_rkp5=table_rkp25; b_rkp6=table_rkp26; b_rkd0=table_rkd20; b_rkd1=table_rkd21; b_rkd2=table_rkd22; b_rkd3=table_rkd23; b_rkd4=table_rkd24; b_rkd5=table_rkd25; b_rkd6=table_rkd26; b_mkp0=table_mkp20; b_mkp1=table_mkp21; b_mkp2=table_mkp22; b_mkp3=table_mkp23; b_mkp4=table_

19、mkp24; b_mkp5=table_mkp25; b_mki0=table_mki20; b_mki1=table_mki21; b_mki2=table_mki22; b_mki3=table_mki23; b_mki4=table_mki24; b_mki5=table_mki25; b_mkd0=table_mkd20; b_mkd1=table_mkd21; b_mkd2=table_mkd22; b_mkd3=table_mkd23; b_mkd4=table_mkd24; b_mkd5=table_mkd25; s_table0=s_ideal20; s_table1=s_id

20、eal21; s_table2=s_ideal22; s_table3=s_ideal23; s_table4=s_ideal24; s_table5=s_ideal25; if(PORTA_PA3=1) b_rkp0=table_rkp30; b_rkp1=table_rkp31; b_rkp2=table_rkp32; b_rkp3=table_rkp33; b_rkp4=table_rkp34; b_rkp5=table_rkp35; b_rkp6=table_rkp36; b_rkd0=table_rkd30; b_rkd1=table_rkd31; b_rkd2=table_rkd3

21、2; b_rkd3=table_rkd33; b_rkd4=table_rkd34; b_rkd5=table_rkd35; b_rkd6=table_rkd36; b_mkp0=table_mkp30; b_mkp1=table_mkp31; b_mkp2=table_mkp32; b_mkp3=table_mkp33; b_mkp4=table_mkp34; b_mkp5=table_mkp35; b_mki0=table_mki30; b_mki1=table_mki31; b_mki2=table_mki32; b_mki3=table_mki33; b_mki4=table_mki3

22、4; b_mki5=table_mki35; b_mkd0=table_mkd30; b_mkd1=table_mkd31; b_mkd2=table_mkd32; b_mkd3=table_mkd33; b_mkd4=table_mkd34; b_mkd5=table_mkd35; s_table0=s_ideal30; s_table1=s_ideal31; s_table2=s_ideal32; s_table3=s_ideal33; s_table4=s_ideal34; s_table5=s_ideal35; /*/賽道特征識別/*/void Roadjudge(void) if(F

23、lag_Chute=0) /普通彎道情況 0 GeneralCtn=0; CurveCtn=0; if(CR>=-23&&CR<=40) /此時對應(yīng)一個車輪的內(nèi)側(cè)壓線 ChuteCtn+; WANCtn=0; if(CR<-23|CR>40) / if(CR<-85|CR>90) /對應(yīng)車輪的外側(cè)壓線 ChuteCtn=0; WANCtn+; else ChuteCtn=0; WANCtn=0; if(ChuteCtn>10000) / 300 Flag_Chute=1; if(WANCtn<-10000) Flag_Chute=

24、2; if(Flag_Chute=1) /長直道情況 1 ChuteCtn=0; GeneralCtn=0; WANCtn=0; if(CR>40)CurveCtn+; / 60 if(CR<-23)CurveCtn-; / -60 if(CurveCtn>1|CurveCtn<-1) if(Flag_gaosu=1) /高速時轉(zhuǎn)入大彎道情況 Flag_Chute=2; if(Flag_gaosu=0) Flag_Chute=0; /低速時轉(zhuǎn)入普通彎道情況 if(Flag_Chute=2) /大彎道情況 2 ChuteCtn=0; CurveCtn=0; WANCtn=

25、0; / if(CR>-23&&CR<35)GeneralCtn+; else GeneralCtn=0; if(GeneralCtn>1300)Flag_Chute=0; /if(GeneralCtn>130)Flag_Ct=1; /else Flag_Ct=0; /if(k_abs(Turn_C-Turn<54) / / Flag_Zhj=1; / / else / / Flag_Zhj=0; / / if(Flag_Pwm=1&&Flag_Ct=1)Flag_Chute=1; /*/舵機(jī)控制*/void rudder_ctrl

26、(void) if(Flag_Chute=0) /普通道 Flag_Pwm=0; if(CR<40&&CR>-23) rkp=b_rkp0; rkd=b_rkd0; else rkp=b_rkp6; rkd=b_rkp6; else if(Flag_Chute=1) /長直道 Flag_Pwm=1; if(Flag_gaosu=0) /disu rkp=b_rkp1; /5 rkd=b_rkd1; else /gaosu rkp=b_rkp2; /4 rkd=b_rkd2; else if(Flag_Chute=2) /大彎道 if(Flag_Pwm=0) /普通道

27、到大彎道 rkp=b_rkp3; rkd=b_rkd3; else if(Flag_Pwm=1) /直道 到 大彎道 rkp=b_rkp4; rkd=b_rkd4; else rkp=b_rkp5; rkd=b_rkd5; f=3800+rkp*CR+rkd*(CR-2*preCR+ppreCR); /舵機(jī)的PID算式 ppreCR=preCR;/計算之后向前推進(jìn)賦值 為下次計算做準(zhǔn)備 preCR=CR; /*電機(jī)控制*/void motor_ctrl1(void) if(Flag_Chute=0) /普通道 Flag_Pwm=0; mkp=b_mkp0; mki=b_mki0; mkd=b_

28、mkd0; ideal_speed=s_table0; else if(Flag_Chute=1) /長直道 Flag_Pwm=1; / Flag_PIDRev=0; mkp=b_mkp1; mki=b_mki1; mkd=b_mkd1; ideal_speed=s_table1; else if(Flag_Chute=2) /大彎道 if(Flag_Pwm=0) /普通道進(jìn)大彎道 mkp=b_mkp2; mki=b_mki2; mkd=b_mkd2; ideal_speed=s_table2; else if(Flag_Pwm=1) /直道進(jìn)大彎道 mkp=b_mkp3; mki=b_mki

29、3; mkd=b_mkd3; ideal_speed=s_table3; else mkp=b_mkp4; mki=b_mki4; mkd=b_mkd4; ideal_speed=s_table4; Flag_Pwm=2; /電機(jī)控制 void motor_ctrl2(void) int error,m_perror,m_ierror,m_derror; int pre_error=0; int ppre_error=0; error=ideal_speed-Pulse_count; m_perror = error - pre_error; m_ierror=error; m_derror=

30、error-2*pre_error+ppre_error; ppre_error=pre_error; pre_error=error; speed+=mkp*m_perror + mki*m_ierror + mkd*m_derror; /速度PID控制算式 if(speed>=9000) speed=9000; if(speed<=-4000) speed=-4000; if(speed>=0) PWMDTY45=0; PWMDTY01=(int)speed/10; else PWMDTY45=(int)(-speed)/10; PWMDTY01=0; /主函數(shù)/void

31、 main(void) DisableInterrupts; PWMDTY23=3800; PLL_Init(); DDRA=0X00;/輸入 boman(); PACBInit(); RTI_init(); PWM_rudder_init(); PWM_init_motor(); AD_Init(); DDRB=0XFF;/輸出 PORTB=0X03;/1號與2號燈亮 delay(5000); /4000 3s左右 PORTB=0xFC;/3號與4號燈亮 Checkstart(); EnableInterrupts; /* put your own code here */ for(;) w

32、hile(!ATD0STAT2_CCF0); / 等待轉(zhuǎn)換結(jié)束while(ATDOSTAT2_CCF0=1) left1=ATD0DR0;/讀取結(jié)果寄存器left1的值 while(!ATD0STAT2_CCF2); / 等待轉(zhuǎn)換結(jié)束while(ATDOSTAT2_CCF1=1) left2=ATD0DR2;/讀取結(jié)果寄存器的值 while(!ATD0STAT2_CCF1); / 等待轉(zhuǎn)換結(jié)束while(ATDOSTAT2_CCF2=1) right1=ATD0DR1;/讀取結(jié)果寄存器的值 while(!ATD0STAT2_CCF3); / 等待轉(zhuǎn)換結(jié)束while(ATDOSTAT2_CCF3

33、=1) right2=ATD0DR3;/讀取結(jié)果寄存器的值 AR_LEFT=left1+left2; AR_RIGHT=right1+right2; CR=(AR_RIGHT-AR_LEFT)/10; if(Pulse_count>65)Flag_gaosu=1; else Flag_gaosu=0; if(AR_RIGHT<110|AR_LEFT<95) / left93 if(pref>3800) /if(AR_RIGHT<110) f=4500; /f=3100; if(pref<3800) /if(AR_LEFT<95) f=3100; /f=

34、4500; mkp=b_mkp5; mki=b_mki5; mkd=b_mkd5; ideal_speed=s_table5; else Roadjudge(); /先對道路進(jìn)行判斷 rudder_ctrl(); /調(diào)整舵機(jī) motor_ctrl1(); /調(diào)整電機(jī)的pid參數(shù) if(f>4500)f=4500; if(f<3100)f=3100; PWMDTY23=f; pref=PWMDTY23; motor_ctrl2(); /* loop forever */ #pragma CODE_SEG _NEAR_SEG NON_BANKED interrupt 7 void Int_TimerOverFlow(void) Pulse_count= PACNT; /脈沖計數(shù)賦值 PACNT