File content as of revision 8:a6a8997a8879:

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

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

const float dt = 0.01;

const float K1x = 0.9,K2x = 1 - K1x; //X
const float K1y = 0.9,K2y = 1 - K1y; //y
const float K1z = 0.9,K2z = 1 - K1z; //Z


int main() {
        //float a = 0; //iのデータ
        float xi =0; //i-1のデータ　oldaccel
        float yi =0;
        float zi =0;
        
        float Ax = 0; //reading STEINER
        float Ay = 0;
        float Az = 0;
        
        float LPFx = 0, HPFx = 0;
        float LPFy = 0, HPFy = 0;
        float LPFz = 0, HPFz = 0;
        
        //float HPFa = 0;  //使わない
        
        float Xspeed = 0;
        float Yspeed = 0;
        float Zspeed = 0;
        
        float SPEED = 0;
        
        int accel[3];//accelを3つの配列で定義
        
    while(1){
        xi = Ax;
        yi = Ay;
        zi = Az;
        
        mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
        int x = accel[0]-123;//x軸方向の加速度
        int y = accel[1]+60;//y軸方向の加速度
        int z = accel[2]+1110 ;//z軸方向の加速度
        
        float X = (float)x*0.000597964111328125;
        float Y = (float)y*0.000597964111328125;
        float Z = (float)z*0.000597964111328125; 
        
        //double a = sqrt(X*X+Y*Y+Z*Z)-9.81;
        //printf("%.2f %.2f %.2f %.2f\r\n",X,Y,Z,a);//速度と変位を表示
        
        Ax = X;
        Ay = Y;
        Az = Z;
        
        //ローパスフィルタ
        LPFx = LPFx*K1x+K2x*X;
        LPFy = LPFy*K1y+K2y*Y;
        LPFz = LPFz*K1z+K2z*Z;
        
        //ハイパスフィルタ
        HPFx = X-LPFx;
        HPFy = Y-LPFy;
        HPFz = Z-LPFz;
        
        //HPFa = sqrt(HPFx*HPFx+HPFy*HPFy+HPFz*HPFz); //使わない
        
        Xspeed = ((HPFx + xi)*dt)/2 + Xspeed;
        Yspeed = ((HPFy + yi)*dt)/2 + Yspeed;
        Zspeed = ((HPFz + zi)*dt)/2 + Zspeed;
        
        //printf("%.2f\r\n",a);//速度と変位を表示
        printf("%.2f %.2f %.2f\r\n",Xspeed,Yspeed,Zspeed);    //速度と変位を表示
        SPEED = sqrt(Xspeed*Xspeed+Yspeed*Yspeed+Zspeed*Zspeed-96.2361);
        printf("%.2f \r\n",SPEED);//速度と変位を表示
        wait(dt);
    }
}