附件2：重点代码
2.1车身角度滤波代码
/************滤波************/
float P[2][2] = {{ 1, 0 },{ 0, 1 }};
float Pdot[4] ={0,0,0,0};
const char C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.01;
void Kalman_Filter(float angle_m,float gyro_m)
{
angle+=(gyro_m-q_bias) * dt;
Pdot[0]=Q_angle - P[0][1] - P[1][0];
Pdot[1]=- P[1][1];
Pdot[2]=- P[1][1];
Pdot[3]=Q_gyro;
P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
angle_err = angle_m - angle;
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * P[0][1];
P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
angle += K_0 * angle_err;
q_bias += K_1 * angle_err;
angle_dot = gyro_m-q_bias;
}
//**************滤波*****************//
static float C_angle,C_angle_dot;
static float bias_cf;
void Complement_filter(float angle_m_cf,float gyro_m_cf)
{
bias_cf=0.998*bias_cf+0.002*gyro_m_cf;
C_angle_dot=gyro_m_cf-bias_cf;
C_angle=0.98*(C_angle+C_angle_dot*0.02)+0.02*angle_m_cf;
}
//***************************** 滤波结束*********************************/
2.2 转向数据处理代码
/************转向************/
void Steering_handle(void)
{
Buf= 0.9 *Buf + 0.1 * AD_Turn;
Turning= Buf -Turn_Zero; //
if(Turning <- Turn_Dead) //死区
Turning+=Turn_Dead;
else if(Turning> Turn_Dead)
Turning-=Turn_Dead;
else Turning= 0;
if (mode==0)
{
Drive_A=0;
Drive_B=0;
if (!(angle>0.1||angle<-0.1))
{
mode=1;
}
}
else
{
if(lab==0)
{
Turning=0;
}
else if (Turning>55||Turning<-55)//
{
Turning=0;
lab=3;// turn error
}
else //按车速整定转向数据
{
//buf2=Drivespeed;
//if (buf2<0)buf2*=-1;
//buf2/=3;
//Turning/=buf2;
Turning/=1;
}
Drive_A=Drivespeed-Turning;
Drive_B=Drivespeed+Turning;
}
}
//***************************** 转向结束*********************************/
2.3遥控部分状态机
/***********按键********/
#define BOOL int
#define FALSE 0
#define TRUE 1
#define INT8U unsigned int
/**********硬件接口***********/
#define KEYPIN1 (PINC&(1<<3))
#define KEYPIN2 (~PINB&(1<<0))
#define KEYPIN3 (~PINB&(1<<1))
#define KEYPIN4 (~PINB&(1<<3))
#define KEYPIN5 (~PINB&(1<<4))
/**********按恪键属性**********/
#define KEY_JT 0x0e
#define KEY_A 0x0d
#define KEY_B 0x0b
#define KEY_C 0x07
#define KEY_D 0x08
#define KEY_NULL 0x0f
//
#define KEY_LONG_PERIOD 250
#define KEY_CONTINUE_PERIOD 25
//
#define KEY_DOWN 0x80
#define KEY_LONG 0x40
#define KEY_CONTINUE 0x20
#define KEY_UP 0x10
//
#define KEY_STATE_INIT 0
#define KEY_STATE_WOBBLE 1
#define KEY_STATE_PRESS 2
#define KEY_STATE_LONG 3
#define KEY_STATE_CONTINUE 4
#define KEY_STATE_RELEASE 5
uchar KeyScan(void)
{
if(KEYPIN2==0) return KEY_A;
if(KEYPIN3==0) return KEY_B;
if(KEYPIN4==0) return KEY_C;
if(KEYPIN5==0) return KEY_D;
if(KEYPIN1==0) return KEY_JT;
return KEY_NULL;
}
void GetKey(uchar *pKeyValue)
{
static char KeyState = KEY_STATE_INIT;
static char KeyTimeCount = 0;
static char LastKey = KEY_NULL;
char KeyTemp = KEY_NULL;
KeyTemp = KeyScan();
switch(KeyState)
{
case KEY_STATE_INIT:
{
if(KEY_NULL!=(KeyTemp))
{
KeyState = KEY_STATE_WOBBLE;
}
}
break;
case KEY_STATE_WOBBLE:
{
KeyState = KEY_STATE_PRESS;
}
break;
case KEY_STATE_PRESS:
{
if(KEY_NULL!=(KeyTemp))
{
LastKey = KeyTemp;
KeyTemp|=KEY_DOWN;
KeyState = KEY_STATE_LONG ;
}
else
{
KeyState = KEY_STATE_INIT;
}
}
break;
case KEY_STATE_LONG:
{
if(KEY_NULL !=(KeyTemp))
{
if(++KeyTimeCount > KEY_LONG_PERIOD)
{
KeyTimeCount = 0;
KeyTemp|=KEY_LONG;
KeyState = KEY_STATE_CONTINUE;
}
}
else
{
KeyState = KEY_STATE_RELEASE;
}
}
break;
case KEY_STATE_CONTINUE:
{
if(KEY_NULL !=(KeyTemp))
{
if(++KeyTimeCount > KEY_CONTINUE_PERIOD)
{
KeyTimeCount = 0;
KeyTemp |= KEY_CONTINUE;
}
}
else
{
KeyState = KEY_STATE_RELEASE;
}
}
break;
case KEY_STATE_RELEASE:
{
LastKey |=KEY_UP;
KeyTemp = LastKey;
KeyState = KEY_STATE_INIT;
}
break;
default:break;
}
*pKeyValue = KeyTemp;
}
2.4电池电压
void Get_Batt_Volt(void)
{
int buf3=0,b=0;
buf3=0.9*buf3+0.1*AD_Batt;
if (b>10)
{
Voltage=buf3*3000.0/1024/65;
b=10;
}
else
{
b++;
}
}
畅学电子
