Diff: main.cpp

Revision:

13:63746a793db9

Parent:

12:eb4befc8d2c4

Child:

14:228a35fcb323

--- a/main.cpp	Sat Dec 14 10:01:17 2019 +0000
+++ b/main.cpp	Sat Dec 14 13:15:57 2019 +0000
@@ -16,6 +16,7 @@
 
 
 GPS gps (p28,p27);                 //GPS
+Timer time_gps;
 
 DigitalOut FET(p21);                //FET
 DigitalOut myled(LED1);
@@ -70,39 +71,31 @@
     
     
       //以下GPS
-     double a;
-     double b;
+     double a=0;
+     double b=0;
      double distance;
+     double timeout_flag=0;
      int k = 0;
      int j = 0;
      
      pc.printf("GPS Start\n");
      xbee.printf("s\n");//printf("1");
-     while(1){
-         /*printf("2");*/int received_data = xbee.getc();
-         if(gps.getgps()||received_data == 97){
+     time_gps.start();
+     while(a==0)
+        {
+            if(gps.getgps()){
+                a=gps.latitude;
+                b=gps.longitude;
+                }
            
-          pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示   
-          k++;
-           a = gps.latitude;
-           b = gps.longitude;
-          if(k<10){
-           printf("%d",k); if(a!=0)
-                         xbee.printf("dont worry! GPSは動いてるよ！\n");
-           }else{
-                 /* a = gps.latitude;
-                  b = gps.longitude;*/
-                 /* if(a!=0)
-                         xbee.printf("dont worry! GPSは動いてるよ！\n");*/
-                  printf("(a =%lf,b =%lf)\r\n",gps.latitude,gps.longitude);//a,bを表示  
-                  break;
-           }
-         }else{
-          pc.printf("NO DATA\r\n");//データ取得失敗
-          wait(1);
+            pc.printf("%lf,%lf\n",a,b);
+            if(time_gps.read()>30){
+                xbee.printf("timeout!!\n");
+                timeout_flag=1;
+                break;
             }
-       }
-       printf("moter on");
+        }
+       printf("moter on%lf",a);
         left1 = 100; //左モーター１００％
         right1 = 100;//右モーター１００％
         
@@ -112,38 +105,39 @@
         right1 = 0;//右モーター０％
         printf("moter off");
         wait(15);
-        printf("GPS restart\n");
         
-        while(1) {
+        while(!timeout_flag) {
+         printf("GPS restart\n");
          if(gps.getgps()){
            
           pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示   
           j ++;
-          if(j<29){
+          if(j<10){
            }else{
             // 球面三角法により、大円距離(メートル)を求める
-           double c;
-           double d; 
-           c = gps.latitude;
-           d = gps.longitude;
+           double lat2;
+           double lon2; 
+           lat2 = gps.latitude;
+           lon2 = gps.longitude;
 
             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 rc = lat2 * pi / 180;
+            double rd = lon2 * pi / 180;
         
-            double e = sin(ra) * sin(rc) +  cos(ra) * cos(rc) * cos(rb - rd);  // 2点の中心角(ラジアン)を求める
+            double e = sin(ra) * sin(rc) +  cos(ra) * cos(rc) * cos(rd - rb);  // 2点の中心角(ラジアン)を求める
             double rr = acos(e);
 
             const double earth_radius = 6378140;   // 地球赤道半径(m)
 
             distance = earth_radius * rr; // 2点間の距離(m)
-            
+            printf("distance=%f\ne=%lf\nrr=%lf\n",distance,e,rr);
             if (distance<5){
              }else{
+                 pc.printf("(a =%lf,b =%lf)\n\r",a,b);
              pc.printf("(c =%lf,d =%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示   
              pc.printf("5m clear!");
              xbee.printf("5m clear!");