accel.v2
Dependencies: mbed MPU6050 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
- Child:
- 8:a6a8997a8879
2018/11/24 9:01
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 | 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 | } |