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);
copyright@ 2008-2022 冰豆网网站版权所有
经营许可证编号:鄂ICP备2022015515号-1