otamesi
Dependencies: mbed
Diff: main.cpp
- Revision:
- 16:44c763c32b0d
- Parent:
- 15:75f014c4c8b8
- Child:
- 17:9bc130ebb5ed
--- a/main.cpp Sat Nov 10 02:12:38 2018 +0000 +++ b/main.cpp Thu Nov 15 09:18:24 2018 +0000 @@ -51,7 +51,7 @@ motor1.stop(0); motor2.stop(0); - /* printf("GPS start\r\n"); + printf("GPS start\r\n"); FILE *fp = fopen("/local/gps.txt", "w"); // Open "gps.txt" on the local file system for writing fprintf(fp, "GPS Start\r\n"); int n; @@ -70,19 +70,19 @@ } fprintf(fp,"GPS finish\r\n"); - fclose(fp); //GPSの測定終了 */ + fclose(fp); // GPSの測定終了 compass.init(); //地磁気センサー動作 - /* + int i; for(i=0;i<20;i++) //地磁気測定 { pc.printf("raw=%f\r\n",compass.getHeadingXYDeg()); //度数法で表記 wait(0.5); -}*/ +} float mc1,mc2; mc1=3.0; mc2=3.0; @@ -145,7 +145,7 @@ float oldAccel = 0;//ひとつ前の加速度 float difference=0;//変位 float timespan=0.1;//時間差 - float accel[3];//accelを3つの配列で定義。 加速度センサー + float accel[3];//accelを3つの配列で定義。 加速度センサー*/ while(1) { @@ -256,6 +256,8 @@ motor1.speed(ms1); //直進 motor2.speed(ms2); + + int t=0; for(t=0;t<100;t++) { @@ -279,7 +281,32 @@ } printf("%f", rightrun); printf("\t%f\r\n", leftrun2); - } + + + + mpu.readAccelData(accel);//加速度の値をaccel[3]に代入 + float x = accel[0]/16384-0.043;//x軸方向の加速度 + float y = accel[1]/16384-0.012;//y軸方向の加速度 + float z = accel[2]/16384;//z軸方向の加速度 + printf("accel x:%f y:%f z:%f\r\n",x,y,z);//一応表示しとくやで~~ + // ローパスフィルター(現在の値 = 係数 * ひとつ前の値 + (1 - 係数) * センサの値) + lowpassValue = lowpassValue * filterCoefficient + x * (1 - filterCoefficient); + // ハイパスフィルター(センサの値 - ローパスフィルターの値) + highpassValue = x - lowpassValue; + + // 速度計算(加速度を台形積分する) + speed = ((highpassValue + oldAccel) * timespan) / 2 + speed; + oldAccel = highpassValue; + + // 変位計算(速度を台形積分する) + difference = ((speed + oldSpeed) * timespan) / 2 + difference; + oldSpeed = speed; + + printf(" speed %f difference %f\n",speed,difference); + } + + + printf("mortor straight\r\n"); motor1.stop(0);