Tatsuya Hiruma / Mbed 2 deprecated test2

gg

Dependencies:   mbed MPU6050 RateLimiter test Math

main.cpp@4:d2bb399c7c8c, 2018-11-09 (annotated)

Committer:
falconsyunya
Date:
Fri Nov 09 21:03:51 2018 +0000
Revision:
4:d2bb399c7c8c
Parent:
3:157b19421485
Child:
5:8bc5cef21589
2018/11/10_6:03

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 4:d2bb399c7c8c 8 float filterCoefficient = 0.3; // ローパスフィルターの係数(これは環境によって要調整。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 4:d2bb399c7c8c 15 float timespan=0.1;//時間差
falconsyunya 4:d2bb399c7c8c 16 float accel[3];//accelを3つの配列で定義。
falconsyunya 0:4ba3fa671062 17 while(1){
falconsyunya 1:958b581f6d21 18 mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
falconsyunya 4:d2bb399c7c8c 19 float x = accel[0]/16384-0.043;//x軸方向の加速度
falconsyunya 4:d2bb399c7c8c 20 float y = accel[1]/16384-0.012;//y軸方向の加速度
falconsyunya 1:958b581f6d21 21 float z = accel[2]/16384;//z軸方向の加速度
falconsyunya 3:157b19421485 22 printf("accel x:%f y:%f z:%f\r\n",x,y,z);//加速度の表示
falconsyunya 4:d2bb399c7c8c 23 // ローパスフィルター(現在の値 = 係数 * ひとつ前の値 ＋ (1 - 係数) * センサの値)
falconsyunya 4:d2bb399c7c8c 24 lowpassValue = lowpassValue * filterCoefficient + x * (1 - filterCoefficient);
falconsyunya 4:d2bb399c7c8c 25 // ハイパスフィルター(センサの値 - ローパスフィルターの値)
falconsyunya 4:d2bb399c7c8c 26 highpassValue = x - lowpassValue;
falconsyunya 4:d2bb399c7c8c 27
falconsyunya 4:d2bb399c7c8c 28 // 速度計算(加速度を台形積分する)
falconsyunya 4:d2bb399c7c8c 29 speed = ((highpassValue + oldAccel) * timespan) / 2 + speed;
falconsyunya 4:d2bb399c7c8c 30 oldAccel = highpassValue;
falconsyunya 4:d2bb399c7c8c 31
falconsyunya 4:d2bb399c7c8c 32 // 変位計算(速度を台形積分する)
falconsyunya 4:d2bb399c7c8c 33 difference = ((speed + oldSpeed) * timespan) / 2 + difference;
falconsyunya 4:d2bb399c7c8c 34 oldSpeed = speed;
falconsyunya 4:d2bb399c7c8c 35
falconsyunya 4:d2bb399c7c8c 36 printf(" speed %f difference %f\n",speed,difference);
falconsyunya 3:157b19421485 37 wait(0.1);
falconsyunya 0:4ba3fa671062 38 }
falconsyunya 0:4ba3fa671062 39 }