两轮自平衡小车资料.pdfVIP

//IDG330 Mannual, 2mv= 1 度/s 的角速率 ,ad 读数 1024,3.3v, 那么每读数对应 3.223mV,所以每读数对应 3.223/2/180*PI= 0.028123 弧度/ 秒 static const double SEMICIRCLE_ARC = 57.29578; /* 半圆对应的弧度值 */ static const double GYRO_OPERATOR = 0.028123; /*AD 读取的陀螺仪数值对 应的弧度算子 */ /*kalman*/ static const double C_0 = 1; static const double Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.005;// 注意： dt 的取值为 kalman 滤波器采样时间 double P[2][2] = {{ 1, 0 }, { 0, 1 }}; double Pdot[4] ={ 0,0,0,0}; double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1; double angle, angle_dot; /*sensor*/ double sensorPort[6] = { 0, 1, 2, 3, 4, 5};/* 传感器地址 , 电路决 定 .accZ,gyroX, AD1, AD2, AD3, AD4*/ double sensorValue[6];/* 传感器的返回值 */ /* 传感器零点 0:Z 轴( 平行轴 ) 1: 陀螺仪中点 */ double sensorZero[2] = {499,505}; double sensorAdjusted[2];/* 传感器的返回值重整 */ //double provAngle; /*moto*/ int E1 = 6; int E2 = 9; int M1 = 7; int M2 = 8; double deadAreaCompensation1 = 45,deadAreaCompensation2 = 35; /*balance*/ double RATE[4] = { 0,0,0,0};/* 公式中的 4 个变量 */ double K[4] = { 60.45, 1.27, 125, 0.75};/* 公式中的 4 个常量 */ double K_AD[4] = { 1, 1, 1, 1};/* 公式中的 4 个常量 */ double wheel_ls[7];/* 左轮 :0: 编码器 1: 位移 (position) 2:position_dot 3:position_dot_filter 4:speedNeed 5:turnNeed 6:speedOutPut */ double wheel_rs[7];/* 右轮 :0: 编码器 1: 位移 (position) 2:position_dot 3:position_dot_filter 4:speedNeed 5:turnNeed 6:speedOutPut */ double Torque;/* 扭矩*/ unsigned int count,count2; boolean OK=false;// 这个是误差达到一定程度后的系统关闭开关 . int bf,X,Y;// 从无线端发来的命令 /*kalman*/ /*angle_m: 经过 atan2(ax,ay) 方法计算的偏角 , 弧度值 gyro_m: 经过初步减去零点的陀螺仪角速度值 , 弧度值 */ void Kalman_Filter(double angle_m,double gyro_m) { angle+=(gyro_m-q_bias) * dt; angle_err = angle_m - angle; Pdot[0]=Q_angle - P[0][1] - P[1][0