飞思卡尔光电组程序Word格式.docx

1、 / e（t） float set_ser = 0; float inc_ser ; 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_Ini

2、t（） TIOS=0x01; /定时器通道0设置为输出比较 TC0=100000; /赋初值，当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; sudu=PACNT ; /读取脉冲数 /setspeed（）; PACNT =0; #pragma CODE_SEG DEFAULT/ /初始化/ vo

3、id base_Init（void） 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（）;/pulse初始化/int base_Puls

4、e_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 = （p_s

5、tart（pchannel*2+1）; /PWME &=p_start; bpwm+=pchannel; /*bpwm=0x0000; bpwm+=4; *bpwm=pper; *bpwm=pdty; if（pper!=0） p_start=0x01; p_start=p_start=0） 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;ims;i+）f

6、or（j=0;j=800） speedpwm=800; else if（speedpwm=700） speedpwm=700; core_Car_speed_set（speedpwm）;void setspeed1（） int idealspeed=7600;static int speedpwm=950;=1000） speedpwm=1000;=900） speedpwm=900;/激光头点亮 流水灯/ void light（） PORTB=0x00; sleep（1）; dushu4=PORTD_PD4; dushu9=PORTC_PC0; PORTB=0x11; dushu1=PORT

7、D_PD1; PORTB=0x22; dushu5=PORTD_PD5; dushu2=PORTD_PD2; PORTB=0x33; PORTB=0x44; dushu6=PORTD_PD6; dushu3=PORTD_PD3; PORTB=0x55; PORTB=0x66; dushu7=PORTD_PD7; dushu8=PORTD_PD0; /转弯判断/ void turn（） light（）; /直道/ if（dushu8=1&dushu7=1） setspeed（）; CarControl（0）; /左转 else if（dushu4=1&dushu7=1&dushu1=0&dush

8、u2=0&dushu3=0&dushu5=0&dushu6=0&dushu8=0） /eft_direction（-11）; CarControl（-9）;dushu5=1& setspeed1（）; /left_direction（-13）; CarControl（-12）;dushu6=1& /left_direction（-14）; CarControl（-14）; /右转 else if（dushu8=1&dushu1=1&dushu4=0&dushu7=0） /right_direction（8）; CarControl（7）; else if（dushu1=1&dushu2=1&d

9、ushu8=1& /right_direction（9）; CarControl（8）;dushu3=1& /right_direction（11）; CarControl（11）; /弯道/ /右转 else if（dushu3=1&dushu8=1） /left_direction（-16）; CarControl（20）;dushu6=1） /left_direction（-30）; CarControl（30）;dushu5=1） /setspeed1（）; core_Car_speed_set（1000）;dushu1=1） /left_direction（-11）; /左转 /ri

10、ght_direction（12）;dushu3=1） /right_direction（22）; CarControl（-22）;dushu2=1） CarControl（-25）; CarControl（-28）; else if（dushu6=1& / right_direction（12）; / else if（dushu1=0&dushu8=0& void CarControl（float measure） /舵机输出增量 float inc_ser; /舵机上一次的输出值，初始化为1500 static float out_pwm = 9250; /舵机变增益控制系数 float

11、servo_kp = 0.4; float servo_kd = 0.2; n_ser_error = measure - set_ser; /这里是测量值 - 设定值 /变增益控制 inc_ser = servo_kp * SERVO_KP * （n_ser_error - l_ser_error） + servo_kd * SERVO_KD * （n_ser_error - 2 * l_ser_error + p_ser_error）; out_pwm += inc_ser; / 初始就为9250 p_ser_error = l_ser_error; l_ser_error = n_ser_error; /防止超限,阈值到时要重新测一测 if（out_pwm = 9500） out_pwm = 9500; base_Pwm_set（PWM_CHANNEL3,10000,out_pwm）;