Diff: main.cpp

Revision:

26:ee62ea2f9362

Parent:

25:6680d67bdf24

--- a/main.cpp	Sat Dec 08 02:46:08 2018 +0000
+++ b/main.cpp	Sat Dec 08 07:27:12 2018 +0000
@@ -104,7 +104,7 @@
     //地磁気センサのキャリブレーション
     motor1.speed(mc1); //車体を時計回りに３秒回転
     motor2.speed(-mc2);
-    wait(3);
+    wait(1.6);
     
     motor1.stop(0);
     motor2.stop(0);
@@ -112,7 +112,7 @@
     
     motor1.speed(-mc1); //車体を反時計回りに３秒回転
     motor2.speed(mc2);
-    wait(3);
+    wait(1.6);
     
     motor1.stop(0);
     motor2.stop(0);
@@ -125,24 +125,24 @@
     mcn2=1.0;
 compass.init();
 heading=compass.getHeadingXYDeg();
-if(92.5<heading<=270.0){
-    motor1.speed(mcn1);
+if(90.0<heading<267.5){
+    motor1.speed(mcn1);//右回転
     motor2.speed(-mcn2);
-    wait((heading-90.0)*0.004722); //角度のずれ*1度回転するのにかかる時間
+    wait((270-heading)*0.004448); //角度のずれ*1度回転するのにかかる時間
     motor1.stop(0);
     motor2.stop(0);
     wait(1);
-}else if(270.0<heading<PI2){
-    motor1.speed(-mcn1);
+}else if(0.0<=heading<=90.0){
+    motor1.speed(-mcn1);//左回転
     motor2.speed(mcn2);
-    wait((PI2-heading+90.0)*0.004722);
+    wait((heading+90.0)*0.004448);
     motor1.stop(0);
     motor2.stop(0);
     wait(1);
-}else if(0<=heading<87.5){
-    motor1.speed(-mcn1);
+}else if(272.5<heading<=360){
+    motor1.speed(-mcn1);//左回転
     motor2.speed(mcn2);
-    wait((90.0-heading)*0.004722);
+    wait((heading-270)*0.004448);
     motor1.stop(0);
     motor2.stop(0);
     wait(1);
@@ -151,6 +151,7 @@
 }
 printf("searchN\r\n"); //機体が北を向く
   
+  
         mu.startUpdates();//start mesuring the distance(超音波センサー)
         int distance; 
         
@@ -268,7 +269,7 @@
 
             motor1.speed(msj1);       //機体を時計回りに９０度回転
             motor2.speed(-msj2);
-            wait(0.77);
+            wait(0.4);
             printf("mortor rotation\r\n");
             
             motor1.stop(0);
@@ -288,7 +289,7 @@
 
             motor1.speed(-msj1);      //機体を反時計回りに９０度回転
             motor2.speed(msj2);
-            wait(0.77);
+            wait(0.4);
             printf("mortor rotation\r\n");
             
             motor1.stop(0);
@@ -372,7 +373,7 @@
 
             motor1.speed(-msj1);      //機体を反時計回りに９０度回転
             motor2.speed(msj2);
-            wait(1);
+            wait(0.4);
             printf("mortor rotation\r\n");
             
             motor1.stop(0);
@@ -387,7 +388,7 @@
             
             motor1.stop(0);
             motor2.stop(0);
-            wait(2);
+            wait(0.4);
             printf("mortor stop\r\n");
 
             motor1.speed(msj1);       //機体を時計回りに９０度回転
@@ -400,6 +401,31 @@
             wait(2);
             printf("mortor stop\r\n");
             
+if(90.0<heading<267.5){
+    motor1.speed(mcn1);//右回転
+    motor2.speed(-mcn2);
+    wait((270-heading)*0.004448); //角度のずれ*1度回転するのにかかる時間
+    motor1.stop(0);
+    motor2.stop(0);
+    wait(1);
+}else if(0.0<=heading<=90.0){
+    motor1.speed(-mcn1);//左回転
+    motor2.speed(mcn2);
+    wait((heading+90.0)*0.004448);
+    motor1.stop(0);
+    motor2.stop(0);
+    wait(1);
+}else if(272.5<heading<=360){
+    motor1.speed(-mcn1);//左回転
+    motor2.speed(mcn2);
+    wait((heading-270)*0.004448);
+    motor1.stop(0);
+    motor2.stop(0);
+    wait(1);
+}else{
+    wait(5);
+}
+printf("searchN\r\n"); //機体が北を向く
             
         }
    }