资源描述：
《角速度+加速度.doc》由会员上传分享，免费在线阅读，更多相关内容在教育资源-天天文库。
1、#include#defineAcc0x1D#defineGyr0x69#defineMag0x1E#defineGry_offset-13//陀螺仪偏移量#defineGyr_Gain0.07//满量程2000dps时灵敏度(dps/digital)#definepi3.14159floatangleG;unsignedlongtimer=0;//采样时间voidsetup(){sensor_init();//配置传感器Serial.begin(19200);//开启串口以便监视数据delay(1000);}voidloop(){longo_timer=timer;/
2、/上一次采样时间(ms)floatY_Accelerometer=gDat(Acc,1);//获取向前的加速度floatZ_Accelerometer=gDat(Acc,2);//获取向下的加速度floatangleA=atan(Y_Accelerometer/Z_Accelerometer)*180/pi;//根据加速度分量得到的角度(degree)timer=millis();//当前时间(ms)intdt=timer-o_timer;//微分时间angleG=angleG+Gyr_Gain*(gDat(Gyr,0)+Gry_offset)*dt/1000;//对角速度积分得到的角
3、度(degree)Serial.print(timer);Serial.print(",");Serial.print(angleA,6);Serial.print(",");Serial.print(angleG,6);Serial.print(";");//输出数据delay(10);}intgDat(intdevice,intaxis){//读九轴姿态传感器寄存器函数//ForArduino,by黑马//调用参数表//typedeviceaxis//012//ADXL345Accxyz//L3G4200DGyrxyz//HMC5883LMagxzy//Example//00#in
4、clude//01#defineAcc0x1D;//02#defineGyr0x69;//03#defineMag0x1E;//04//05voidsetup(){//06sensor_init();//07delay(1000);//08}//09//10voidloop(){//11intZ-Gyroscope;//12Z-Gyroscope=gDat(Gyr,2);//13delay(50);//14}intv;bytevL,vH,address;//存放byte数值if(device==Acc)address=0x32;//ADXL345的读数地址if(devi
5、ce==Gyr)address=0xA8;//L3G4200D的读数地址if(device==Mag)address=0x03;//HMC5883L的读数地址address=address+axis*2;//数据偏移-坐标轴Wire.beginTransmission(device);//开始传输数据Wire.send(address);//发送指针Wire.requestFrom(device,2);//请求2byte数据while(Wire.available()<2);//成功获取前等待vL=Wire.receive();vH=Wire.receive();//读取数据Wire.
6、endTransmission();//结束传输if(device==Mag)v=(vL<<8)
7、vH;elsev=(vH<<8)
8、vL;//将byte数据合并为Intreturnv;//返回读书值}voidsensor_init(){//配置九轴姿态传感器writeRegister(Acc,0x2D,0b00001000);//测量模式//配置ADXL345writeRegister(Gyr,0x20,0b00001111);//设置睡眠模式、x,y,z轴使能writeRegister(Gyr,0x21,0b00000000);//选择高通滤波模式和高通截止频率writeRegist
9、er(Gyr,0x22,0b00000000);//设置中断模式writeRegister(Gyr,0x23,0b00110000);//设置量程(2000dps)、自检状态、SPI模式writeRegister(Gyr,0x24,0b00000000);//FIFO&高通滤波//配置L3G4200D(2000deg/sec)writeRegister(Mag,0x02,0x00);//连续测量//配置HMC5883L}voidwriteReg
