Diff: main.cpp

Revision:

7:1c1b782263cf

Parent:

4:8b52fd631b32

--- a/main.cpp	Sat Oct 20 03:10:04 2018 +0000
+++ b/main.cpp	Mon Oct 22 01:28:54 2018 +0000
@@ -63,7 +63,42 @@
     }    
     fprintf(fp,"GPS finish\r\n");
     fclose(fp);                      //GPSの測定終了  
-  
+    
+float mc1,mc2; //地磁気センサのキャリブレーション
+    mc1=3.0;
+    mc2=3.0;
+    
+    motor1.speed(mc1); //車体を時計回りに3秒回転
+    motor2.speed(-mc2);
+    wait(2);
+    
+    motor1.stop(0);
+    motor2.stop(0);
+    wait(1);
+    
+    motor1.speed(-mc1); //車体を反時計回りに３秒回転
+    motor2.speed(mc2);
+    wait(2);
+    
+    motor1.stop(0);
+    motor2.stop(0);
+    wait(1);
+    printf("compass carriblation\r\n"); //キャリブレーション終了
+    
+    if(M_PI<raw<357.5){
+        motor1.speed(mc1);
+        motor2.speed(-mc2);
+        wait(1);
+        }else if(357.5<=raw<PI2 && raw<=2.5){
+            motor1.speed(-mc1);
+            motor2.speed(mc2);
+            wait(1);
+            }else
+            motor1.stop(0);
+            motor2.stop(0);
+            wait(2);
+            printf("search north\r\n"); //機体が北を向く
+    
         mu.startUpdates();//start mesuring the distance(超音波センサー)
         int distance;