Diff: main.cpp

Revision:

7:30613e31988e

Parent:

6:f1106d9e843c

Child:

8:2b8cd80bea3b

diff -r f1106d9e843c -r 30613e31988e main.cpp
--- a/main.cpp	Tue Nov 20 13:51:57 2018 +0000
+++ b/main.cpp	Fri Nov 23 16:49:19 2018 +0000
@@ -4,46 +4,25 @@
 DigitalOut myled(LED1);
 MPU6050 mpu(p9,p10);//(SDA,SCL)のピン配置
 
-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 main(void) {
         int accel[3];//accelを3つの配列で定義。
-    while(1){
+        int bias=0;//補正値を定義。
+        int z;//z軸方向の加速度。
+        float Z;//加速度の変換後の値。
+//以下はキャリブレーションテスト用のプログラム。値を入れてテストしてください。
+//while(1){mpu.readAccelData(accel);z = accel[2]+1150;Z = z*0.000597964111328125;printf("%f\r\n",Z);}
+    while(Z < 9.7970444){
         mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
-        int x = accel[0];//x軸方向の加速度
-        int y = accel[1];//y軸方向の加速度
-        int z = accel[2];//z軸方向の加速度
-        float a = x^2+y^2+z^2;
-        printf("%f\r\n",a);
-        if (a>0){
-        a = sqrt(a);
-        }
-        if (a<0) {
-        a = abs(a);
-        a = -sqrt(a);
-        }
-        // ローパスフィルター(現在の値 = 係数 * ひとつ前の値 ＋ (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.5);
+        //int x = accel[0];//x軸方向の加速度
+        //int y = accel[1];//y軸方向の加速度
+        z = accel[2]+bias;//z軸方向の加速度
+        //float X = x*0.000597964111328125;
+        //float Y = y*0.000597964111328125;
+        Z = z*0.000597964111328125; 
+        //double a = X*X+Y*Y+Z*Z-95.982071137936;
+        //printf("%lf %f %f %f\r\n",a,X,Y,Z);
+        printf("%f\r\n",Z);
+        bias++;
     }
+        printf("bias=%d\r\n",bias);
 }