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

--- 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);
     }
 }
+