File content as of revision 1:4df856c1b9d8:

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

DigitalOut myled(LED1);
Serial pc(USBTX, USBRX);
MPU6050 mpu;
Timer t;

int main()
{
    int16_t ax, ay, az;//acceleration
    int16_t gx, gy, gz;//angular velocity
    
    double gx0,gy0,gz0;//angular velocity
    double theta_x,theta_y,theta_z,theta_x0,theta_y0,theta_z0;//angle

    double p, q, r;

    double t_read = 0;
    
    pc.printf("MPU6050 test\n\n");
    pc.printf("MPU6050 initialize \n");

    mpu.initialize();
    pc.printf("MPU6050 testConnection \n");

    bool mpu6050TestResult = mpu.testConnection();
    if(mpu6050TestResult) {
        pc.printf("MPU6050 test passed \n");
    } else {
        pc.printf("MPU6050 test failed \n");
    }

    /***************calibration***************/
    double sum_x,sum_y,sum_z,ave_x,ave_y,ave_z;

    sum_x = 0;
    sum_y = 0;
    sum_z = 0;
    
    pc.printf("Calibration start ...\nJust a moment.\n");
    
    //ドリフト補正(1000回値をとって、その平均値を算出)
    for(int i=0;i<1000;i++){
        mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
        sum_x = sum_x+gx;
        sum_y = sum_y+gy;
        sum_z = sum_z+gz;
        wait(0.01);
    }
    
    ave_x = sum_x/1000;
    ave_y = sum_y/1000;
    ave_z = sum_z/1000;
    
    pc.printf("Calibration result:%f;%f;%f\n", ave_x, ave_y, ave_z);

    /*************main*************/
    gx0 = 0;
    gy0 = 0;
    gz0 = 0;
    theta_x0 = 0;
    theta_y0 = 0;
    theta_z0 = 0;
    
    while(1) {
        //timer start.
        t.start();
        mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
        
        //ドリフト補正＆スケーリング:scalling factor = 131[LSB/(°/s)]
        p = ((double)gx-ave_x) / 131;
        q = ((double)gy-ave_y) / 131;
        r = ((double)gz-ave_z) / 131;

        pc.printf("gx:%7.2f gy:%7.2f gz:%7.2f ",p, q, r);
        
        //台形積分
        theta_x = (gx0+p)/2*t_read+theta_x0;
        theta_y = (gy0+q)/2*t_read+theta_y0;
        theta_z = (gz0+r)/2*t_read+theta_z0;
        
        pc.printf("theta_x:%7.0f theta_y:%7.0f theta_z:%7.0f\n",theta_x,theta_y,theta_z);

        gx0 = p;
        gy0 = q;
        gz0 = r;
        theta_x0 = theta_x;
        theta_y0 = theta_y;
        theta_z0 = theta_z;
        
        wait(0.1);
        
        //timer reset & stop.
        t.stop();
        t_read = t.read();
        t.reset();
    }
}