飞思卡尔光电组源程序-创新电子社

1、#include /* common defines and macros */ #include /* derivative information */ #pragma LINK_INFO DERIVATIVE mc9s12dp512 #include Sheetdef.h / 首先从原文件的位置开始搜索指定文件 /*路径识别变量*/ uint16 t; uint16 posmax; uint16 i; uint16 sensor; /纪录传感器信息，低 14位表示从右至左 14个传感器信息 uint16 Control_Light; uint16 Light=0; uint16 Ligh

2、t_Number=0; uint16 Light_Count=1; uint16 Sensor_Receive=0; uint16 Sensor_K15=0; int16 posold;/最左侧1的位置 int16 count1;/纪录 1的个数 int16 posnew;/纪录最左侧 1的位置 int16 tmp;/判断1之间是否有 0 uint16 Left_Sign=0; uint16 Right_Sign=0; int16 posnnn; 29 int16 posmmm; int16 count111; int16 posnnn; int16 posmmm; /*舵机左右极限*/ in

3、t16 angle_max; int16 angle_mid; int16 angle_min; /*路径信息处理变量*/ int32 err1=0; int32 err2=0; int32 poslong1=16000; int32 poslong=16000; int16 err_a=0; /*控制变量*/ int32 err11=0; int32 err22=0; int16 POS; int16 ERR1; int16 ERR2; int32 PV5; int32 PEV5; int32 PED5; int32 Kp=0;/角度的 P变量 int32 K_Left=0; int32 K

4、_Right=0; 30 int32 Kd=0;/角度的 D变量 int32 PID_Mohu=0; /int32 P5=2,5,8,11,13; /小 S弯偏得厉害 /int32 P5=3,5,8,11,14; /19.4s int32 P5=8,10,12,14,16; /17.9s /int32 P5=5,7,10,13,16; int32 D5=0,5,15,25,40; /int32 D5=0,5,10,20,30; /*int32 angle_kd55 = 24,18,15,4,2, 18, 15 ,12, 6,4, 8, 0, 0, 0, 8, 4, 6, 12, 15,18,

5、2,4,15,18,24 ;*/ /*int32 angle_kd55 = 22,16,15,10,8, 16, 13, 10, 5,4, 2, 0, 0, 0, 2, 4, 5, 10, 13,16, 8,10,15,16,22 ;*/ int32 speed_fk5=15,18,20,25,30;/16.6s int32 speed_fk15=20,25,30,35,40;/17.7s int32 speed_fk25=20,25,30,35,40; int32 speed_fk35=20,25,30,35,40; int32 speed_fk45=20,25,30,35,40; int3

6、2 speed_fk55=20,25,30,35,40; int32 speed_fk65=20,25,30,35,40; int32 speed_fk75=20,25,30,35,40; int32 speed_fk85=20,25,30,35,40; int32 PID_rudder_pwm; 31 int32 Mohu_rudder_pwm; int32 rudder_pwm;/舵机目标值 int32 duoji; /*速度闭环控制变量*/ int32 motorpwm=1000; int32 speederr1=0; int32 speederr=0; int32 speednow=0

7、; int32 exspeed=0; int32 pwmspeed=0; int32 exspeed_z=0; int32 exspeedold=0; int32 oldmotorpwm=0; int32 motorerr=0; int16 add_kd=1; uint16 flag=0; /*启动，停车变量*/ int16 Z_flag=0; int16 LeftW_flag=0; int16 RightW_flag=0; uint32 start_k=0; int16 k_flag=0; int16 kk_flag=0; int16 start_acc; int16 cross_cnt=0

8、; int16 sign=0; int16 sign_stop=0; int16 s_flag=0; int32 delay=0; int32 delay2=0; uint32 stop; 32 /*其他变量*/ uint16 left=0; uint16 right=0; uint16 s_sign=0; uint16 leftS=0; uint16 rightS=0; uint16 Ssign=1; int32 flagS=0; uint16 Sign=0; uint16 Stop_sign=0; uint16 Sign_z=1; uint16 sign_z=0; uint16 Kd_z=

9、1; uint16 Kp_z=1; uint16 Kp_w=0;/大 S弯KP 附加量 uint16 Extra_Kd=1; uint16 RightWan=0;/右弯计数 uint16 LeftWan=0; /左弯计数 uint16 LeftSign=0; /左大 S弯标志 uint16 RightSign=0; /右大S弯标志 /拨码开关变量 uint16 PTM_K=0; uint16 XSSign1=0; /小S弯计数 uint16 XSSign2=0; uint16 XSflag1=0; uint16 XSflag2=0; uint16 XS_Sign1=0; uint16 XS_S

10、ign2=0; uint16 XS_flag=0; 33 uint16 Z_Over=0; int Forward_Delay=0; int Back_Delay=0; int Speed_Data500; int Speed_i=0; /激光摆头控制变量 int32 Light_Err=0; int32 Light_RudderPwm; int32 Light_angle_mid; int32 Light_angle_max; int32 Light_angle_min; int32 Accumu_Err=0; /* 函数声明*/ void systemboot(void); void Ge

11、t_Sensor(void); void makedecision(void); void path_recog(void); void PWM_init(void); void ect_init(void); void SpeedChoice(void); void Scan_Light(void); 34 void SpeedChoice(void) PTM=0x00; PTM_K=PTM;/拨码开关接到 S口上 PTM_K=PTM_K0xffff; PTM_K=PTM_K&0xff; if(PTM_K=0x01) for(i=0;i=4;i+) speed_fki=speed_fk1i;

12、 if(PTM_K=0x02) for(i=0;i=4;i+) speed_fki=speed_fk2i; if(PTM_K=0x04) for(i=0;i=4;i+) speed_fki=speed_fk3i; if(PTM_K=0x08) for(i=0;i=4;i+) speed_fki=speed_fk4i; if(PTM_K=0x10) for(i=0;i=4;i+) speed_fki=speed_fk5i; if(PTM_K=0x20) 35 for(i=0;i=4;i+) speed_fki=speed_fk6i; if(PTM_K=0x40) for(i=0;i=4;i+)

13、speed_fki=speed_fk7i; if(PTM_K=0x80) for(i=0;iLight_angle_max) Light_RudderPwm=Light_angle_max; if(Light_RudderPwm1999) motorpwm=1999; 38 if(motorpwm2000) Get_Sensor(); delay2=0; */ Get_Sensor(); Scan_Light(); makedecision(); err_a = posnew- 16; 39 if(err_aangle_max) rudder_pwm = angle_max; if(rudde

14、r_pwm angle_min) rudder_pwm = angle_min; PWMDTY67=rudder_pwm; ctrl_speed(); if(start_k20000) start_k=20000; if(cross_cnt=1) delay+; if(delay600) delay=0; sign_stop+; cross_cnt=0; 40 if(sign_stop=6) sign+; if(sign=2) /PWMDTY45=1000; posold=posnew; PTS=0x00; EnableInterrupts; /识别传感器信息 uint16 temp; voi

15、d Get_Sensor(void) Control_Light=0xffff; / PORTA=0xff; / PORTB=0xff; Sensor_Receive=PORTK; Sensor_Receive=Sensor_Receive0xff; Sensor_Receive=Sensor_Receive&0x1f; t=0x017; PORTA=Light&0xff;/A 口低七位用来控制左边7个激光管的亮灭，次高位对应第一个管 if(Light_Number=15) Light_Number=0; sensor=0; temp=0; Light_Count=1; for(i=0;i0)

16、 temp+=Light_Count; else temp+=0; Light_Count=Light_Count*2; sensor=temp; void path_recog(void) t = 0x01; 42 count1 = 0; tmp = 1; posmax = 0; for(i = 1; i 0) if(tmp = 0 & count1 0) return; count1 +; posnew = i; tmp = 1; if(posmax = 0) posmax = i; else tmp = 0; t *= 2; posmax *= 2; posnew *= 2; if(co

17、unt10&count1=3&(posmax-posnew)=(count1*2)&(abs(posnew - posold)14) posnew=2; if(posnew!=2) Left_Sign=0; /从右边丢线，则黑线应该是最先被右边激光管检测到 if(count1=0&posold=30) Right_Sign=1; if(Right_Sign) if(posold-posnew)14) posnew=30; if(posnew!=30) Right_Sign=0; /根据pwmold,count1old做出决策 void makedecision(void) 44 PV0=0;

18、PV1=0; PV2=0; PV3=0; PV4=0; PEV0=0;/控制速度 PEV1=0; PEV2=0; PEV3=0; PEV4=0; PED0=0;/控制舵机 PED1=0; PED2=0; PED3=0; PED4=0; PID_Mohu=0; poslong1 = poslong; poslong = (poslong * 10 + posnew * 1000)/11; err1 = (err1*40 + 100*(poslong - poslong1)/41; err2 = (err2*30 + err1)/31; if(posnew=6&posnew12&posnew=20

19、&posnew26) PID_Mohu=1; if(posnew=6) PV0=10; else if(6posnew&posnew=10) PV0=(100-10*posnew)/4; PV1=10-PV0; else if(10posnew&posnew=16) PV1=(160-10*posnew)/6; PV2=10-PV1; else if(16posnew&posnew=22) PV2=(220-10*posnew)/6; PV3=10-PV2; else if(22posnew&posnew26) PV3=(260-10*posnew)/4; PV4=10-PV3; else PV4=10; if(err11-5000) PED0=10; else if(err11=-5000) PED0=-(err11+3000)/200; 46 PED1=10-PED0; else if(err11-3000&err11-500&err11500&err