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); //車体を時計回りに３秒回転
     motorReverse();
-    wait(3);
+    wait(1.6);
     
     motor1.stop(0);
     motor2.stop(0);
@@ -128,7 +128,7 @@
     
     motor1.speed(-mc1); //車体を反時計回りに３秒回
     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);       //機体を時計回りに９０度回転
             motorReverse();
-            wait(0.57);
+            wait(0.4);
             printf("mortor rotation\r\n");
             
             motor1.stop(0);
@@ -310,7 +344,7 @@
 
             motor1.speed(-msj1);      //機体を反時計回りに９０度回転
             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"); //機体が北を向く
             
         }
    }