otamesi
Dependencies: mbed
Diff: main.cpp
- Revision:
- 35:40df2f91cea9
- Parent:
- 34:0e8f95f07612
- Child:
- 36:c2b4071970de
--- a/main.cpp Fri Dec 14 06:53:50 2018 +0000 +++ b/main.cpp Fri Dec 14 14:57:28 2018 +0000 @@ -6,9 +6,12 @@ #include "MPU6050.h" -PwmOut motorSpeed(p26); -DigitalOut motorDir1(p19); -DigitalOut motorDir2(p20); +PwmOut motorSpeed1(p26); +PwmOut motorSpeed2(p25); +DigitalOut motor1Dir1(p19); +DigitalOut motor1Dir2(p20); +DigitalOut motor2Dir1(p16); +DigitalOut motor2Dir2(p17); Serial pc(USBTX, USBRX); //地磁気センサー,GPS @@ -45,9 +48,11 @@ void motorStop(void); void motorReverse(void); - +void motorForward2(void); +void motorStop2(void); +void motorReverse2(void); -LocalFileSystem local("local"); // Create the local filesystem under the name "local" データ保存 +LocalFileSystem local("local"); // Create the local filesystem under the name "local" データ保存 float culculate_distance_3(float a,float b); @@ -67,7 +72,7 @@ wait(10); } else{ - /* FET = 0; //FET、ニクロム線切断 + FET = 0; //FET、ニクロム線切断 wait(20); FET = 1; wait(12); @@ -75,7 +80,7 @@ wait(10); FET = 1; wait(12); - FET = 0; */ + FET = 0; SW = 0; //p23をlow(0V)にする。 break; } @@ -87,9 +92,9 @@ wait(20); //確認用 -// FILE *fp = fopen("/local/gps,foto.txt", "w"); // Open "gps.txt" on the local file system for writing +// FILE *fp = fopen("/local/gps.txt", "w"); // Open "gps.txt" on the local file system for writing - printf("GPS start\r\n"); + printf("GPS start\r\n"); FILE *fp = fopen("/local/gps,foto.txt", "w"); // Open "gps.txt" on the local file system for writing fprintf(fp, "GPS Start\r\n"); int n; @@ -108,11 +113,12 @@ } fprintf(fp,"GPS finish\r\n"); - // fclose(fp); // GPSの測定終了 */ + // fclose(fp); // GPSの測定終了 */ - motorSpeed.period_ms(4); //モーター調節 - motorSpeed = 0.955; - + motorSpeed1.period_ms(4); //モーター調節 + motorSpeed1 = 0.955; + motorSpeed2.period_ms(4); //モーター調節 + motorSpeed2 = 0.955; compass.init(); //地磁気センサー動作 @@ -128,7 +134,7 @@ mc1=1.0; mc2=1.0; //地磁気センサのキャリブレーション - motor1.speed(mc1); //車体を時計回りに3秒回転 + motorForward2(); //車体を時計回りに3秒回転 motorReverse(); wait(1.6); @@ -136,7 +142,7 @@ motor2.stop(0); wait(1); - motor1.speed(-mc1); //車体を反時計回りに3秒回 + motorReverse2(); //車体を反時計回りに3秒回 motorForward(); wait(1.6); @@ -152,21 +158,21 @@ compass.init(); heading=compass.getHeadingXYDeg(); if(90<heading<267.5){ - motor1.speed(mcn1);//右回転 + motorForward2();//右回転 motorReverse(); wait((270-heading)*0.004448); //角度のずれ*1度回転するのにかかる時間 motor1.stop(0); motor2.stop(0); wait(1); }else if(0<=heading<=90.0){ - motor1.speed(-mcn1);//左回転 + motorReverse2();//左回転 motorForward(); wait((heading+90.0)*0.004448); motor1.stop(0); motor2.stop(0); wait(1); }else if(272.5<heading<360){ - motor1.speed(-mcn1);//左回転 + motorReverse2();//左回転 motorForward(); wait((heading-270)*0.004448); motor1.stop(0); @@ -187,19 +193,13 @@ float leftrun2; rightrun=0.0; leftrun2=0.0; - - /* 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 accel[3];//accelを3つの配列で定義。*/ int tt=0; float run=0; + fprintf(fp, "x,y,z,lefttrun2,rightrun\r\n"); + while(1) { @@ -210,35 +210,8 @@ 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 + 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("speed %f diference %f\r\n",speed,difference);//速度(m)と変位を表示*/ - - printf("x=%f,y=%f,z=%f\r\n",x,y,z); + printf("x=%f,y=%f,z=%f\r\n",x,y,z); //加速度の表示 printf("%d\r\n", test.read()); //フォトインタラプタ printf("%d\r\n", test2.read()); @@ -261,29 +234,29 @@ leftrun2 += 56.5625; printf("test2.read else\r\n"); } - printf("%f", rightrun); - printf("\t%f\r\n", leftrun2); + printf("%f", leftrun2); + printf("\t%f\r\n", rightrun); run=culculate_distance_3(rightrun,leftrun2); - if (15000<run && run<15050){ //半分くらい進んだら方位を計測し直す + if (5000<run && run<5050){ //半分くらい進んだら方位を計測し直す motor1.stop(0); motor2.stop(0); wait(1); if(90<heading<267.5){ - motor1.speed(mcn1);//右回転 + motorForward2();//右回転 motorReverse(); wait((270-heading)*0.004448); //角度のずれ*1度回転するのにかかる時間 motor1.stop(0); motor2.stop(0); wait(1); }else if(0<=heading<=90.0){ - motor1.speed(-mcn1);//左回転 + motorReverse2();//左回転 motorForward(); wait((heading+90.0)*0.004448); motor1.stop(0); motor2.stop(0); wait(1); }else if(272.5<heading<360){ - motor1.speed(-mcn1);//左回転 + motorReverse2();//左回転 motorForward(); wait((heading-270)*0.004448); motor1.stop(0); @@ -294,7 +267,7 @@ } printf("searchN\r\n"); //機体が北を向く } - else if (run >= 30000){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する + else if (run >= 10000){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する break; //つまりゴールについたらこのループからぬける } @@ -303,7 +276,7 @@ break; } */ - motor1.speed(0.5); //通常走行 + motorForward2(); //通常走行 motorForward(); //Do something else here // mu.checkDistance(); //call checkDistance() as much as possible, as this is where @@ -311,14 +284,8 @@ wait(0.01); - tt++; - if(tt == 100) - { - fprintf(fp, "accel.rightrun.leftrun2\r\n"); - fprintf(fp,"(%d, %d, %d, %d,%d)\r\n", x, y, z, rightrun, leftrun2);//加速度とフォトインタラプタによる距離を出力 - tt=0; - } + fprintf(fp,"%5d, %5d, %5d, %8lf, %8lf\r\n", x, y, z, leftrun2, rightrun);//加速度とフォトインタラプタによる距離を出力 if(100 < distance && distance < 500) //障害物発見❕ { @@ -337,7 +304,7 @@ wait(2); printf("mortor stop\r\n"); - motor1.speed(msj1); //機体を時計回りに90度回転 + motorForward2(); //機体を時計回りに90度回転 motorReverse(); wait(0.5); printf("mortor rotation\r\n"); @@ -347,7 +314,7 @@ wait(2); printf("mortor stop\r\n"); - motor1.speed(ms1); //直進 + motorForward2(); //直進 motorForward(); wait(2); printf("mortor straight\r\n"); @@ -357,7 +324,7 @@ wait(2); printf("mortor stop\r\n"); - motor1.speed(-msj1); //機体を反時計回りに90度回転 + motorReverse2(); //機体を反時計回りに90度回転 motorForward(); wait(0.5); printf("mortor rotation\r\n"); @@ -367,7 +334,7 @@ wait(2); printf("mortor stop\r\n"); - motor1.speed(ms1); //直進 + motorForward2(); //直進 motorForward(); @@ -392,8 +359,8 @@ flag2 = 0; leftrun2 += 56.5625; } - printf("%f", rightrun); - printf("\t%f\r\n", leftrun2); + printf("%f", leftrun2); + printf("\t%f\r\n", rightrun); @@ -404,9 +371,8 @@ /* float X = x*0.000597964111328125; float Y = y*0.000597964111328125; float Z = z*0.000597964111328125; */ - fprintf(fp, "accel.rightrun.leftrun2\r\n"); + fprintf(fp,"%5d, %5d, %5d, %8lf, %8lf\r\n", x, y, z, leftrun2, rightrun);//加速度とフォトインタラプタによる距離を出力 - fprintf(fp,"(%d, %d, %d, %d,%d)\r\n", x, y, z, rightrun, leftrun2);//加速度とフォトインタラプタによる距離を出力 /* double a = X*X+Y*Y+Z*Z-95.982071137936; if (a>0){ a = sqrt(a); @@ -430,9 +396,9 @@ oldSpeed = speed; //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示 - // printf("speed %f diference %f\r\n",speed,difference);//速度(m)と変位を表示*/ + printf("speed %f diference %f\r\n",speed,difference);//速度(m)と変位を表示*/ - if (run >= 300000){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する + if (run >= 10000){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する break; //つまりゴールについたらこのループからぬける } @@ -449,7 +415,7 @@ wait(2); printf("mortor stop\r\n"); - motor1.speed(-msj1); //機体を反時計回りに90度回転 + motorReverse2(); //機体を反時計回りに90度回転 motorForward(); wait(0.5); printf("mortor rotation\r\n"); @@ -459,7 +425,7 @@ wait(2); printf("mortor stop\r\n"); - motor1.speed(ms1); //直進 + motorForward2(); //直進 motorForward(); wait(2); printf("mortor straight\r\n"); @@ -469,7 +435,7 @@ wait(2); printf("mortor stop\r\n"); - motor1.speed(msj1); //機体を時計回りに90度回転 + motorForward2(); //機体を時計回りに90度回転 motorReverse(); wait(0.5); printf("mortor rotation\r\n"); @@ -480,21 +446,21 @@ printf("mortor stop\r\n"); if(90<heading<267.5){ - motor1.speed(mcn1);//右回転 + motorForward2();//右回転 motorReverse(); wait((270-heading)*0.004448); //角度のずれ*1度回転するのにかかる時間 motor1.stop(0); motor2.stop(0); wait(1); }else if(0<=heading<=90.0){ - motor1.speed(-mcn1);//左回転 + motorReverse2();//左回転 motorForward(); wait((heading+90.0)*0.004448); motor1.stop(0); motor2.stop(0); wait(1); }else if(272.5<heading<360){ - motor1.speed(-mcn1);//左回転 + motorReverse2();//左回転 motorForward(); wait((heading-270)*0.004448); motor1.stop(0); @@ -511,11 +477,12 @@ motor2.stop(0); - fprintf(fp, "last accel.photo\r\n"); + fprintf(fp, "last photo\r\n"); fprintf(fp,"(%lf)\r\n", run);//最後の加速度とフォトインタラプタによる距離を出力 - fprintf(fp, "last right.left\r\n"); - fprintf(fp,"(%lf, %lf)\r\n", rightrun, leftrun2); + fprintf(fp, "last left.right\r\n"); + fprintf(fp,"(%lf, %lf)\r\n", leftrun2, rightrun); fclose(fp); + } @@ -529,19 +496,36 @@ } void motorForward() { motorStop(); - motorDir1 = 1; - motorDir2 = 0; + motor1Dir1 = 1; + motor1Dir2 = 0; } void motorReverse() { motorStop(); - motorDir1 = 0; - motorDir2 = 1; + motor1Dir1 = 0; + motor1Dir2 = 1; } void motorStop() { - motorDir1 = 0; - motorDir2 = 0; + motor1Dir1 = 0; + motor1Dir2 = 0; +} + +void motorForward2() { + motorStop(); + motor2Dir1 = 1; + motor2Dir2 = 0; +} + +void motorReverse2() { + motorStop(); + motor2Dir1 = 0; + motor2Dir2 = 1; +} + +void motorStop2() { + motor2Dir1 = 0; + motor2Dir2 = 0; }