高级会员
- 积分
- 921
- 威望
- 463
- 贡献
- 296
- 兑换币
- 268
- 注册时间
- 2013-11-19
- 在线时间
- 81 小时
|
#include <hidef.h> /* common defines and macros */
#include <math.h>
#include "sysinit.h"
#include "derivative.h" /* derivative-specific definitions */
/************数据查表部分***********/
//根据电磁跑道电流在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 Boma_pa=0;
unsigned char count_10ms=0; //计时10ms
unsigned char duoji_10ms=0; //duoji计时10ms
unsigned char count_20ms=0; //计时20ms
unsigned char count_2s=0; //起跑延时2s count_10ms计数200次
unsigned int shache_10ms=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
void interrupt 22 ATD0(void)
{
unsigned int i,j,k,m,n;
//DisableInterrupts;
AD_Result[0]=ATD0DR0L;
AD_Result[1]=ATD0DR1L;
AD_Result[2]=ATD0DR2L;
AD_Result[3]=ATD0DR3L;
AD_Result[4]=ATD0DR4L;
AD_Result[5]=ATD0DR5L;
AD_Result[6]=ATD0DR6L;
AD_Result[7]=ATD0DR7L;
//EnableInterrupts;
if(flag_arrive2s==0)
for(n=0;n<8;n++)
ADave_Result[n]=AD_Result[n];
else
{
for(i=5;i>0;i--)
for(j=0;j<8;j++)
AD_OldResult[i][j]=AD_OldResult[i-1][j];
for(k=0;k<8;k++)
AD_OldResult[0][k]=AD_Result[k];
for(m=0;m<8;m++)
ADave_Result[m]=(AD_OldResult[0][m]+AD_OldResult[1][m]+AD_OldResult[2][m]+AD_OldResult[3][m]+AD_OldResult[4][m])/5;
}
}
/*****定时器PIT0中断10ms函数*****/
//把中断处理(程序/函数)放在不分页FLASH区
#pragma CODE_SEG NON_BANKED
void interrupt 66 PIT0(void)
{
//DisableInterrupts;
count_10ms++;
count_2s++;
count_20ms++;
shache_10ms++;
duoji_10ms++;
if(count_20ms==2)
{
pulse_count2=pulse_count1;
pulse_count1=PACNT;
count_20ms=0;
flag_isrdelay20ms=1;
}
if(count_2s==200)
{
flag_arrive2s=1;
count_2s=0;
}
//if(flag_isrdelay20ms>10)
//flag_isrdelay20ms=0;
if(shache_10ms==1)
{
flag_shache=1;
shache_10ms=0;
}
//collect_status();
PITTF=0x01; //清除标志位
//EnableInterrupts;
}
/******脉冲累加器溢出中断******/
//把中断处理(程序/函数)放在不分页FLASH区
#pragma CODE_SEG NON_BANKED
void interrupt 17 PULSE_ISR(void)
{
//DisableInterrupts;
pulse_overcount++;
PACNT=0;
PAFLG=0x03; //清除标志位
//EnableInterrupts;
}
/********起跑线识别外部PE1/I\R\Q\中断*********/
//把中断处理(程序/函数)放在不分页FLASH区
#pragma CODE_SEG NON_BANKED
void interrupt 6 SCRATCHLINE_ISR(void)
{
//DisableInterrupts;
scratchline_count++; //过起跑线次数 加个延时20ms
flag_isrdelay20ms=0;
while(flag_isrdelay20ms==0);
if(scratchline_count>=2)
{
DisableInterrupts;
parking();
}
//EnableInterrupts;
}
/*************采集状态函数************/
#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;
//signed int chazhi[4]={chazhi34,chazhi16,chazhi31,chazhi46};
chazhi34=ADave_Result[3]-ADave_Result[4];//3-4
//chazhi25=ADave_Result[2]-ADave_Result[5];
chazhi16=ADave_Result[1]-ADave_Result[6];//1-6
//chazhi07=ADave_Result[0]-ADave_Result[7];
//chazhi31=ADave_Result[3]-ADave_Result[1];//3-1
//chazhi20=ADave_Result[2]-ADave_Result[0];
//chazhi46=ADave_Result[4]-ADave_Result[6];//4-6
//chazhi57=ADave_Result[5]-ADave_Result[7];
//chazhi2345=chazhi25+chazhi34;
//chazhi0167=chazhi07+chazhi16;
//chazhi[0]=chazhi34;
//chazhi[1]=chazhi16;
if(chazhi34>=datatable[9]&&chazhi34<=datatable[10]) //判别为跑道中间位置[-33,34]
{
piandu=0;
PORTB=0xff;
//Speedup+=4;
//if(Speedup>=14)
Speedup=14;
Jiaozheng_speed=3;
PWMDTY67=MID_Turn+Turndata[piandu+10];
}
if(chazhi34>=datatable[8]&&chazhi34<datatable[9]) //判别为左偏右拐一档
{
piandu=-1;
PORTB=0xef;
//Speedup+=1;
//if(Speedup>=13)
Speedup=13;
Jiaozheng_speed=0;
//PWMDTY67=MID_Turn+Turndata[piandu+10];
//shache();
}
if(chazhi34>=datatable[7]&&chazhi34<datatable[8]) //判别为左偏右拐二档
{
piandu=-2;
PORTB=0xdf;
Speedup=5;
Jiaozheng_speed=0;
//PWMDTY67=MID_Turn+Turndata[piandu+10];
//shache();
//shache();
}
if(chazhi34>=datatable[6]&&chazhi34<datatable[7]) //判别为左偏右拐三档
{
piandu=-3;
PORTB=0xbf;
Speedup=3;
Jiaozheng_speed=0;
//PWMDTY67=MID_Turn+Turndata[piandu+10];
//shache();
}
if(chazhi34>=datatable[5]&&chazhi34<datatable[6]) //判别为左偏右拐四档
{
piandu=-4;
PORTB=0x7f;
Speedup=5;
Jiaozheng_speed=0;
//PWMDTY67=MID_Turn+Turndata[piandu+10];
}
if(chazhi34>=datatable[4]&&chazhi34<datatable[5]) //判别为左偏右拐五档
{
piandu=-5;
PORTB=0x6f;
Speedup=6;
Jiaozheng_speed=0;
//PWMDTY67=MID_Turn+Turndata[piandu+10];
}
if(chazhi34>=datatable[3]&&chazhi34<datatable[4]) //判别为左偏右拐六档
{
piandu=-6;
PORTB=0x5f;
Speedup=5;
Jiaozheng_speed=0;
//PWMDTY67=MID_Turn+Turndata[piandu+10];
}
if(chazhi34>=datatable[2]&&chazhi34<datatable[3]) //判别为左偏右拐七档
{
piandu=-7;
PORTB=0x3f;
Speedup=5;
Jiaozheng_speed=0;
//PWMDTY67=MID_Turn+Turndata[piandu+10];
}
if(chazhi34>=datatable[1]&&chazhi34<datatable[2]) //判别为左偏右拐八档
{
piandu=-8;
PORTB=0x2f;
Speedup=3;
Jiaozheng_speed=0;
//PWMDTY67=MID_Turn+Turndata[piandu+10];
}
if(chazhi34>=datatable[0]&&chazhi34<datatable[1]) //判别为左偏右拐九档
{
piandu=-9;
PORTB=0x1f;
Speedup=4;
Jiaozheng_speed=0;
//PWMDTY67=MID_Turn+Turndata[piandu+10];
}
if(chazhi34<datatable[0]) //判别为左偏右拐十档
{
piandu=-10;
PORTB=0x0f;
Speedup=0;
Jiaozheng_speed=0;
//PWMDTY67=MID_Turn+Turndata[piandu+10];
}
if(chazhi34>datatable[10]&&chazhi34<=datatable[11]) //判别为右偏左拐一档
{
piandu=1;
PORTB=0xf7;
//Speedup+=1;
//if(Speedup>=13)
Speedup=13;
Jiaozheng_speed=0;
//PWMDTY67=MID_Turn+Turndata[piandu+10];
//shache();
}
if(chazhi34>datatable[11]&&chazhi34<=datatable[12]) //判别为右偏左拐二档
{
piandu=2;
PORTB=0xfb;
Speedup=5;//11;
Jiaozheng_speed=0;
// PWMDTY67=MID_Turn+Turndata[piandu+10];
//shache();
//shache();
}
if(chazhi34>datatable[12]&&chazhi34<=datatable[13]) //判别为右偏左拐三档
{
piandu=3;
PORTB=0xfd;
Speedup=3;//9;
Jiaozheng_speed=0;
//PWMDTY67=MID_Turn+Turndata[piandu+10];
//shache();
}
if(chazhi34>datatable[13]&&chazhi34<=datatable[14]) //判别为右偏左拐四档
{
piandu=4;
PORTB=0xfe;
Speedup=5;
Jiaozheng_speed=0;
//PWMDTY67=MID_Turn+Turndata[piandu+10];
}
if(chazhi34>datatable[14]&&chazhi34<=datatable[15]) //判别为右偏左拐五档
{
piandu=5;
PORTB=0xf6;
Speedup=6;
Jiaozheng_speed=0;
//PWMDTY67=MID_Turn+Turndata[piandu+10];
}
if(chazhi34>datatable[15]&&chazhi34<=datatable[16]) //判别为右偏左拐六档
{
piandu=6;
PORTB=0xfa;
Speedup=5;//5;
Jiaozheng_speed=0;
//PWMDTY67=MID_Turn+Turndata[piandu+10];
}
if(chazhi34>datatable[16]&&chazhi34<=datatable[17]) //判别为右偏左拐七档
{
piandu=7;
PORTB=0xfc;
Speedup=5;
Jiaozheng_speed=0;
// PWMDTY67=MID_Turn+Turndata[piandu+10];
}
if(chazhi34>datatable[17]&&chazhi34<=datatable[18]) //判别为右偏左拐八档
{
piandu=8;
PORTB=0xf4;
Speedup=3;
Jiaozheng_speed=0;
//PWMDTY67=MID_Turn+Turndata[piandu+10];
}
if(chazhi34>datatable[18]&&chazhi34<=datatable[19]) //判别为右偏左拐九档
{
piandu=9;
PORTB=0xf8;
Speedup=4;
Jiaozheng_speed=0;
// PWMDTY67=MID_Turn+Turndata[piandu+10];
}
if(chazhi34>datatable[19]) //判别为右偏左拐十档
{
piandu=10;
PORTB=0xf0;
Speedup=0;
Jiaozheng_speed=0;
//PWMDTY67=MID_Turn+Turndata[piandu+10];
}
Duoji_Position=PID_turn(0,piandu);
PWMDTY67=MID_Turn+Turndata[Duoji_Position+10];
//OldDuoji_PWM=MID_Turn+Turndata[piandu+7];
if(ADave_Result[3]<6||ADave_Result[4]<6)
{
if(chazhi16<-46)
{
PWMDTY67=MID_Turn-170;//+Turndata[13];
shache();
PWMDTY01=100;
OldDuoji_PWM=MID_Turn-180;
}
if(chazhi16>46)
{
PWMDTY67=MID_Turn+125;//Turndata[1];
shache();
PWMDTY01=100;
OldDuoji_PWM=MID_Turn+128;
}
Speedup=-6;
Jiaozheng_speed=0;
//PWMDTY01=70;
}
if(ADave_Result[3]<8&&ADave_Result[4]<8&&ADave_Result[1]<7&&ADave_Result[6]<7)
{
PWMDTY67=OldDuoji_PWM;
Speedup=-4;
Jiaozheng_speed=0;
//Jiaozheng_speed=-1*Boma_speed;
//PWMDTY01=70;
}
//PWMDTY67=MID_Turn+Turndata[piandu+7];
//Set_speed=10+Boma_speed+Speedup+Jiaozheng_speed;
}
/*********等待2秒子函数******************/
void wait2s(void)
{
while(flag_arrive2s==0);
}
/*************刹车子函数***************/
void shache(void)
{
PWMDTY01=80; //先降速70
flag_shache=0; //可调延时即调节刹车时间 10ms
while(flag_shache==0);
}
/***********第二次检测到起跑线停车函数**********/
void parking(void)
{
//shache();
//PWMDTY01=0;
//PWMDTY45=0;
//PWMDTY67=MID_Turn;
for(;;)
{
PWMDTY01=0;
PWMDTY45=0;
PWMDTY67=MID_Turn;
}
//while(1);
}
/*******速度拨码开关调节子函数*******/
void DIPswitch_adjust(void)
{
unsigned int speed_adjust=0;
unsigned int Boma_turn=0;
Boma_pa=PORTA;
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;
}
}
/**********************************************/
//Set_speed=20+Boma_speed+Speedup+Jiaozheng_speed;
/************PID调节函数**************/
void PID_speed(unsigned int want_speed,unsigned int now_speed)
{
float Dianjiout_PWM;
Ei_speed=want_speed-now_speed;
Ek_speed=Ei_speed-OldEi_speed;
Ed_speed=Ek_speed-OldEk_speed;
OldEi_speed=Ei_speed;
OldEk_speed=Ek_speed;
Dianjiout_PWM=Kp_speed*Ek_speed+Ki_speed*Ei_speed+Kd_speed*Ed_speed;
if(PWMDTY01+Dianjiout_PWM>=0)
{
PWMDTY45=0;
if(PWMDTY01+Dianjiout_PWM>=180)
PWMDTY01=180; //限定占空比最高为90%
else
PWMDTY01+=Dianjiout_PWM;
}
else
{
PWMDTY01=0;
PWMDTY45+=Dianjiout_PWM*(-1);
}
}
unsigned int PID_turn(signed int Set_turn,signed int Measure_turn)
{
float Duojiout_Position;
Ek_turn=Measure_turn-Set_turn;
//Ei_turn+=Ek_turn;
Ed_turn=Ek_turn-OldEk_turn;
OldEk_turn=Ek_turn;
Duojiout_Position=Kp_turn*Ek_turn+Kd_turn*Ed_turn;//+Ki_turn*Ei_turn;
//return(unsigned int)(Duojiout_Position);
if(Duojiout_Position>10)
Duojiout_Position=10;
if(Duojiout_Position<-10)
Duojiout_Position=-10;
return(unsigned int)(Duojiout_Position);
}
//需要限幅
/****************主函数*************/
#pragma CODE_SEG DEFAULT
void main(void)
{
/* put your own code here */
DisableInterrupts;
SYS_Init();
EnableInterrupts;
DIPswitch_adjust();
wait2s();
for(;;)
{
//DIPswitch_adjust();
if(duoji_10ms==2)
{
collect_status();
duoji_10ms=0;
}
if(count_10ms==2)
{
//collect_status();
//PID_turn(signed int Set_turn,signed int Measure_turn);
//PWMDTY67=MID_Turn+Turndata[var_turn+8];
Set_speed=11+Boma_speed+Speedup+Jiaozheng_speed;
if(pulse_count1>=pulse_count2)
Measure_speed=pulse_count1-pulse_count2;
else
Measure_speed=65535-pulse_count2+pulse_count1;
PID_speed(Set_speed,Measure_speed);
//Duojiout=PID_turn(Set_turn,Measure_turn);
//PWMDTY67=MID_Turn+Duojiout;
count_10ms=0;
}
//舵机占空比MID_Turn=1600为中间 大于MID_Turn往左拐 小于MID_Turn往右拐
//范围(-30°,30°)
_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;
//电机采用增量式PID方式
//公式： ΔU(k)=U(k)-U(k-1)=Kp*[E(k)-E(k-1)]+Ki*E(k)+Kd*{[E(k)-E(k-1)]-[E(k-1)-E(k-2)]}
Ei_speed=Set_speed-Measure_speed;
Ek_speed=Ei_speed-OldEi_speed;
Ed_speed=Ek_speed-OldEk_speed;
OldEi_speed=Ei_speed;
OldEk_speed=Ek_speed;
Dianjiout_PWM=Kp_speed*Ek_speed+Ki_speed*Ei_speed+Kd*Ed_speed;
*/
|
|
发表于 2014-3-9 19:08:24
微信
收藏
转播
分享
顶
踩
提升卡
变色卡
抢沙发
楼主