![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
加速度センサのプログラム第2版です。 (PCに表示、zの値の正負で上下が分かると思います。)
Dependencies: mbed MPU6050 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?
User | Revision | Line number | New 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 | } |