我的自平衡小车D2——加速度计与陀螺仪获取姿态参数的差异.docVIP

查看: 4984|回复: 17 我的自平衡小车D2——加速度计与陀螺仪获取姿态参数的差异 [复制链接] 黑马 22 主题 11 好友 854 积分 版主 RedDot Winner 发消息 电梯直达 楼主 发表于 2012-3-23 11:00:50 |只看该作者 |倒序浏览 滤波的算法还没有研究明白，先放上一段从传感器直接采样的结果。 #include Wire.h #define Acc 0x1D #define Gyr 0x69 #define Mag 0x1E #define Gry_offset -13 // 陀螺仪偏移量 #define Gyr_Gain 0.07 // 满量程2000dps时灵敏度(dps/digital) #define pi 3.14159 float angleG; unsigned long timer = 0; // 采样时间 void setup() { sensor_init(); // 配置传感器 Serial.begin(19200); // 开启串口以便监视数据 delay(1000); } void loop() { long o_timer = timer; // 上一次采样时间(ms) float Y_Accelerometer = gDat(Acc, 1); // 获取向前的加速度 float Z_Accelerometer = gDat(Acc, 2); // 获取向下的加速度 float angleA = atan(Y_Accelerometer / Z_Accelerometer) * 180 / pi; // 根据加速度分量得到的角度(degree) timer = millis(); // 当前时间(ms) int dt = timer - o_timer; // 微分时间 angleG = angleG + Gyr_Gain * (gDat(Gyr, 0) + Gry_offset) * dt / 1000; // 对角速度积分得到的角度(degree) Serial.print(timer); Serial.print(,); Serial.print(angleA, 6); Serial.print(,); Serial.print(angleG, 6); Serial.print(;); // 输出数据 delay(10); } int gDat(int device, int axis) { // 读九轴姿态传感器寄存器函数 // For Arduino, by 黑马 // 调用参数表 // type device axis // 0 1 2 // ADXL345 Acc x y z // L3G4200D Gyr x y z // HMC5883L Mag x z y // Example // 00 #include Wire.h // 01 #define Acc 0x1D; // 02 #define Gyr 0x69; // 03 #define Mag 0x1E; // 04 // 05 void setup() { // 06 sensor_init(); // 07 delay(1000); // 08 } // 09 // 10 void loop() { // 11 int Z-Gyroscope; // 12 Z-Gyroscope = gDat(Gyr, 2); // 13 delay(50); // 14 } int v; byte vL, vH, address; // 存放byte数值 if (device == Acc) address = 0x32; // ADXL345的读数地址 if (device == Gyr) address = 0xA8; // L3G