飞思卡尔光电组程序

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 /微分增益/函数声明/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; /主函数/ void main(void) DDRB=0XFF; DDRD=0X00; DDRC=0X00;EnableInterrupts; base_Init(); core_Car_speed_set(750); for(;) turn(); _FEED_COP(); /定时测脉冲/ void Time0_Init() TIOS=0x01; /定时器通道0设置为输出比较 TC0=1000

4、00; /赋初值，当TCNT从0计数到此值时第一次进入中断 TCTL2=0x01; TSCR2=0x07; /计一个数用5.33微秒 TSCR1=0x80; TIE=0x01; /中断： #pragma CODE_SEG _NEAR_SEG NON_BANKED interrupt 8 TIM0(void) TFLG1_C0F=1;/清中断标志位 TIE=0x00; TC0=100000; sudu=PACNT ; /读取脉冲数 /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(); /锁相环设置/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计数器/pwm设置/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; /速度设置/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);/方向设置/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+) /调整速度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; /实际速度 error_sp=idealspeed-realspeed; /当前误差 计算积分项I errord_sp=error_sp-errorlast_sp; /计算比例项P errordd_sp=errord_sp-errordlast_sp; /计算微分项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; /实际速度 error_sp=idealspeed-realspeed; /当前误差 计算积分项I errord_sp=error_sp-errorlast_sp; /计算比例项P errordd_sp=errord_sp-errordlast_sp; /计算微分项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); /激光头点亮 流水灯/ 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; /转弯判断/ void turn() light(); /直道/ if(dushu8=1&&dushu7=1) se

15、tspeed(); CarControl(0); /左转 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); /右转 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、道/ /右转 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); /左转 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); /