飞思跑的不错pid程序

1、#include /* common defines and macros */#include /* derivative information */#pragma LINK_INFO DERIVATIVE mc9s12xs128/*变量*/*角变量*/unsigned char sam_atd_g13;/传感器坐标标志位/ATD 电压比较阀/舵机转角/车的当前位置unsignedunsignedadc_limit;angle_data;/car_pre_itn;itn;unsignedblack_sensor_number; itn_temp10;t;/检测到黑线的传感器个数/unsig

2、ned/*chen 速变量*/unsigned unsignedunsignedcar_driver; pulse_count; ideal_speed;speed_error; pre_error;pre_d_error;/驱动力参数/电机当前速度/理想状态下的速度/理想速度与当前速度的差值/控制的速度差值控制的速度上一次差值pk;/速度的值/*标志变量*/unsigned char start_line_add;/unsigned char finish_start_flag;/检测起跑线的次数/完成起跑线检测标志位 为 1 表示 3 次检测到起跑线/unsigned/unsignedti

3、mes;circle;/控制跑几圈停车/驱动标志/刹车标志/统计时间/显示时间unsigned char dri_flag;unsigned char brake_flag;/unsigned/unsigned/unsignedthe_time;show_time;start_flag, start_count;/开始标志/*表格值*/unsigned char speed_table111=26,25,24,20,18,18,18,17,14,14,10; unsigned char speed_table211=24,23,20,19,17,17,17,15,12,11,10;#defin

4、e kp 2000#define ki 5#define kd 10#define Angle_Center/宏定义/宏定义/宏定义/宏定义/宏定义/40ms1055/#define lose_limit 30000#define TIME_ER 40/#define car_driver 10000/*函数定义*/void data_init(void);void SetBusCLK_32M(void); void atd_init(void);/关键数据初始化/锁相环初始化/模/数转化初始化void_init(void);/脉冲初始化void TIM_init(void);/测速模块 TI

5、M 初始化/模/数转化模块函数/检测起跑线函数/计算当前模型车的位置函数/计算舵机的转角值函数/计算车的速度控制函数/车的舵机和电机的驱动函数/关键参数实时修改函数/延时函数void sam_ition(void);/void check_start(void);void car_ition(void);void angle(void); void speed(void); void driver(void); void pre_start(void); void delay(void);/void found_start(void);void stop(void);/停车控制函数void(v

6、oid);/速度的控制函数unsignedabsolute();/传感器坐标负值的转化为正值函数/*主程序*/void main(void)data_init(); SetBusCLK_32M();/TIM_init(); atd_init();/_init();/先初始化各个初始化函数/设置基本参数/锁相环初始化/TIM 初始化/ATD 初始化/初始化/pre_start();Enablefor(;)errupts;/控制车行驶的函数sam_ition();/读 ATD 转化结果/检测起跑线/计算车的位置/计算舵机的转角/计算车的速度/驱动控制/check_start();/car_itio

7、n();angle();speed();/ driver();/*初始化函数*/*数据初始化*/ void data_init(void)adc_limit=153;/start_line_add=0;/ finish_start_flag=0;/ DDRA=0XFF;/ DDRB=0XFF;/ PORTA=0X3F;/ PORTB=0X3F;/ times=0;/比较电压阀值的设置 为 1 伏/检测起跑线次数/完成起跑线检测标志位/I/O 通信端口用于数码管/I/O 通信端口用于数码管/I/O 通信端口用于数码管/I/O 通信端口用于数码管/*锁相环初始 hua*/ void SetBusC

8、LK_32M(void)CLKSEL=0X00; PLLCTL_PLLON=1; SYNR=3; REFDV=1;_asm(nop);_asm(nop);/disengage PLL to system/turn on PLL/pllclock=2*osc*(1+SYNR)/(1+REFDV)=64MHz;/BUS CLOCK=32Mwhile(!(CRG_LOCK=1);/when pll is steady ,then use it;/engage PLL to system;CLKSEL_PLLSEL=1;/*ATD 初始化*/ void atd_init(void)ATD0CTL1=0

9、 x00; /8 位精度，ATD0CTL2=0X40; ATD0CTL3=0XFA;ATD0CTL4=0X07;/快速清除标志位,外部触发，不使能中断/右对齐方式 完成转换后冻结 转换序列长度为 15/采样时间为4*ATD_clock,ATD_CLOCK=32/(PRS+1)*0.5=2MHZATD0CTL5=0X30;ATD0DIEN =0 x00; /多通道转换和连续转换数字输入/*TIM 初始化*/ void TIM_init(void)TIOS=0X00; TCTL4=0X01;TSCR2=0X03;/设置 PT 口为输入/设置上升沿为捕捉/设置 TIMECOLK=BUSCOLK/8=

10、4MHZ/清除所有中断标志/清除 TOF 位TT1=0XFF;2=0X80;TIE=0X00;/TC0 通道使能TSCR1=0XA0;/TC 功能使能/TCTL4=0X01;/ PBCTL=0X40;背景调试模式下定时器/设置捕捉上升沿/级联两个 8 位累加器(PAC0 和 PAC1)/0=0X0000; T=60000;MCCTL=0XC7;/设置计算时间为 40ms(60000*24M/16)/ TSCR1=0X10;/*初始*/void_init(void)CTL=0XE0; PRCLK=0X11; SCLA=0X08; SCLB=0X08; CLK=0XA8; POL=0XA8;CAE

11、=0X00;/设置 6,72,34,6通道连级/SA_COLK=16MHZSB_COLK=16MHZ/COLKSA=1MHZ/COLKSB=1MHZ/23 选择SB_CLK4.5=SA6.7=SB/极性选择起始为/左对齐方式T23=0X0000; T45=0X0000; T67=0X0000;PER23=20000; PER45=100; PER67=100;DTY23=1055;/设置周期为 20ms23 驱动舵机/F=10kHZ/F=10kHZ/舵机回正23 驱动电机反转67 驱动电机正转E_E_ E_E3=1;E5=0; E7=0;/*参数实时修改程 xu*/ void pre_star

12、t(void)unsignedi;/DTY01=Angle_Center;E_E1=1;/ for(i=0;i50;i+)DTY67=0;DTY01=DTY01+100;/DTY23=0;/ for(i=0;i10;i+) delay();/DDRH=0X00; if(PTH_PTH0)else if(PTH_PTH1)else if(PTH_PTH2)/设置 H 端口为输入/不停车/跑一圈停车/跑二圈停车circle=0; circle=2;circle=3;/*循环控制函数*/读 ATD 转换值void sam_ition(void)unsigned char i;/ delay(); s

13、am_atd_g1=ATD0DR10L; sam_atd_g2=ATD0DR9L; sam_atd_g3=ATD0DR8L; sam_atd_g4=ATD0DR7L; sam_atd_g5=ATD0DR6L; sam_atd_g6=ATD0DR5L; sam_atd_g7=ATD0DR4L; sam_atd_g8=ATD0DR3L; sam_atd_g9=ATD0DR2L; sam_atd_g10=ATD0DR1L; sam_atd_g11=ATD0DR0L;for(i=1;iadc_limit)sam_atd_gi=0;elsesam_atd_gi=1;/检测起跑线*/*void check

14、_start(void)unsigned char i,j=0; if(circle)start_flag=0;for(i=0;i=4) start_flag=1;if(sam_atd_gisam_atd_gi+1)j+;if(start_flag)start_count+;elseifstart_count=0;*/*计算车的当前位置*/void car_ition(void)unsignedi,k;/k=0;temp;black_sensor_number=0;for(i=1;i0)dri_flag=1;/驱动标志k*=2; k/=black_sensor_number;k-=12;car

15、_itn=k;/计算出来的车位置/*计算舵机的转角数值*/ void angle(void)if(car_itn0)angle_data=Angle_Center+10*car_elseitn;angle_data=Angle_Center-10*absolute(car_itn);/*计算车的速度*/ void speed(void)/if(PTH_PTH7)ideal_speed=speed_table1absolute(car_itn);/*else if(PTH_PTH6)*/ ideal_speed=speed_table2absolute(car_itn);speed_error=

16、ideal_speed-pulse_count;/speed_error 为当前的速度之差brake_flag=1; if(speed_error=0)car_driver=42; else if(speed_error-3)();else if(speed_error-5)/刹车标志 1 为正驱动,0 为制动/情况 1:全加速/情况 2:用/情况 3:反转制动 1elsebrake_flag=0; car_driver=70; /情况 4:反转制动 2 brake_flag=0; car_driver=70; /*负值转为正值*/unsignedabsolute(x)if(x0)x=-x;r

17、eturn(x);/*驱动电机和舵机*/ void driver(void)unsignedfor(i=0;i14;i+) for(j=0;j25;j+)i,j;DTY23=DTY23=DTY23+20;DTY23-20;/DTY01=Angle_Center+10;DTY01=Angle_Center-10;/DTY01=angle_data; unsignedi,j, car_dr,k,x;k=0; x=0;car_dr=10000; for(i=1;i30;i+)/*if(brake_flag)*/ /E_E_E7=1;DTY67=5000;/for(j=0;j30;j+)E3=1;x=

18、car_dr+100;DTY23=x;/if(dri_flag=1)else if (dri_flag=2)DTY67=car_driver;DTY67=8000;/正常驱动/保持滑行elseDTY67=0;/滑行/if(finish_start_flag)stop();/停车/*控制*/void(void)signederror, d_error, dd_error;/error=speed_errorerror=ideal_speed-pulse_count; d_error=error-pre_error; dd_error=d_error-pre_d_error; pre_error=error;pre_d_error=d_error;/d_error 当前速度差与上一次速度差之差/当前偏差pk+=kp*d_error+ki*error+kd*dd_error;if(pk=70)pk=70;car_drivk;/*停车控制*/*void stop()E=0X02;/PORable(show_time/25)%10;/PORTB=table(show_time%25)*2/5;*/