飛思卡爾光電組程序

1、/頭文件/#include <hidef.h> #include "derivative.h" #include "mc9s12g128.h"#include "base.h"/#define idealspeed 12000#define SERVO_KP 31.5 /比例增益#define SERVO_KD 6.3 /微分增益/函數(shù)聲明/void base_Init(void);void sleep(int ms);void Time0_Init();void setspeed();void light();void

2、turn();void CarControl(float measure);void setspeed1();/變量定義/ int k,sudu; int jiaodu=0; int m=0,i=0; int dushu6,dushu5,dushu4,dushu3,dushu2,dushu1,dushu7,dushu8,dushu9; float l_ser_error = 0; / e(t - 1) float p_ser_error = 0; / e(t - 2) float n_ser_error = 0; / e(t) float set_ser = 0; float inc_ser

3、; float servo_kp; /PID變量定義/ typedef struct char sp_p; char sp_d; char sp_i;sp_pid;sp_pid pid=11,3,42; /主函數(shù)/ void main(void) DDRB=0XFF; DDRD=0X00; DDRC=0X00;EnableInterrupts; base_Init(); core_Car_speed_set(750); for(;) turn(); _FEED_COP(); /定時(shí)測(cè)脈沖/ void Time0_Init() TIOS=0x01; /定時(shí)器通道0設(shè)置為輸出比較 TC0=1000

4、00; /賦初值，當(dāng)TCNT從0計(jì)數(shù)到此值時(shí)第一次進(jìn)入中斷 TCTL2=0x01; TSCR2=0x07; /計(jì)一個(gè)數(shù)用5.33微秒 TSCR1=0x80; TIE=0x01; /中斷： #pragma CODE_SEG _NEAR_SEG NON_BANKED interrupt 8 TIM0(void) TFLG1_C0F=1;/清中斷標(biāo)志位 TIE=0x00; TC0=100000; sudu=PACNT ; /讀取脈沖數(shù) /setspeed(); PACNT =0; TIE=0x01; #pragma CODE_SEG DEFAULT/ /初始化/ void base_Init(voi

5、d) base_Pll_init(); base_Pwm_init(); base_Pulse_count_init(); Time0_Init(); /鎖相環(huán)設(shè)置/int base_Pll_init(void) CPMUCLKS=0x00; CPMUSYNR=0x00 | 0x02; CPMUREFDIV=0x80 | 0x01; CPMUPOSTDIV=0x00; CPMUOSC_OSCE=1; while(!(CPMUFLG_LOCK=1); CPMUCLKS_PLLSEL=1; _DISABLE_COP(); EnableInterrupts;/pulse初始化/int base_Pu

6、lse_count_init() PACTL=0x50; PACNT = 0x0000; /清0計(jì)數(shù)器/pwm設(shè)置/int base_Pwm_init(void) PWME=0x00; PWMCLK=0xff; PWMPRCLK=0x30; PWMSCLA=0x01; PWMSCLB=0x03; PWMPOL=0x00; PWMCAE=0x00; PWMCTL=0xf0; int base_Pwm_set(Pwm_Channel pchannel,int pper,int pdty) int *bpwm=(int*)0x00ac; uchar p_start=0x01; p_start = (

7、p_start<<(pchannel*2+1); /PWME &=p_start; bpwm+=pchannel; /*bpwm=0x0000; bpwm+=4; *bpwm=pper; bpwm+=4; *bpwm=pdty; if(pper!=0) p_start=0x01; p_start=p_start<<(pchannel*2+1); PWME |=p_start; /速度設(shè)置/int core_car_speed_setting=0;int core_Car_speed_start() base_Pwm_set(PWM_CHANNEL0,1200,1

8、200); base_Pwm_set(PWM_CHANNEL2,1200,1200);int core_Car_speed_set(int speed) base_Pwm_set(PWM_CHANNEL0,1200,1200); base_Pwm_set(PWM_CHANNEL2,1200,speed); int core_Car_speed_close() base_Pwm_set(PWM_CHANNEL0,0,0); base_Pwm_set(PWM_CHANNEL2,0,0);/方向設(shè)置/int core_Car_direction_set(int size) if(size>=0

9、) base_Pwm_set(PWM_CHANNEL3,10000,9250+250*size/45); else base_Pwm_set(PWM_CHANNEL3,10000,9250-250*abs(size)/45); void sleep(int ms) int i,j;for(i=0;i<ms;i+)for(j=0;j<2003;j+) /調(diào)整速度PID/void setspeed() int idealspeed=17750;static int speedpwm=700;static int error_sp=0,errorlast_sp=0,errord_sp=0

10、,errordlast_sp=0,errordd_sp=0; int realspeed; realspeed=sudu; /實(shí)際速度 error_sp=idealspeed-realspeed; /當(dāng)前誤差 計(jì)算積分項(xiàng)I errord_sp=error_sp-errorlast_sp; /計(jì)算比例項(xiàng)P errordd_sp=errord_sp-errordlast_sp; /計(jì)算微分項(xiàng)D errorlast_sp=error_sp; /前一次誤差 errordlast_sp=errord_sp; /前兩次誤差 speedpwm=speedpwm+pid.sp_p*errord_sp+pid.

11、sp_d*errordd_sp/10+pid.sp_i*error_sp/10; if(speedpwm>=800) speedpwm=800; else if(speedpwm<=700) speedpwm=700; core_Car_speed_set(speedpwm); void setspeed1() int idealspeed=7600;static int speedpwm=950;static int error_sp=0,errorlast_sp=0,errord_sp=0,errordlast_sp=0,errordd_sp=0; int realspeed;

12、 realspeed=sudu; /實(shí)際速度 error_sp=idealspeed-realspeed; /當(dāng)前誤差 計(jì)算積分項(xiàng)I errord_sp=error_sp-errorlast_sp; /計(jì)算比例項(xiàng)P errordd_sp=errord_sp-errordlast_sp; /計(jì)算微分項(xiàng)D errorlast_sp=error_sp; /前一次誤差 errordlast_sp=errord_sp; /前兩次誤差 speedpwm=speedpwm+pid.sp_p*errord_sp+pid.sp_d*errordd_sp/10+pid.sp_i*error_sp/10; if(s

13、peedpwm>=1000) speedpwm=1000; else if(speedpwm<=900) speedpwm=900; core_Car_speed_set(speedpwm); /激光頭點(diǎn)亮 流水燈/ void light() PORTB=0x00; sleep(1); dushu4=PORTD_PD4; dushu9=PORTC_PC0; PORTB=0x11; sleep(1); dushu4=PORTD_PD4; dushu1=PORTD_PD1; PORTB=0x22; sleep(1); dushu5=PORTD_PD5; dushu2=PORTD_PD2

14、; PORTB=0x33; sleep(1); dushu5=PORTD_PD5; dushu2=PORTD_PD2; PORTB=0x44; sleep(1); dushu6=PORTD_PD6; dushu3=PORTD_PD3; PORTB=0x55; sleep(1); dushu6=PORTD_PD6; dushu3=PORTD_PD3; PORTB=0x66; sleep(1); dushu7=PORTD_PD7; dushu8=PORTD_PD0; /轉(zhuǎn)彎判斷/ void turn() light(); /直道/ if(dushu8=1&&dushu7=1) se

15、tspeed(); CarControl(0); /左轉(zhuǎn) else if(dushu4=1&&dushu7=1&&dushu1=0&&dushu2=0&&dushu3=0&&dushu5=0&&dushu6=0&&dushu8=0) setspeed(); /eft_direction(-11); CarControl(-9); else if(dushu4=1&&dushu7=1&&dushu5=1&&dushu1=0&&am

16、p;dushu2=0&&dushu3=0&&dushu6=0&&dushu8=0) setspeed1(); /left_direction(-13); CarControl(-12); else if(dushu4=1&&dushu7=1&&dushu5=1&&dushu6=1&&dushu1=0&&dushu2=0&&dushu3=0&&dushu8=0) setspeed1(); /left_direction(-14); Car

17、Control(-14); /右轉(zhuǎn) else if(dushu8=1&&dushu1=1&&dushu2=0&&dushu3=0&&dushu5=0&&dushu6=0&&dushu4=0&&dushu7=0) setspeed(); /right_direction(8); CarControl(7); else if(dushu1=1&&dushu2=1&&dushu8=1&&dushu3=0&&dushu5=0&a

18、mp;&dushu6=0&&dushu4=0&&dushu7=0) setspeed1(); /right_direction(9); CarControl(8); else if(dushu1=1&&dushu2=1&&dushu3=1&&dushu8=1&&dushu5=0&&dushu6=0&&dushu4=0&&dushu7=0) setspeed1(); /right_direction(11); CarControl(11); /彎

19、道/ /右轉(zhuǎn) else if(dushu3=1&&dushu2=1&&dushu1=1&&dushu8=1) setspeed1(); /left_direction(-16); CarControl(20); else if(dushu3=1&&dushu2=1&&dushu1=1&&dushu8=1&&dushu6=1) setspeed1(); /left_direction(-30); CarControl(30); else if(dushu3=1&&dus

20、hu2=1&&dushu1=1&&dushu8=1&&dushu6=1&&dushu5=1) /setspeed1(); core_Car_speed_set(1000); CarControl(30); else if(dushu8=1&&dushu1=1) setspeed1(); /left_direction(-11); CarControl(11); /左轉(zhuǎn) else if(dushu4=1&&dushu5=1&&dushu6=1&&dushu7=1) se

21、tspeed1(); /right_direction(12); CarControl(-12); else if(dushu4=1&&dushu5=1&&dushu6=1&&dushu7=1&&dushu3=1) setspeed1(); /right_direction(22); CarControl(-22); else if(dushu4=1&&dushu5=1&&dushu6=1&&dushu7=1&&dushu3=1&&dushu2=1)

22、/setspeed1(); core_Car_speed_set(1000); CarControl(-25); else if(dushu4=1&&dushu5=1&&dushu6=1&&dushu7=1&&dushu3=1&&dushu2=1&&dushu1=1) /setspeed1(); core_Car_speed_set(1000); CarControl(-28); else if(dushu6=1&&dushu7=1) setspeed1(); / right_direction(12); CarControl(-12); /