otamesi
Dependencies: mbed
Diff: main.cpp
- Revision:
- 19:8ad7dcfef11f
- Parent:
- 18:2a47ed430cfe
- Child:
- 20:e78bffff80ee
--- a/main.cpp Fri Nov 23 09:31:07 2018 +0000 +++ b/main.cpp Sun Nov 25 04:12:06 2018 +0000 @@ -64,6 +64,8 @@ motor1.stop(0); motor2.stop(0); + wait(30); //確認用 + 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"); @@ -83,7 +85,7 @@ } fprintf(fp,"GPS finish\r\n"); - fclose(fp); // GPSの測定終了 + // fclose(fp); // GPSの測定終了 @@ -166,11 +168,13 @@ printf("%d\r\n",distance); 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); + 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); } @@ -178,10 +182,11 @@ a = abs(a); a = -sqrt(a); } + //printf("%lf %f %f %f\r\n",a,X,Y,Z); // ローパスフィルター(現在の値 = 係数 * ひとつ前の値 + (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; @@ -192,8 +197,7 @@ oldSpeed = speed; //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示 - //printf("%f,",speed);//速度を表示 - printf("%f,",difference);//変位を表示 単位m + printf("speed %f diference %f\r\n",speed,difference);//速度(m)と変位を表示 printf("%d\r\n", test.read()); //フォトインタラプタ printf("%d\r\n", test2.read()); @@ -222,6 +226,11 @@ break; //つまりゴールについたらこのループからぬける } + if(difference >= 0.3) + { + break; + } + motor1.speed(0.5); //通常走行 motor2.speed(0.5); //Do something else here @@ -308,13 +317,25 @@ 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]-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 + x * (1 - filterCoefficient); + lowpassValue = lowpassValue * filterCoefficient + a * (1 - filterCoefficient); // ハイパスフィルター(センサの値 - ローパスフィルターの値)// - highpassValue = x - lowpassValue; + highpassValue = a - lowpassValue; // 速度計算(加速度を台形積分する) speed = ((highpassValue + oldAccel) * timespan) / 2 + speed; @@ -325,8 +346,7 @@ oldSpeed = speed; //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示 - //printf("%f,",speed);//速度を表示 - printf("%f,",difference);//変位を表示 + printf("speed %f diference %f\r\n",speed,difference);//速度(m)と変位を表示 wait(0.01); } @@ -375,7 +395,12 @@ } motor1.stop(0); motor2.stop(0); - + + + fprintf(fp, "last accel.photo\r\n"); + fprintf(fp,"(%lf, %lf)\r\n", difference, rightrun);//最後の加速度とフォトインタラプタによる距離を出力 + fclose(fp); + }