MPU6050のオフセット値を測定し修正するプログラムです。

Dependencies:   mbed MPU6050

main.cpp

Committer:
falconsyunya
Date:
2018-11-23
Revision:
8:2b8cd80bea3b
Parent:
7:30613e31988e
Child:
9:79c7f239b6a0

File content as of revision 8:2b8cd80bea3b:

#include "mbed.h"
#include "MPU6050.h"

DigitalOut myled(LED1);
MPU6050 mpu(p9,p10);//(SDA,SCL)のピン配置

int main(void) {
        int accel[3];//accelを3つの配列で定義。
        int biasx=0;//x補正値を定義。
        float biasxsum=0;
        float biasxav=0;
        int biasy=0;//y補正値を定義。
        float biasysum=0;
        float biasyav=0;
        int biasz=0;//z補正値を定義。
        float biaszsum=0;
        float biaszav=0;
        int x;//x軸方向の加速度。
        int y;//y軸方向の加速度。
        int z;//z軸方向の加速度。
        float X;//加速度の変換後の値。
        float Y;//加速度の変換後の値。
        float Z;//加速度の変換後の値。
//Z軸方向の加速度バイアス平均値計算。
    for(int i=0;i<2000;i++){
        mpu.readAccelData(accel);
        if(Z<9.797044){
        biasz++;
        z = accel[2]+biasz;//z軸方向の加速度
        Z = z*0.000597964111328125; 
        printf("%f\r\n",Z);
        }
        else{
        biasz--;
        z = accel[2]+biasz;//z軸方向の加速度
        Z = z*0.000597964111328125; 
        printf("%f\r\n",Z);
        }
        }
    for(int i=0;i<100;i++){
        mpu.readAccelData(accel);
        if(Z<9.797044){
        biasz++;
        z = accel[2]+biasz;
        Z = z*0.000597964111328125; 
        printf("%f\r\n",Z);
        }
        else{
        biasz--;
        z = accel[2]+biasz;
        Z = z*0.000597964111328125; 
        printf("%f\r\n",Z);
        }
        biaszsum = biaszsum + biasz;
        }
        biaszav = biaszsum/101;
        printf("bias=%f\r\n",biaszav);
        getchar();
//Z軸方向の加速度バイアス計算終了。
//X軸方向の加速度バイアス平均値計算。
    for(int i=0;i<2000;i++){
        mpu.readAccelData(accel);
        if(X<9.797044){
        biasx++;
        x = accel[0]+biasx;
        X = x*0.000597964111328125; 
        printf("%f\r\n",X);
        }
        else{
        biasx--;
        x = accel[2]+biasx;
        X = x*0.000597964111328125; 
        printf("%f\r\n",X);
        }
        }
    for(int i=0;i<100;i++){
        mpu.readAccelData(accel);
        if(X<9.797044){
        biasx++;
        x = accel[0]+biasx;
        X = x*0.000597964111328125; 
        printf("%f\r\n",X);
        }
        else{
        biasx--;
        x = accel[0]+biasx;
        X = x*0.000597964111328125; 
        printf("%f\r\n",X);
        }
        biasxsum = biasxsum + biasx;
        }
        biasxav = biasxsum/101;
        printf("bias=%f\r\n",biasxav);
        getchar();
//X軸方向の加速度バイアス計算終了。
    while(1){
        mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
        x = accel[0]+biasx;//x軸方向の加速度
        y = accel[1];//y軸方向の加速度
        z = accel[2]+biasz;//z軸方向の加速度
        X = x*0.000597964111328125;
        Y = y*0.000597964111328125;
        Z = z*0.000597964111328125; 
        double a = X*X+Y*Y+Z*Z-95.982071137936;
        printf("%lf %f %f %f\r\n",a,X,Y,Z);
    }
    }