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 ADDR_MPU        0x68//MPU6050I2C总线地址
#define ADDR_ACC_X      0x3B//X轴加速度数据地址
#define ADDR_ACC_Y      0x3D//Y轴加速度数据地址
#define ADDR_ACC_Z      0x3F//Z轴加速度数据地址
#define ADDR_GYR_X      0x43//X轴角速度数据地址
#define ADDR_GYR_Y      0x45//Y轴角速度数据地址
#define ADDR_GYR_Z      0x47//Z轴角速度数据地址
/*
每个存储的数据均为2字节,16位(-32768~+32768)
加速度以g为单位,默认量程为2g
加速度以度/s为单位,默认量程为250
因此每个数据地址之间会有一个字节(如XY轴加速度地址之间的0x3C)
加速度和角速度地址之间还有温度数据的地址
*/
#define dataNum         7
#define Rad2Deg         57.2957795//由弧度转变为角度,=180/PI
 
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;//数据零偏,将根据经验值给出或在启动后计算。

//向MPU6050的addr位置写入data的数据
void writeMPUData(int addr, unsigned char data)
{
    Wire.beginTransmission(ADDR_MPU);
    Wire.write(addr);
    Wire.write(data);
    Wire.endTransmission(true);
    return; 
}
//从MPU6050的addr位置读取一个字节的数据
unsigned char readMPUData(int addr)
{
    Wire.beginTransmission(ADDR_MPU);
    Wire.write(addr);
    Wire.requestFrom(MPU, 1, true);//从总线中读取一个字节的数据
    Wire.endTransmission(true);
    return Wire.read(); 
}

//从MPU6050读取加速度及角加速度
void readAccGyrData()
{
    Wire.beginTransmission(ADDR_MPU);
    Wire.write(ADDR_ACC_X);
    Wire.requestFrom(MPU, dataNum * 7, true);//7个数据共14个字节
    Wire.endTransmission(true);
    for (int i = 0; i < 7; ++i)
    {
        dataBufferOfMPU[i] = (Wire.read() << 8) | Wire.read();
    }
    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.

Delete this entry (admin only).