cansat-d_2018
/
MPU6050_get_offset
MPU6050のオフセット値を測定し修正するプログラムです。
Diff: main.cpp
- Revision:
- 6:f1106d9e843c
- Parent:
- 5:8bc5cef21589
- Child:
- 7:30613e31988e
diff -r 8bc5cef21589 -r f1106d9e843c main.cpp --- a/main.cpp Sat Nov 17 09:40:32 2018 +0000 +++ b/main.cpp Tue Nov 20 13:51:57 2018 +0000 @@ -13,16 +13,25 @@ float oldAccel = 0;//ひとつ前の加速度 float difference=0;//変位 float timespan=0.01;//時間差 - float accel[3];//accelを3つの配列で定義。 + int accel[3];//accelを3つの配列で定義。 while(1){ mpu.readAccelData(accel);//加速度の値をaccel[3]に代入 - float x = accel[0]/16384-0.043;//x軸方向の加速度 - float y = accel[0]/16384;//y軸方向の加速度 - float z = accel[0]/16384;//z軸方向の加速度 + 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 + x * (1 - filterCoefficient); + lowpassValue = lowpassValue * filterCoefficient + a * (1 - filterCoefficient); // ハイパスフィルター(センサの値 - ローパスフィルターの値)// - highpassValue = x - lowpassValue; + highpassValue = a - lowpassValue; // 速度計算(加速度を台形積分する) speed = ((highpassValue + oldAccel) * timespan) / 2 + speed; @@ -34,7 +43,7 @@ //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示 //printf("%f,",speed);//速度を表示 - printf("%f,",difference);//変位を表示 - wait(0.01); + //printf("speed %f diference %f\r\n",speed,difference);//変位を表示 + wait(0.5); } }