Diff: main.cpp

Revision:

20:e78bffff80ee

Parent:

19:8ad7dcfef11f

Child:

22:1e01f4f7097c

Child:

23:2ca79873ef44

--- a/main.cpp	Sun Nov 25 04:12:06 2018 +0000
+++ b/main.cpp	Tue Nov 27 01:11:38 2018 +0000
@@ -41,14 +41,14 @@
  
 int main() {
     
-  /*   FET = 0; 
+   FET = 0; 
      while(1) {
     if(flight==1) {
         wait(10);
         }
         
     else{
-    FET = 0;                       //FET、ニクロム線切断
+        FET = 0;                       //FET、ニクロム線切断
         wait(20);
         FET = 1;
         wait(12);
@@ -59,7 +59,7 @@
         FET = 0; 
     break;
             }
-    }*/
+    }
     
     motor1.stop(0);
     motor2.stop(0);
@@ -99,12 +99,12 @@
     wait(0.5);
 }
 float mc1,mc2;
-    mc1=3.0;
-    mc2=3.0;
+    mc1=1.0;
+    mc2=1.0;
     //地磁気センサのキャリブレーション
     motor1.speed(mc1); //車体を時計回りに３秒回転
     motor2.speed(-mc2);
-    wait(2);
+    wait(3);
     
     motor1.stop(0);
     motor2.stop(0);
@@ -112,34 +112,37 @@
     
     motor1.speed(-mc1); //車体を反時計回りに３秒回転
     motor2.speed(mc2);
-    wait(2);
+    wait(3);
     
     motor1.stop(0);
     motor2.stop(0);
     wait(1);
     printf("compass carriblation\r\n"); //キャリブレーション終了
     
-float mcr1,mcr2,mcl1,mcl2;    
+float mcn1,mcn2;    
 double heading;
-    mcr1=heading*10*0.00026;
-    mcr2=heading*10*0.00026;
-    mcl1=(PI2-heading)*10*0.00026;
-    mcl2=(PI2-heading)*10*0.00026;
+    mcn1=1.0;
+    mcn2=1.0;
 compass.init();
+heading=compass.getHeadingXYDeg();
 if(2.5<heading<=M_PI){
-    motor1.speed(mcr1);
-    motor2.speed(-mcr2);
+    motor1.speed(mcn1);
+    motor2.speed(-mcn2);
+    wait(heading*0.01); //角度のずれ*1度回転するのにかかる時間
+    motor1.stop(0);
+    motor2.stop(0);
     wait(1);
 }else if(M_PI<heading<357.5){
-    motor1.speed(-mcl1);
-    motor2.speed(mcl2);
+    motor1.speed(-mcn1);
+    motor2.speed(mcn2);
+    wait((PI2-heading)*0.01);
+    motor1.stop(0);
+    motor2.stop(0);
     wait(1);
 }else{
-    wait(2);
+    wait(5);
 }
 printf("searchN\r\n"); //機体が北を向く
-
-
   
         mu.startUpdates();//start mesuring the distance(超音波センサー)
         int distance;