![](/media/cache/profiles/sample02.jpg.50x50_q85.jpg)
accel.v2
Dependencies: mbed MPU6050 Math
Revision 8:a6a8997a8879, committed 2022-06-04
- Comitter:
- kosukesuzuki
- Date:
- Sat Jun 04 10:26:02 2022 +0000
- Parent:
- 7:8f914ead7fc0
- Commit message:
- vertical from accel.v1
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sat Nov 24 00:01:13 2018 +0000 +++ b/main.cpp Sat Jun 04 10:26:02 2022 +0000 @@ -1,52 +1,81 @@ #include "mbed.h" #include "MPU6050.h" -DigitalOut myled(LED1); MPU6050 mpu(p9,p10);//(SDA,SCL)のピン配置 +const float dt = 0.01; + +const float K1x = 0.9,K2x = 1 - K1x; //X +const float K1y = 0.9,K2y = 1 - K1y; //y +const float K1z = 0.9,K2z = 1 - K1z; //Z + + int main() { - float filterCoefficient = 0.9; // ローパスフィルターの係数(これは環境によって要調整。1に近づけるほど平滑化の度合いが大きくなる。 - float lowpassValue = 0; - float highpassValue = 0; - float speed = 0;//加速度時から算出した速度 - float oldSpeed = 0;//ひとつ前の速度 - float oldAccel = 0;//ひとつ前の加速度 - float difference=0;//変位 - float timespan=0.01;//時間差 - int accel[3];//accelを3つの配列で定義。 + //float a = 0; //iのデータ + float xi =0; //i-1のデータ oldaccel + float yi =0; + float zi =0; + + float Ax = 0; //reading STEINER + float Ay = 0; + float Az = 0; + + float LPFx = 0, HPFx = 0; + float LPFy = 0, HPFy = 0; + float LPFz = 0, HPFz = 0; + + //float HPFa = 0; //使わない + + float Xspeed = 0; + float Yspeed = 0; + float Zspeed = 0; + + float SPEED = 0; + + int accel[3];//accelを3つの配列で定義 + while(1){ + xi = Ax; + yi = Ay; + zi = Az; + mpu.readAccelData(accel);//加速度の値をaccel[3]に代入 int x = accel[0]-123;//x軸方向の加速度 int y = accel[1]+60;//y軸方向の加速度 int z = accel[2]+1110 ;//z軸方向の加速度 - float X = x*0.000597964111328125; - float Y = y*0.000597964111328125; - float Z = z*0.000597964111328125; - double a = X*X+Y*Y+Z*Z-95.982071137936; - if (a>0){ - a = sqrt(a); - } - if (a<0) { - a = abs(a); - a = -sqrt(a); - } - //printf("%lf %f %f %f\r\n",a,X,Y,Z); - // ローパスフィルター(現在の値 = 係数 * ひとつ前の値 + (1 - 係数) * センサの値) - lowpassValue = lowpassValue * filterCoefficient + a * (1 - filterCoefficient); - // ハイパスフィルター(センサの値 - ローパスフィルターの値)// - highpassValue = a - lowpassValue; - - // 速度計算(加速度を台形積分する) - speed = ((highpassValue + oldAccel) * timespan) / 2 + speed; - oldAccel = highpassValue; - - // 変位計算(速度を台形積分する) - difference = ((speed + oldSpeed) * timespan) / 2 + difference; - oldSpeed = speed; - - //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示 - //printf("%f,",speed);//速度を表示 - printf("speed %f diference %f\r\n",speed,difference);//速度と変位を表示 - wait(0.01); + + float X = (float)x*0.000597964111328125; + float Y = (float)y*0.000597964111328125; + float Z = (float)z*0.000597964111328125; + + //double a = sqrt(X*X+Y*Y+Z*Z)-9.81; + //printf("%.2f %.2f %.2f %.2f\r\n",X,Y,Z,a);//速度と変位を表示 + + Ax = X; + Ay = Y; + Az = Z; + + //ローパスフィルタ + LPFx = LPFx*K1x+K2x*X; + LPFy = LPFy*K1y+K2y*Y; + LPFz = LPFz*K1z+K2z*Z; + + //ハイパスフィルタ + HPFx = X-LPFx; + HPFy = Y-LPFy; + HPFz = Z-LPFz; + + //HPFa = sqrt(HPFx*HPFx+HPFy*HPFy+HPFz*HPFz); //使わない + + Xspeed = ((HPFx + xi)*dt)/2 + Xspeed; + Yspeed = ((HPFy + yi)*dt)/2 + Yspeed; + Zspeed = ((HPFz + zi)*dt)/2 + Zspeed; + + //printf("%.2f\r\n",a);//速度と変位を表示 + printf("%.2f %.2f %.2f\r\n",Xspeed,Yspeed,Zspeed); //速度と変位を表示 + SPEED = sqrt(Xspeed*Xspeed+Yspeed*Yspeed+Zspeed*Zspeed-96.2361); + printf("%.2f \r\n",SPEED);//速度と変位を表示 + wait(dt); } } +