/************数据查表部分***********/
//根据电磁跑道电流在100mA选用的参数,默认选中零组;
signed int const datatable[20]={-135,-120,-105,-90,-72,-53,-38,-26,-16,-10,10,16,26,38,53,72,90,105,120,135}; //16
//signed int const datatable[2][14]={{-135,-115,-86,-61,-33,-21,-10,10,21,33,61,86,115,135},//3-4/1-6
// {-80,-68,-60,-50,-38,-25,-13,13,25,38,50,60,68,80}};//2-5/0-7
//舵机转向角度与位置对应数据表和舵机对应占空比变化数据表
signed int const Turndegree[15]={35,30,25,20,15,10,5,0,-5,-10,-16,-22,-28,-34,-40};
signed int const Turndata[21]={-185,-168,-152,-138,-118,-100,-78,-52,-37,-20,0,18,30,43,60,73,85,95,108,120,133};
//signed int const Turndata[21]={133,120,108,95,85,73,60,43,30,18,0,-23,-47,-73,-105,-130,-160,-185};
//signed int const Turndata[15]={130,113,95,76,58,39,19,0,-22,-46,-72,-98,-125,-153,-185};
//signed int const Turndata[15]={130,110,92,73,55,36,18,0,-22,-46,-72,-98,-125,-153,-185};
/*************宏定义部分*************/
//#define MID_Turn 1560 //舵机零度对应脉宽 sysinit.h文件中已有定义
#define Kp_speed 2.0
#define Ki_speed 0.5
#define Kd_speed 0.1
#define Kp_turn 1.0
//#define Ki_turn 0.0
#define Kd_turn 0.8
#define speedup_max 30
#define speedup_min -30
/**************变量定义部分**************/
//定义复位不自动清零全局变量
//#pragma DATA_SEG HOLD_RAM
unsigned int flag_arrive2s=0; //等待两秒时间到标志
//定义复位自动清零全局变量
#pragma DATA_SEG DEFAULT
unsigned int AD_Result[8]; //存放AD转换结果
unsigned int ADave_Result[8]; //存放AD转换结果average平均值
unsigned int AD_OldResult[5][8]; //存放5组先前的AD转换结果
unsigned int sum_left45=0; //
unsigned int sum_right23=0;
signed int chazhi[4]={0}; //检测电路切换
unsigned int pulse_overcount=0; //脉冲累加器溢出次数计数
unsigned int pulse_count1=0; //脉冲累加器计数值1
unsigned int pulse_count2=0; //脉冲累加器计数值2
unsigned int Measure_speed=0; //速度测量Measure_speed=pulse_count1-pulse_count2;(pulse_count1>=pulse_count2)
//或65535-pulse_count2+pulse_count1;(pulse_count1<pulse_count2)
unsigned int scratchline_count=0; //经过起跑线次数计数
signed int piandu=0; //小车偏度 左偏右拐为正(1、2、3、4、5、6、7)
// 右偏左拐为负(-1、-2、-3、-4、-5、-6、-7)
unsigned int Set_speed=0; //给定速度
signed int Ek_speed=0; //速度偏差
signed int Ei_speed=0; //速度积分项
signed int Ed_speed=0; //速度微分项
signed int OldEk_speed=0; //上一次速度偏差
signed int OldEi_speed=0;
//signed int Set_turn=0;
//signed int Measure_turn=0;
signed int Ek_turn=0.0;
//float Ei_turn=0.0;
signed int Ed_turn=0.0;
signed int OldEk_turn=0.0;
unsigned int Duojiout=0;
unsigned int OldDuoji_PWM=MID_Turn;
signed int dianliu=0; //跑到电流大小
signed int Speedup=0; //速度加速变量
signed int Jiaozheng_speed=0; //速度校正变量
unsigned int Boma_speed=0; //拨码开关速度设定
unsigned int var_turn=0;
/********************状态标志变量***************************/
//unsigned int flag_arrive2s=0; //等待两秒时间到标志
unsigned int flag_shache=0; //刹车延时时间到标志
signed int flag_direction=0; //中间为零,偏左为-1,偏右为+1;
unsigned int flag_wandao=0; //弯道为1,直道为0;
unsigned int flag_crossroad=1; //十字交叉口识别标志 是标志为0;不是为1;
unsigned int flag_isrdelay20ms=0;//起跑线识别延时20ms标志
unsigned int flag_ds=0; //大弯道识别标志
unsigned int flag_xs=0; //小弯道识别标志
/**************子函数声明**************/
void wait2s(void); //等待2秒子函数
void collect_status(void); //状态采集子函数
void DIPswitch_adjust(void); //拨码开关参数调节子函数
void PID_speed(unsigned int want_speed,unsigned int now_speed); //速度PID调节子函数
unsigned int PID_turn(signed int Set_turn,signed int Measure_turn); //转向PID调节子函数
void parking(void); //第二次检测到起跑线停车子函数
void shache(void); //刹车子函数
/***********AD中断函数***********/
//把中断处理(程序/函数)放在不分页FLASH区
#pragma CODE_SEG NON_BANKED
/*************采集状态函数************/
#pragma CODE_SEG DEFAULT
void collect_status(void)
{
signed int Duoji_Position;
unsigned int n=1;
unsigned int k=0;
signed int chazhi34;
//signed int chazhi25;
signed int chazhi16;
//signed int chazhi07;
//signed int chazhi31;
//signed int chazhi20;
//signed int chazhi46;
// signed int chazhi57;
//signed int chazhi2345;
// signed int chazhi0167;
Boma_turn=PTH;
//Boma_speed=PORTA;
if(Boma_pa&0x01==1)
switch(Boma_pa)
{
case 0xff:speed_adjust=0 ;break;
//case 0xfe:speed_adjust=1;break;
case 0xfd:speed_adjust=1;break;
case 0xfb:speed_adjust=2;break;
case 0xf7:speed_adjust=3;break;
case 0xef:speed_adjust=4;break;
case 0xdf:speed_adjust=5;break;
case 0xbf:speed_adjust=6;break;
case 0x7f:speed_adjust=7;break;
default: speed_adjust=0;break;
}
else
switch(Boma_pa)
{
//case 0xff:speed_adjust=0 ;break;
case 0xfc:speed_adjust=8;break;
case 0xfa:speed_adjust=9;break;
case 0xf6:speed_adjust=10;break;
case 0xee:speed_adjust=11;break;
case 0xde:speed_adjust=12;break;
case 0xbe:speed_adjust=13;break;
case 0x7e:speed_adjust=14;break;
default: speed_adjust=0;break;
}
Boma_speed=speed_adjust;
switch(Boma_turn)
{
case 0xff:var_turn=8;break;
case 0xfe:var_turn=0;break;
case 0xfd:var_turn=1;break;
case 0xfb:var_turn=2;break;
case 0xf7:var_turn=3;break;
case 0xef:var_turn=4;break;
case 0xdf:var_turn=5;break;
case 0xbf:var_turn=6;break;
case 0x7f:var_turn=7;break;
default: var_turn=8;break;
}
}
_FEED_COP(); /* feeds the dog */
} /* loop forever */
/* please make sure that you never leave main */
}
/**********************************************************/
/* //舵机采用位置式PID方式
//公式: Uk=Kp*E(k)+Ki*∑E(i)+Kd*[E(k)-E(k-1)]
Ek_turn=Set_turn-Measure_turn;
Ei_turn+=Ek_turn;
Ed_turn=Ek_turn-OldEk_turn;
OldEk_turn=Ek_turn;
Duojiout_PWM=Kp_turn*Ek_turn+Ki*Ei+Kd*Ed;