角速度+加速度.doc

《角速度+加速度.doc》由会员分享，可在线阅读，更多相关《角速度+加速度.doc（2页珍藏版）》请在得力文库 - 分享文档赚钱的网站上搜索。

1、精品文档，仅供学习与交流，如有侵权请联系网站删除#include #define Acc 0x1D#define Gyr 0x69#define Mag 0x1E#define Gry_offset -13 / 陀螺仪偏移量#define Gyr_Gain 0.07 / 满量程2000dps时灵敏度(dps/digital)#define pi 3.14159float angleG;unsigned long timer = 0; / 采样时间void setup() sensor_init(); / 配置传感器 Serial.begin(19200); / 开启串口以便监视数据 delay

2、(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; /

3、微分时间 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

4、黑马/ 调用参数表/ type device axis/ 0 1 2/ ADXL345 Acc x y z/ L3G4200D Gyr x y z/ HMC5883L Mag x z y/ Example/ 00 #include / 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-Gyrosc

5、ope = 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; / L3G4200D的读数地址 if (device = Mag) address = 0x03; / HMC5883L的读数地址 address = address + axis * 2; / 数据偏移-坐标轴 Wire.beginTransmission(device);

6、 / 开始传输数据 Wire.send(address); / 发送指针 Wire.requestFrom(device, 2); / 请求2 byte数据 while(Wire.available() 2); / 成功获取前等待 vL = Wire.receive(); vH = Wire.receive(); / 读取数据 Wire.endTransmission(); / 结束传输 if (device = Mag) v = (vL 8) | vH; else v = (vH 8) | vL; / 将byte数据合并为Int return v; / 返回读书值void sensor_in

7、it() / 配置九轴姿态传感器 writeRegister(Acc, 0x2D, 0b00001000); / 测量模式 / 配置ADXL345 writeRegister(Gyr, 0x20, 0b00001111); / 设置睡眠模式、x, y, z轴使能 writeRegister(Gyr, 0x21, 0b00000000); / 选择高通滤波模式和高通截止频率 writeRegister(Gyr, 0x22, 0b00000000); / 设置中断模式 writeRegister(Gyr, 0x23, 0b00110000); / 设置量程(2000dps)、自检状态、SPI模式 writeRegister(Gyr, 0x24, 0b00000000); / FIFO & 高通滤波 / 配置L3G4200D(2000 deg/sec) writeRegister(Mag, 0x02, 0x00); / 连续测量 / 配置HMC5883Lvoid writeRegister(int device, byte address, byte val) / 写寄存器 Wire.beginTransmission(device); Wire.send(address); Wire.send(val); Wire.endTransmission();【精品文档】第 2 页