Diff: main.cpp

Revision:

9:ec770f68440e

Parent:

8:765a73e21907

--- a/main.cpp	Fri Dec 06 08:35:28 2019 +0000
+++ b/main.cpp	Sun Dec 08 13:38:35 2019 +0000
@@ -55,74 +55,79 @@
      double a;
      double b;
      double distance;
-    
+     int k = 0;
+     int j = 0;
+     
     pc.printf("GPS Start\r\n");
-    
-     while(1)
-     {
-         if(gps.getgps())
-         {
+    xbee.printf("s\n");
+      if(gps.getgps()){
+           
+          pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示   
+          i ++;
+          if(k<59){
+           }else{
            a = gps.latitude;
            b = gps.longitude;
-          pc.printf("位置情報取得成功！\r\n"); 
-          pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示
-          
+           printf("(a =%lf,b =%lf)\r\n",gps.latitude,gps.longitude);//a,bを表示   
            break;
-           
+           }
          }else{
-               pc.printf("NO DATA\r\n");//データ取得失敗
-               wait(1);
-               }
-      }
-
-      while(1)
-      { printf("移動距離測定開始\r\n");
-        left1=100;
-        right1=100;
-        wait(5);
-        left1=0;
-        right1=0;
-        wait(3);
-         if(gps.getgps()) {
+          pc.printf("NO DATA\r\n");//データ取得失敗
+          wait(1);
+            }
+       }
+       printf("moter on");
+        left = 100; //左モーター１００％
+        right = 100;//右モーター１００％
+        
+        wait(30);
+        printf("moter off");
+        left = 0; //左モーター１０％
+        right = 0;//右モーター１０％(左折)
+        printf("moter off");
+        wait(15);
+        printf("GPS restart\n");
+        
+        while(1) {
+         if(gps.getgps()){
            
-          pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示   
-          
-          // 球面三角法により、大円距離(メートル)を求める
-          double c;
-          double d; 
-          c = gps.latitude;
-          d = gps.longitude;
+          pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示   
+          j ++;
+          if(j<29){
+           }else{
+            // 球面三角法により、大円距離(メートル)を求める
+           double c;
+           double d; 
+           c = gps.latitude;
+           d = gps.longitude;
 
-          const double pi = 3.14159265359; // 円周率
+            pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示   
+            const double pi = 3.14159265359; // 円周率
                            
-          double ra = a * pi / 180;
-          double rb = b * pi / 180;     // 緯度経度をラジアンに変換
-          double rc = c * pi / 180;
-          double rd = d * pi / 180;
+            double ra = a * pi / 180;
+            double rb = b * pi / 180;     // 緯度経度をラジアンに変換
+            double rc = c * pi / 180;
+            double rd = d * pi / 180;
         
-          double e = sin(ra) * sin(rc) +  cos(ra) * cos(rc) * cos(rb - rd);  // 2点の中心角(ラジアン)を求める
-          double rr = acos(e);
+            double e = sin(ra) * sin(rc) +  cos(ra) * cos(rc) * cos(rb - rd);  // 2点の中心角(ラジアン)を求める
+            double rr = acos(e);
 
-          const double earth_radius = 6378140;   // 地球赤道半径(m)
+            const double earth_radius = 6378140;   // 地球赤道半径(m)
 
-          distance = earth_radius * rr; // 2点間の距離(m)
+            distance = earth_radius * rr; // 2点間の距離(m)
             
-           
-             
-
-         if (distance<5){
-             printf("走行距離=%lf\r\n",distance);   
+            if (distance<5){
              }else{
+             pc.printf("(c =%lf,d =%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示   
              pc.printf("5m clear!");
-             break;
-                  }
-          
-             }else{
-           pc.printf("NO DATA\r\n");//データ取得失敗
-           wait(1);
-           }
-        }
-        //GPS End     
+             xbee.printf("5m clear!");
+              break;
+           }  
+            }
+       }else{
+         printf("No data");
+         }
+       } //GPS End     
 
  int i=1;
   float th;