loading#include
#include
#include \ // 首先从原文件的位置开始搜索指定文件 /*路径识别变量*/ uint16 t; uint16 posmax;
uint16 i;
uint16 sensor; //纪录传感器信息，低 14位表示从右至左 14个传感器信息 uint16 Control_Light; uint16 Light=0;
uint16 Light_Number=0; uint16 Light_Count=1; uint16 Sensor_Receive=0; uint16 Sensor_K[15]=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; /*舵机左右极限*/ int16 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 PV[5];
int32 PEV[5];
int32 PED[5];
int32 Kp=0;//角度的 P变量 int32 K_Left=0; int32 K_Right=0;
30
int32 Kd=0;//角度的 D变量
int32 PID_Mohu=0;
//int32 P[5]={2,5,8,11,13}; //小 S弯偏得厉害 //int32 P[5]={3,5,8,11,14}; //19.4s int32 P[5]={8,10,12,14,16}; //17.9s //int32 P[5]={5,7,10,13,16}; int32 D[5]={0,5,15,25,40}; //int32 D[5]={0,5,10,20,30};
/*int32 angle_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} };*/
/*int32 angle_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} };*/
int32 speed_fk[5]={15,18,20,25,30};//16.6s int32 speed_fk1[5]={20,25,30,35,40};//17.7s int32 speed_fk2[5]={20,25,30,35,40}; int32 speed_fk3[5]={20,25,30,35,40}; int32 speed_fk4[5]={20,25,30,35,40}; int32 speed_fk5[5]={20,25,30,35,40}; int32 speed_fk6[5]={20,25,30,35,40}; int32 speed_fk7[5]={20,25,30,35,40}; int32 speed_fk8[5]={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; 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; 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=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_Sign2=0; uint16 XS_flag=0; 33
uint16 Z_Over=0; int Forward_Delay=0; int Back_Delay=0; int Speed_Data[500]; 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 Get_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_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)