搜索
#includehidef.h/*commondefinesandmacros*/#includemc9s12dp512.h/*derivativeinformation*/#pragmaLINK_INFODERIVATIVEmc9s12dp512#includeSheetdef.h//首先从原文件的位置开始搜索指定文件/*路径识别变量*/uint16t;uint16posmax;uint16i;uint16sensor;//纪录传感器信息，低14位表示从右至左14个传感器信息uint16Control_Light;uint16Light=0;uint16Light_Number=0;uint16Light_Count=1;uint16Sensor_Receive=0;uint16Sensor_K[15]=0;int16posold;//最左侧1的位置int16count1;//纪录1的个数int16posnew;//纪录最左侧1的位置int16tmp;//判断1之间是否有0uint16Left_Sign=0;uint16Right_Sign=0;int16posnnn;29int16posmmm;int16count111;int16posnnn;int16posmmm;/*舵机左右极限*/int16angle_max;int16angle_mid;int16angle_min;/*路径信息处理变量*/int32err1=0;int32err2=0;int32poslong1=16000;int32poslong=16000;int16err_a=0;/*控制变量*/int32err11=0;int32err22=0;int16POS;int16ERR1;int16ERR2;int32PV[5];int32PEV[5];int32PED[5];int32Kp=0;//角度的P变量int32K_Left=0;int32K_Right=0;30int32Kd=0;//角度的D变量int32PID_Mohu=0;//int32P[5]={2,5,8,11,13};//小S弯偏得厉害//int32P[5]={3,5,8,11,14};//19.4sint32P[5]={8,10,12,14,16};//17.9s//int32P[5]={5,7,10,13,16};int32D[5]={0,5,15,25,40};//int32D[5]={0,5,10,20,30};/*int32angle_kd[5][5]={{24,18,15,4,2},{18,15,12,6,4},{8,0,0,0,8},{4,6,12,15,18},{2,4,15,18,24}};*//*int32angle_kd[5][5]={{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}};*/int32speed_fk[5]={15,18,20,25,30};//16.6sint32speed_fk1[5]={20,25,30,35,40};//17.7sint32speed_fk2[5]={20,25,30,35,40};int32speed_fk3[5]={20,25,30,35,40};int32speed_fk4[5]={20,25,30,35,40};int32speed_fk5[5]={20,25,30,35,40};int32speed_fk6[5]={20,25,30,35,40};int32speed_fk7[5]={20,25,30,35,40};int32speed_fk8[5]={20,25,30,35,40};int32PID_rudder_pwm;31int32Mohu_rudder_pwm;int32rudder_pwm;//舵机目标值int32duoji;/*速度闭环控制变量*/int32motorpwm=1000;int32speederr1=0;int32speederr=0;int32speednow=0;int32exspeed=0;int32pwmspeed=0;int32exspeed_z=0;int32exspeedold=0;int32oldmotorpwm=0;int32motorerr=0;int16add_kd=1;uint16flag=0;/*启动，停车变量*/int16Z_flag=0;int16LeftW_flag=0;int16RightW_flag=0;uint32start_k=0;int16k_flag=0;int16kk_flag=0;int16start_acc;int16cross_cnt=0;int16sign=0;int16sign_stop=0;int16s_flag=0;int32delay=0;int32delay2=0;uint32stop;32/*其他变量*/uint16left=0;uint16right=0;uint16s_sign=0;uint16leftS=0;uint16rightS=0;uint16Ssign=1;int32flagS=0;uint16Sign=0;uint16Stop_sign=0;uint16Sign_z=1;uint16sign_z=0;uint16Kd_z=1;uint16Kp_z=1;uint16Kp_w=0;//大S弯KP附加量uint16Extra_Kd=1;uint16RightWan=0;//右弯计数uint16LeftWan=0;//左弯计数uint16LeftSign=0;//左大S弯标志uint16RightSign=0;//右大S弯标志//拨码开关变量uint16PTM_K=0;uint16XSSign1=0;//小S弯计数uint16XSSign2=0;uint16XSflag1=0;uint16XSflag2=0;uint16XS_Sign1=0;uint16XS_Sign2=0;uint16XS_flag=0;33uint16Z_Over=0;intForward_Delay=0;intBack_Delay=0;intSpeed_Data[500];intSpeed_i=0;//激光摆头控制变量int32Light_Err=0;int32Light_RudderPwm;int32Light_angle_mid;int32Light_angle_max;int32Light_angle_min;int32Accumu_Err=0;/*函数声明*/voidsystemboot(void);voidGet_Sensor(void);voidmakedecision(void);voidpath_recog(void);voidPWM_init(void);voidect_init(void);voidSpeedChoice(void);voidScan_Light(void);34voidSpeedChoice(void){PTM=0x00;PTM_K=PTM;//拨码开关接到S口上PTM_K=PTM_K^0xffff;PTM_K=PTM_K&0xff;if(PTM_K==0x01){for(i=0;i=4;i++)speed_fk[i]=speed_fk1[i];}if(PTM_K==0x02){for(i=0;i=4;i++)speed_fk[i]=speed_fk2[i];}if(PTM_K==0x04){for(i=0;i=4;i++)speed_fk[i]=speed_fk3[i];}if(PTM_K==0x08){for(i=0;i=4;i++)speed_fk[i]=speed_fk4[i];}if(PTM_K==0x10){for(i=0;i=4;i++)speed_fk[i]=speed_fk5[i];}if(PTM_K==0x20){35for(i=0;i=4;i++)speed_fk[i]=speed_fk6[i];}if(PTM_K==0x40){for(i=0;i=4;i++)speed_fk[i]=speed_fk7[i];}if(PTM_K==0x80){for(i=0;i=4;i++)speed_fk[i]=speed_fk8[i];}}voidsystemboot(void){CRGFLG_SCM=0;//使用外部时钟SYNR=9;//PLLCLK=2xOSCCLKx[SYNR+1]/[REFDV+1]REFDV=3;PLLCTL=0x71;//打开锁相环电路while(CRGFLG_LOCK==0);//确保系统的频率可以工作CLKSEL=0x80;//PLLSEL=1,意思是时钟频率源自晶振DDRA=0xff;//PortA为输出PORTA=0xff;//cleanPortADDRB=0xff;//PortB为输出PORTB=0xff;//cleanPortBDDRS=0xff;//PortS为输入PTS=0x00;//cleanPortS36DDRM=0x00;PTM=0x00;DDRK=0x00;//PortK为输入PORTK=0x00;PWM_init();ect_init();PID_rudder_pwm=angle_mid;Mohu_rudder_pwm=angle_mid;rudder_pwm=angle_mid;//初始舵机正中posold=16;}voidPWM_init(void){PWMPOL=0xFF;//极性设置，一开始为高PWMCLK=0xAA;//每一个PWM可以选择两个，为SA/SB或者A/B,此处选前者;PWMPRCLK=0x11;//设置频率A=busclock/2=20MHz,B=busclock/2=20MHz;PWMSCLA=0x01;//SA方式的算法：SA=A/2=10M;PWMSCLB=0x05;//SB方式的算法：SB=B/2/5=2M;PWMCTL=0xF0;//级联各个通道，成为16位PWM;PWMCAE=0x00;//左对齐方式PWMPER01=67;//输出频率为SA/67=149.25k;PWMDTY01=35;PWMPER23=40000;//控制激光管摆头舵机PWM输出通道PWMDTY23=Light_angle_mid;PWMPER45=2000;//输出频率的算法：SAclock/2000=5KHz;PWMDTY45=1000;//初始时刻占空比;37PWMPER67=40000;//23通道的输出频率：SBclock/40000=50Hz;PWMDTY67=angle_mid;//初始时刻占空比；PWME=0xAA;//打开01，23，45通道（只需开通高位即可）}voidect_init(void){//模数递减方式RTICTL=0X8B;CRGFLG_RTIF=1;CRGINT_RTIE=1;PACTL_PAMOD=0;PACTL_PEDGE=1;PACTL_PAEN=1;}voidScan_Light(void){Light_Err=posnew-16;Light_RudderPwm=Light_angle_mid-10*Light_Err;if(Light_RudderPwmLight_angle_max)Light_RudderPwm=Light_angle_max;if(Light_RudderPwmLight_angle_min)Light_RudderPwm=Light_angle_min;PWMDTY23=Light_RudderPwm;}voidctrl_speed(void){speederr1=speederr;speederr=pwmspeed-speednow;motorpwm=PWMDTY45+0.2*speederr+11*(speederr-spe