# Demo entry 6663242

MPU6050

Submitted by 孙之问 on Nov 29, 2017 at 15:59
Language: Arduino. Code size: 2.9 kB.

```/***************************************************************************************************************************
Self-balancing Car
@file: D:\study\北理科协\自平衡小车\方案设计书\代码示例\MPU6050.ino
@author: Zhiwen Sun
@date: 2017.11.29
@bridf: introduction to MPU6050
***************************************************************************************************************************/

#include <Wire.h>

/*

*/
#define dataNum         7

int        dataBufferOfMPU[dataNum];//从MPU6050读取的数据缓存，0-3为X, Y, Z轴加速度，4为温度，5-7为X, Y, Z轴角速度
float      dt = 0.005;//时间间隔5ms
float      weightOfAcc;//一阶滤波中由加速度计算所得角度的权重，由实验与经验值给出
float      accXOffset, accYOffset, accZOffset, gyrXOffset, gyrYOffset, gyrZOffset;//数据零偏，将根据经验值给出或在启动后计算。

void writeMPUData(int addr, unsigned char data)
{
Wire.write(data);
Wire.endTransmission(true);
return;
}
{
Wire.requestFrom(MPU, 1, true);//从总线中读取一个字节的数据
Wire.endTransmission(true);
}

//从MPU6050读取加速度及角加速度
{
Wire.requestFrom(MPU, dataNum * 7, true);//7个数据共14个字节
Wire.endTransmission(true);
for (int i = 0; i < 7; ++i)
{
}
return;
}

//通过数据处理融合得到倾斜角度
float getAngle()
{
float angle, angleFromAcc, angleFromGyr;
float data[dataNum - 1];//存储目前的数据

//将数据缓存中的数据转换存储并消除零偏
data[0] = (float)(dataBufferOfMPU[0] - accXOffset) * 2.0 / 32768;
data[1] = (float)(dataBufferOfMPU[1] - accYOffset) * 2.0 / 32768;
data[2] = (float)(dataBufferOfMPU[2] - accZOffset) * 2.0 / 32768;

data[3] = (float)(dataBufferOfMPU[4] - gyrXOffset) * 250 / 32768;
data[4] = (float)(dataBufferOfMPU[5] - gyrYOffset) * 250 / 32768;
data[5] = (float)(dataBufferOfMPU[6] - gyrZOffset) * 250 / 32768;
//其中的data1，data3，data5好像没啥用，但还是读出来了意思意思

angleFromAcc = atan2(data[0], 1 - data[2]) * Rad2Deg;
angleFromGyr = anglePre + data[4] * dt;
angle = angleFromAcc * weightOfAcc + angleFromGyr * (1 - weightOfAcc);//一阶滤波
return angle;
}
```

This snippet took 0.01 seconds to highlight.

Back to the Entry List or Home.