otamesi
Dependencies: mbed
Diff: main.cpp
- Revision:
- 15:75f014c4c8b8
- Parent:
- 13:b884f5960fbf
- Child:
- 16:44c763c32b0d
--- a/main.cpp Thu Nov 08 11:44:46 2018 +0000 +++ b/main.cpp Sat Nov 10 02:12:38 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; @@ -76,13 +76,13 @@ 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; @@ -137,6 +137,14 @@ rightrun=0.0; leftrun2=0.0; + float filterCoefficient = 0.3; // ローパスフィルターの係数(これは環境によって要調整。1に近づけるほど平滑化の度合いが大きくなる。 + float lowpassValue = 0; + float highpassValue = 0; + float speed = 0;//加速度時から算出した速度 + float oldSpeed = 0;//ひとつ前の速度 + float oldAccel = 0;//ひとつ前の加速度 + float difference=0;//変位 + float timespan=0.1;//時間差 float accel[3];//accelを3つの配列で定義。 加速度センサー while(1) @@ -145,11 +153,24 @@ printf("%d\r\n",distance); mpu.readAccelData(accel);//加速度の値をaccel[3]に代入 - float x = accel[0]/16384;//x軸方向の加速度 - float y = accel[1]/16384;//y軸方向の加速度 + 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("%d\r\n", test.read()); //フォトインタラプタ printf("%d\r\n", test2.read()); @@ -160,7 +181,7 @@ else if (test.read() == 0 and flag == 1){ //そうじゃなくて今度はとうとうtestが0でスリットの部分になった瞬間なのにスイッチが1で切れているときは flag = 0; //まずこれでスイッチを0にして入れる。 //こうすることで同じスリットの中でtestが複数回0を返した時に何回も52.5mmを加算しつづけるということがなくなる - rightrun += 52.5; //総距離runに52.5を加算する + rightrun += 54.95; //総距離runに52.5を加算する printf("test.read else\r\n"); } if (test2.read() == 1 and flag2 == 0){ @@ -169,12 +190,12 @@ } else if (test2.read() == 0 and flag2 == 1){ flag2 = 0; - leftrun2 += 52.5; + leftrun2 += 54.95; printf("test2.read else\r\n"); } printf("%f", rightrun); printf("\t%f\r\n", leftrun2); - if (rightrun > 525){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する + if (rightrun >= 4396){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する break; //つまりゴールについたらこのループからぬける } @@ -268,7 +289,7 @@ motor1.speed(-msj1); //機体を反時計回りに90度回転 motor2.speed(msj2); - wait(0.77); + wait(1); printf("mortor rotation\r\n"); motor1.stop(0); @@ -288,7 +309,7 @@ motor1.speed(msj1); //機体を時計回りに90度回転 motor2.speed(-msj2); - wait(0.77); + wait(1); printf("mortor rotation\r\n"); motor1.stop(0);