(多分)全部+フライトピン+新しい加速度
Dependencies: mbed
Diff: main.cpp
- Revision:
- 30:3bcf14e8dd63
- Parent:
- 29:05b390de92ed
- Child:
- 31:1ee80995740a
--- a/main.cpp Wed Dec 12 07:48:58 2018 +0000 +++ b/main.cpp Wed Dec 12 08:29:05 2018 +0000 @@ -120,7 +120,7 @@ //地磁気センサのキャリブレーション motor1.speed(mc1); //車体を時計回りに3秒回転 motorReverse(); - wait(3); + wait(1.6); motor1.stop(0); motor2.stop(0); @@ -128,7 +128,7 @@ motor1.speed(-mc1); //車体を反時計回りに3秒回 motorForward(); - wait(3); + wait(1.6); motor1.stop(0); motor2.stop(0); @@ -141,17 +141,24 @@ mcn2=1.0; compass.init(); heading=compass.getHeadingXYDeg(); -if(2.5<heading<=M_PI){ - motor1.speed(mcn1); +if(90<heading<267.5){ + motor1.speed(mcn1);//右回転 motorReverse(); - wait(heading*0.01); //角度のずれ*1度回転するのにかかる時間 + wait((270-heading)*0.004448); //角度のずれ*1度回転するのにかかる時間 motor1.stop(0); motor2.stop(0); wait(1); -}else if(M_PI<heading<357.5){ - motor1.speed(-mcn1); +}else if(0<=heading<=90.0){ + motor1.speed(-mcn1);//左回転 motorForward(); - wait((PI2-heading)*0.01); + wait((heading+90.0)*0.004448); + motor1.stop(0); + motor2.stop(0); + wait(1); +}else if(272.5<heading<360){ + motor1.speed(-mcn1);//左回転 + motorForward(); + wait((heading-270)*0.004448); motor1.stop(0); motor2.stop(0); wait(1); @@ -245,7 +252,34 @@ printf("%f", rightrun); printf("\t%f\r\n", leftrun2); run=culculate_distance_3(rightrun,leftrun2); - if (run >= 4396){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する + if (2000<run<2100){ //半分くらい進んだら方位を計測し直す + if(90<heading<267.5){ + motor1.speed(mcn1);//右回転 + motorReverse(); + wait((270-heading)*0.004448); //角度のずれ*1度回転するのにかかる時間 + motor1.stop(0); + motor2.stop(0); + wait(1); +}else if(0<=heading<=90.0){ + motor1.speed(-mcn1);//左回転 + motorForward(); + wait((heading+90.0)*0.004448); + motor1.stop(0); + motor2.stop(0); + wait(1); +}else if(272.5<heading<360){ + motor1.speed(-mcn1);//左回転 + motorForward(); + wait((heading-270)*0.004448); + motor1.stop(0); + motor2.stop(0); + wait(1); +}else{ + wait(5); +} +printf("searchN\r\n"); //機体が北を向く +} + else if (run >= 4396){ //もし総距離が250以上ならば、というのもここの値は暫定値。とりあえずゴール地点が決まればまたその値に修正する break; //つまりゴールについたらこのループからぬける } @@ -290,7 +324,7 @@ motor1.speed(msj1); //機体を時計回りに90度回転 motorReverse(); - wait(0.57); + wait(0.4); printf("mortor rotation\r\n"); motor1.stop(0); @@ -310,7 +344,7 @@ motor1.speed(-msj1); //機体を反時計回りに90度回転 motorForward(); - wait(0.57); + wait(0.4); printf("mortor rotation\r\n"); motor1.stop(0); @@ -427,6 +461,31 @@ wait(2); printf("mortor stop\r\n"); +if(90<heading<267.5){ + motor1.speed(mcn1);//右回転 + motorReverse(); + wait((270-heading)*0.004448); //角度のずれ*1度回転するのにかかる時間 + motor1.stop(0); + motor2.stop(0); + wait(1); +}else if(0<=heading<=90.0){ + motor1.speed(-mcn1);//左回転 + motorForward(); + wait((heading+90.0)*0.004448); + motor1.stop(0); + motor2.stop(0); + wait(1); +}else if(272.5<heading<360){ + motor1.speed(-mcn1);//左回転 + motorForward(); + wait((heading-270)*0.004448); + motor1.stop(0); + motor2.stop(0); + wait(1); +}else{ + wait(5); +} +printf("searchN\r\n"); //機体が北を向く } }