Diff: main.cpp

Revision:

10:280a25bcc8bb

Parent:

9:fb19a93df88f

Child:

11:80284e989390

--- a/main.cpp	Fri Oct 26 07:04:55 2018 +0000
+++ b/main.cpp	Mon Nov 05 12:46:14 2018 +0000
@@ -10,7 +10,7 @@
 GPS gps(p28, p27);                  //GPS
 HMC5883L compass(p9, p10);          //地磁気センサー   
 
-MPU6050 mpu(p9,p10);                //(SDA,SCL)のピンをアサインしてね☆   加速度センサー
+MPU6050 mpu(p9, p10);                //(SDA,SCL)のピンをアサインしてね☆   加速度センサー
 
 
 DigitalOut FET(p21);                //FET
@@ -75,14 +75,56 @@
   
     
      
-     compass.init();                  //地磁気センサー動作
+compass.init();                  //地磁気センサー動作
       
-    int i;
-    for(i=0;i<20;i++)                 //地磁気測定
-    {
-        pc.printf("raw=%f\r\n",compass.getHeadingXYDeg()); //度数法で表記
-        wait(0.5);
-    }
+int i;
+for(i=0;i<20;i++)                 //地磁気測定
+{
+    pc.printf("raw=%f\r\n",compass.getHeadingXYDeg()); //度数法で表記
+    wait(0.5);
+}
+float mc1,mc2;
+    mc1=3.0;
+    mc2=3.0;
+    //地磁気センサのキャリブレーション
+    motor1.speed(mc1); //車体を時計回りに３秒回転
+    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"); //キャリブレーション終了
+    
+float mcr1,mcr2,mcl1,mcl2;    
+double heading;
+    mcr1=heading*10*0.00026;
+    mcr2=heading*10*0.00026;
+    mcl1=(PI2-heading)*10*0.00026;
+    mcl2=(PI2-heading)*10*0.00026;
+compass.init();
+if(2.5<heading<=M_PI){
+    motor1.speed(mcr1);
+    motor2.speed(-mcr2);
+    wait(1);
+}else if(M_PI<heading<357.5){
+    motor1.speed(-mcl1);
+    motor2.speed(mcl2);
+    wait(1);
+}else{
+    wait(2);
+}
+printf("searchN\r\n"); //機体が北を向く
+
+
   
         mu.startUpdates();//start mesuring the distance(超音波センサー)
         int distance; 
@@ -250,6 +292,8 @@
             motor2.stop(0);
             wait(2);
             printf("mortor stop\r\n");
+            
+            
         }
    }