📄 新建 文本文档.txt
字号:
车模源代码
#include <hidef.h> /* common defines and macros */
#include <mc9s12dg128.h> /* derivative information */
#include "printp.h"
#include "smartcar.h"
#pragma LINK_INFO DERIVATIVE "mc9s12dg128b"
typedef struct PID {
double SetPoint; // 设定目标Desired value
double Proportion; // 比例常数Proportional Const
double Integral; // 积分常数Integral Const
double Derivative; // 微分常数Derivative Const
double LastError; // Error[-1]
double PrevError; // Error[-2]
double SumError; // Sums of Errors
} PID;
/*====================================================================================================
PID计算部分
=====================================================================================================*/
double PIDCalc( PID *pp, double NextPoint )
{
double dError,
Error;
Error = pp->SetPoint - NextPoint; // 偏差
pp->SumError += Error; // 积分
dError = pp->LastError - pp->PrevError; // 当前微分
pp->PrevError = pp->LastError;
pp->LastError = Error;
return (pp->Proportion * Error // 比例项
+ pp->Integral * pp->SumError // 积分项
+ pp->Derivative * dError // 微分项
);
}
/*====================================================================================================
Initialize PID Structure
=====================================================================================================*/
void PIDInit (PID *pp)
{
memset ( pp,0,sizeof(PID));
}
unsigned int vspeed;
unsigned char vspeedcount=254;
unsigned char vspeed_stop=0;
#define INTERVAL_EQU_1 //当INTERVAL是1时(1ms)
#define MY_DEBUG //定义后去除lcd代码
#define DESK_DEBUG //定义后,进入桌面调试
/* BUF <-> BUFFER */
#define L_BUF 4 // 在L_BUF之外: <L_BUF
#define R_BUF 7 // 在R_BUF之外: >R_BUF
#define LSPEED 0
#define HSPEED 1
#define NONE 0
#define L_LING 1 /*L*/
#define H_LING 2
#define A_LING 3
#define A_L_REING 4
#define H_L_REING 5
#define H_CRI_LING 6
#define L_RING 11 /*L*/
#define H_RING 12
#define A_RING 13
#define A_R_REING 14
#define H_R_REING 15
#define H_CRI_RING 16
#define BLIND 1
#define BLIND_L 2
#define BLIND_R 3
#define NGROUP 4
#define NFORMATION 8
#define LENGTH NGROUP+NFORMATION-1
#define A_IN_OUT_CONVERT(p) (p*2/5) // a_in * CONVERT == a_out
#define fpre(p) (p==0)?(NGROUP+NFORMATION-2):(p-1)
#define fnext(p) (p==NGROUP+NFORMATION-2)?(0):(p+1)
#define fabs(p) (p>0?p:-p)
#define INTERVAL 1 // ms 主定时器间隔
#define WHEEL_CIR 160
#define WHEEL_DETECTOR_COUNT
#define WHEEL_ARC 32 // mm 两感应器间隔弧长
#define NDETECTOR 8 //主探头数目
#define RDETECTOR 7 //右端探头编号
#define LDETECTOR 0 //左端探头编号
#define NOERROR 0 //数据是可靠的
#define BLIND 1 //数据不可靠
#define L_CRI 200
#define LING 1
#define RING 2
#define LBACK 3
#define RBACK 4
extern unsigned char p;
extern unsigned char flagA;
extern unsigned char flagB;
extern unsigned int zcount;
extern unsigned int cum_timer;
extern unsigned char flagBLIND;
extern int d[8]; /*I*/ //探头组返回的数据 200~500 间
struct data
{
unsigned char gru_tail; //组的最后一个元素地址
unsigned char for_tail;
/* refresh */
int d[8]; /*I*/ //探头组返回的数据 200~500 间
unsigned char min; /*I*/ //指示最小值的探头序号 0~7
int offset; /*I*/ // （min*100 + 公式计算值）
unsigned char d_dep; //数据可靠性
// [offset]
// +-|-+---+---+---+---+---+-|-+
// 0 1 2 3 4 5 6 7 (* 100)
unsigned char turning;
unsigned int vspeed;
unsigned int l;
unsigned int app_val;
unsigned char avr; //11个区域
unsigned char pre_avr;
char step;
};
extern struct data arr[NGROUP+NFORMATION];
extern struct data current;
unsigned int vspeed;
unsigned char vspeedcount=254;
unsigned char vspeed_stop=0;
//***************************************************************************
unsigned char readcounter(void)
{
ulong p ;
p = TC0H ;
return PA0H ;
}
void fENGINE(unsigned char i)
{ switch(i)
{
case 0 : PWMDTY5 = 0x09 ; break ;
case 10 : PWMDTY5 = 0x07 ; break ;
}
}
void fPWM( int i)
{
/* 0 1 2 3 4 */
/* -90 -45 0 45 90 */
/* ... */
switch(i)
{ case -5: PWMDTY7 = 17 ; break ;
case -4: PWMDTY7 = 16 ; break ;
case -3: PWMDTY7 = 15 ; break ;
case -2: PWMDTY7 = 14 ; break ;
case -1: PWMDTY7 = 13 ; break ;
case 0: PWMDTY7 = 12 ; break ; /*--处在中间,小了向右转,大了向左转,最小6,最大17--*/
case 1: PWMDTY7 = 11 ; break ;
case 2: PWMDTY7 = 10 ; break ;
case 3: PWMDTY7 = 9 ; break ;
case 4: PWMDTY7 = 8 ; break ;
case 5: PWMDTY7 = 7 ; break ;
}
}
//***************************************************************************
void refresh_vspeed(void) /* 更新 vspeed */
{
vspeedcount++;
if (readcounter()==1) // 读计数器寄存器 <------------------------ !!!
{
#ifdef INTERVAL_EQU_1
vspeed= 32000 / vspeedcount;
#else
vspeed= WHEEL_ARC * 1000 / ( vspeedcount * INTERVAL); // mm/s
#endif
vspeedcount=0;
vspeed_stop=0;
}
if (vspeedcount==255)
{
vspeedcount--; //可以测定的最小速度：125mm/s
vspeed_stop++;
if (vspeed_stop>250)
{
vspeed=0;
vspeed_stop=0;
}
}
current.vspeed=vspeed;
}
//****************************************************
void repeat_data(void) /* 计算min并检查数据的可靠性 */
{
unsigned char i;
unsigned char min;
unsigned char count_1=0;
unsigned char count_2=0;
unsigned char count_4=NDETECTOR;
current.d_dep = NOERROR;
min=0;
for (i=0;i<8;i++)
{
/* 数据可靠性附加检查 */
if (current.d[i]<current.d[min])
{
min=i;
}
if (current.d[i] <400)
{
count_1++;
if (count_1 > 3)
{
current.d_dep = BLIND;
return;
}
if (current.d[i]<250)
{
count_2++;
if (count_2 >2)
{
current.d_dep = BLIND;
return;
}
if (count_4==NDETECTOR)
{
count_4 = i;
}
else
{
if ( i-count_4 > 1 )
{
current.d_dep = BLIND;
return;
}
}
} //(current.d[i]<250)
}
}
if (count_2==0)
{
current.d_dep = BLIND;
return;
}
current.min=min;
}
//****************************************************
void get_offset(void)
{
int i;
unsigned char min;
int x,y,z;
min=current.min;
if (min==0)
{
current.offset=50;
}
else if (min==7)
{
current.offset=650;
}
else /* min>=1 && min<=6 */
{
x=current.d[min -1];
y=current.d[min];
z=current.d[min +1];
// i=z>x?(- ((z-y)-(x-y)) *50 /(z-y)) : ( ((x-y)-(z-y))*50/(x-y) );
if (z==y) // z-y==0
{
i=50;
}
else if (x==y) // x-y==0
{
i=-50;
}
else
{
i=z>x?(- (z-x)*50 /(z-y) ) : ( (x-z)*50 /(x-y) );
}
current.offset = min*100 + i;
}
}
//****************************************************
void get_avr(void)
{
if (current.offset<100)
{
current.avr=0;
}
else if (current.offset>=600)
{
current.avr=11;
}
else
{
current.avr= current.offset / 50 -1;
}
}
void refresh_vspeed(void) /* 更新 vspeed */
{
vspeedcount++;
if (readcounter()==1) // 读计数器寄存器 <------------------------ !!!
{
#ifdef INTERVAL_EQU_1
vspeed= 32000 / vspeedcount;
#else
vspeed= WHEEL_ARC * 1000 / ( vspeedcount * INTERVAL); // mm/s
#endif
vspeedcount=0;
vspeed_stop=0;
}
if (vspeedcount==255)
{
vspeedcount--; //可以测定的最小速度：125mm/s
vspeed_stop++;
if (vspeed_stop>250)
{
vspeed=0;
vspeed_stop=0;
}
}
current.vspeed=vspeed;
}
//****************************************************
void repeat_data(void) /* 计算min并检查数据的可靠性 */
{
unsigned char i;
unsigned char min;
unsigned char count_1=0;
unsigned char count_2=0;
unsigned char count_4=NDETECTOR;
current.d_dep = NOERROR;
min=0;
for (i=0;i<8;i++)
{
/* 数据可靠性附加检查 */
if (current.d[i]<current.d[min])
{
min=i;
}
if (current.d[i] <400)
{
count_1++;
if (count_1 > 3)
{
current.d_dep = BLIND;
return;
}
if (current.d[i]<250)
{
count_2++;
if (count_2 >2)
{
current.d_dep = BLIND;
return;
}
if (count_4==NDETECTOR)
{
count_4 = i;
}
else
{
if ( i-count_4 > 1 )
{
current.d_dep = BLIND;
return;
}
}
} //(current.d[i]<250)
}
}
if (count_2==0)
{
current.d_dep = BLIND;
return;
}
current.min=min;
}
//****************************************************
void refresh_all_datas()
{
refresh_vspeed(); /* 更新 vspeed */
getnewdata(); /* 更新 d[] */
repeat_data(); /* 计算min并检查数据的可靠性 */
if (current.d_dep != NOERROR)
{ /* BLIND */
flagBLIND = BLIND;
if (current.vspeed <= 600)
{
fENGINE(10);
}
else if (current.vspeed >1200)
{
fENGINE(0);
}
// blind_data(p);
if (current.avr < 6 )
{
fPWM(-5);
}
else
{
fPWM(5);
}
return ; // 数据不可靠，不进行后续运算
}
if (flagBLIND == BLIND )
{
flagBLIND=0;
fENGINE(10);
flagB=NONE;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -