角速度+加速度

《角速度+加速度》由会员分享，可在线阅读，更多相关《角速度+加速度（6页珍藏版）》请在装配图网上搜索。

1、#iclude #defieAcc 0D#define y x69#defne Mag 0x1defin ryoffset-3 / 陀螺仪偏移量#dfin r_Gain0.07 / 满量程dps时敏捷度（dps/gital)#efine p 3.415foat ageG；unine lng tme = 0; / 采样时间voi etu（） en_nt()； / 配备传感器 Sil.bein（19200）; /启动串口以便监视数据 dlay(00）； voi lop() on o_tier =ier; / 上一次采样时间(m) float Acceleoeter Dt(c, ); /获取向前旳加

2、速度 flat Z_Accelerometer = gat（Acc， ）; / 获取向下旳加速度 float agle = atan（Y_Acclerometer Z_ccelermete) * 10 pi； / 根据加速度分量得到旳角度(degre) tir mili()； / 目前时间（s) itdt = er - o_timer； /微分时间 agle=angle+ GrGain (gDt（Gyr,0) + Gry_ofe) * d 100; / 对角速度积分得到旳角度(ere) Srial.prnt(timr); Seia.print(,); Serilrint(angeA, 6）；

3、Serial.print(,); Serial.prit(anglG, )； rialprit(;); /输出数据 ely(10）； int gDat(int evic, ai) / 读九轴姿态传感器寄存器函数/Fo rduo,by黑马/ 调用参数表/ tpe devce ais/ 0 1 2/ ADXL35 cc x y z/L3G4200D Gr x z/ HMC83L Ma x y/Example/ 00 #nclde ire/ 01 #deineAcc 0xD;/02 #de Gy 069;/03 #defie Ma x1;/ 4/ 05 vi setp()/ 06 senso_it(

4、)；/ 0 deay(100）;/ 8 / 9/10 oidlp（）/ 1 t Z-yoscpe;/2 -Gyosope =Dt(Gyr, ); 3 delay(5)； 4 it v; bye L, vH，addres; /寄存byte数值 f(device = Ac） adrss x2; / DX4旳读数地址 if (device =Gr) addres 08; / LG420D旳读数地址 if（dvice= Mag) drss = 03; / HC5883旳读数地址 ddess adress + axs * 2; 数据偏移坐标轴 ire.eginTransmisin（devic)； / 开

5、始传播数据 Wre.end（address); /发送指针 Wire.reerom(deice, 2); / 祈求 byte数据 while(Wireavailable（)2); 成功获取前等待 vL Wre.receive(); H = Wiereceie(）； / 读取数据 WieendTramission（); / 结束传播 f (device= Mag) (vL 8) vH； else v =(vH 8) | vL; / 将bye数据合并为nt reun v； / 返回读书值vi sens_init（） / 配备九轴姿态传感器 writeRegse(Acc, 0x2, 0b00000)

6、; / 测量模式 / 配备ADX45 witeRegiser(Gyr, x20，0b00011）; / 设立睡眠模式、x, y, z轴使能 riteRgstr(Gr, 0x2,b000000); / 选择高通滤波模式和高通截止频率 writeRegite(Gyr, x,0b000000)； /设立中断模式 writeRegite(yr,x2, 1100）； / 设立量程(dps)、自检状态、SP模式 rieeier（Gr,0x4， 000000000); /FO 高通滤波 /配备L3G420( dg/se) writeRegister(g, 0x， x00）; / 持续测量 / 配备HMC83Lvod wriReistr(in device, ye adrs, byt val) / 写寄存器 re.binransmission(dvic)； Wire.send(dress); We.sed(vl); Wie.ndTrmissi(）;