附件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++;
}
}
评 论
请勿进行人身攻击，谩骂以及任何违法国家相关法律法规的言论。
正在加载评论...