Tatsuya Hiruma / Mbed 2 deprecated test2

gg

Dependencies:   mbed MPU6050 RateLimiter test Math

main.cpp@7:8f914ead7fc0, 2018-11-24 (annotated)

Committer:
falconsyunya
Date:
Sat Nov 24 00:01:13 2018 +0000
Revision:
7:8f914ead7fc0
Parent:
6:f1106d9e843c 
2018/11/24 9:01

Who changed what in which revision?

UserRevisionLine numberNew contents of line
falconsyunya 0:4ba3fa671062 1 #include "mbed.h"
falconsyunya 0:4ba3fa671062 2 #include "MPU6050.h"
falconsyunya 0:4ba3fa671062 3
falconsyunya 0:4ba3fa671062 4 DigitalOut myled(LED1);
falconsyunya 3:157b19421485 5 MPU6050 mpu(p9,p10);//(SDA,SCL)のピン配置
falconsyunya 0:4ba3fa671062 6
falconsyunya 0:4ba3fa671062 7 int main() {
falconsyunya 5:8bc5cef21589 8 float filterCoefficient = 0.9; // ローパスフィルターの係数(これは環境によって要調整。1に近づけるほど平滑化の度合いが大きくなる。
falconsyunya 4:d2bb399c7c8c 9 float lowpassValue = 0;
falconsyunya 4:d2bb399c7c8c 10 float highpassValue = 0;
falconsyunya 4:d2bb399c7c8c 11 float speed = 0;//加速度時から算出した速度
falconsyunya 4:d2bb399c7c8c 12 float oldSpeed = 0;//ひとつ前の速度
falconsyunya 4:d2bb399c7c8c 13 float oldAccel = 0;//ひとつ前の加速度
falconsyunya 4:d2bb399c7c8c 14 float difference=0;//変位
falconsyunya 5:8bc5cef21589 15 float timespan=0.01;//時間差
falconsyunya 6:f1106d9e843c 16 int accel[3];//accelを3つの配列で定義。
falconsyunya 0:4ba3fa671062 17 while(1){
falconsyunya 1:958b581f6d21 18 mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
falconsyunya 7:8f914ead7fc0 19 int x = accel[0]-123;//x軸方向の加速度
falconsyunya 7:8f914ead7fc0 20 int y = accel[1]+60;//y軸方向の加速度
falconsyunya 7:8f914ead7fc0 21 int z = accel[2]+1110 ;//z軸方向の加速度
falconsyunya 7:8f914ead7fc0 22 float X = x*0.000597964111328125;
falconsyunya 7:8f914ead7fc0 23 float Y = y*0.000597964111328125;
falconsyunya 7:8f914ead7fc0 24 float Z = z*0.000597964111328125;
falconsyunya 7:8f914ead7fc0 25 double a = X*X+Y*Y+Z*Z-95.982071137936;
falconsyunya 6:f1106d9e843c 26 if (a>0){
falconsyunya 6:f1106d9e843c 27 a = sqrt(a);
falconsyunya 6:f1106d9e843c 28 }
falconsyunya 6:f1106d9e843c 29 if (a<0) {
falconsyunya 6:f1106d9e843c 30 a = abs(a);
falconsyunya 6:f1106d9e843c 31 a = -sqrt(a);
falconsyunya 6:f1106d9e843c 32 }
falconsyunya 7:8f914ead7fc0 33 //printf("%lf %f %f %f\r\n",a,X,Y,Z);
falconsyunya 4:d2bb399c7c8c 34 // ローパスフィルター(現在の値 = 係数 * ひとつ前の値 ＋ (1 - 係数) * センサの値)
falconsyunya 6:f1106d9e843c 35 lowpassValue = lowpassValue * filterCoefficient + a * (1 - filterCoefficient);
falconsyunya 5:8bc5cef21589 36 // ハイパスフィルター(センサの値 - ローパスフィルターの値)//
falconsyunya 6:f1106d9e843c 37 highpassValue = a - lowpassValue;
falconsyunya 4:d2bb399c7c8c 38
falconsyunya 4:d2bb399c7c8c 39 // 速度計算(加速度を台形積分する)
falconsyunya 4:d2bb399c7c8c 40 speed = ((highpassValue + oldAccel) * timespan) / 2 + speed;
falconsyunya 4:d2bb399c7c8c 41 oldAccel = highpassValue;
falconsyunya 4:d2bb399c7c8c 42
falconsyunya 4:d2bb399c7c8c 43 // 変位計算(速度を台形積分する)
falconsyunya 4:d2bb399c7c8c 44 difference = ((speed + oldSpeed) * timespan) / 2 + difference;
falconsyunya 4:d2bb399c7c8c 45 oldSpeed = speed;
falconsyunya 4:d2bb399c7c8c 46
falconsyunya 5:8bc5cef21589 47 //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示
falconsyunya 5:8bc5cef21589 48 //printf("%f,",speed);//速度を表示
falconsyunya 7:8f914ead7fc0 49 printf("speed %f diference %f\r\n",speed,difference);//速度と変位を表示
falconsyunya 7:8f914ead7fc0 50 wait(0.01);
falconsyunya 0:4ba3fa671062 51 }
falconsyunya 0:4ba3fa671062 52 }