![](/media/cache/profiles/sample02.jpg.50x50_q85.jpg)
accel.v2
Dependencies: mbed MPU6050 Math
main.cpp@5:8bc5cef21589, 2018-11-17 (annotated)
- Committer:
- falconsyunya
- Date:
- Sat Nov 17 09:40:32 2018 +0000
- Revision:
- 5:8bc5cef21589
- Parent:
- 4:d2bb399c7c8c
- Child:
- 6:f1106d9e843c
2018/11/17
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 | 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 | 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 | 5:8bc5cef21589 | 20 | float y = accel[0]/16384;//y軸方向の加速度 |
falconsyunya | 5:8bc5cef21589 | 21 | float z = accel[0]/16384;//z軸方向の加速度 |
falconsyunya | 4:d2bb399c7c8c | 22 | // ローパスフィルター(現在の値 = 係数 * ひとつ前の値 + (1 - 係数) * センサの値) |
falconsyunya | 4:d2bb399c7c8c | 23 | lowpassValue = lowpassValue * filterCoefficient + x * (1 - filterCoefficient); |
falconsyunya | 5:8bc5cef21589 | 24 | // ハイパスフィルター(センサの値 - ローパスフィルターの値)// |
falconsyunya | 4:d2bb399c7c8c | 25 | highpassValue = x - lowpassValue; |
falconsyunya | 4:d2bb399c7c8c | 26 | |
falconsyunya | 4:d2bb399c7c8c | 27 | // 速度計算(加速度を台形積分する) |
falconsyunya | 4:d2bb399c7c8c | 28 | speed = ((highpassValue + oldAccel) * timespan) / 2 + speed; |
falconsyunya | 4:d2bb399c7c8c | 29 | oldAccel = highpassValue; |
falconsyunya | 4:d2bb399c7c8c | 30 | |
falconsyunya | 4:d2bb399c7c8c | 31 | // 変位計算(速度を台形積分する) |
falconsyunya | 4:d2bb399c7c8c | 32 | difference = ((speed + oldSpeed) * timespan) / 2 + difference; |
falconsyunya | 4:d2bb399c7c8c | 33 | oldSpeed = speed; |
falconsyunya | 4:d2bb399c7c8c | 34 | |
falconsyunya | 5:8bc5cef21589 | 35 | //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示 |
falconsyunya | 5:8bc5cef21589 | 36 | //printf("%f,",speed);//速度を表示 |
falconsyunya | 5:8bc5cef21589 | 37 | printf("%f,",difference);//変位を表示 |
falconsyunya | 5:8bc5cef21589 | 38 | wait(0.01); |
falconsyunya | 0:4ba3fa671062 | 39 | } |
falconsyunya | 0:4ba3fa671062 | 40 | } |