gps.cpp

00001 #include "mbed.h"
00002 #include "getGPS.h"
00003 #include "math.h"
00004 #include "TB6612.h"
00005 
00006 Serial pc(USBTX,USBRX);
00007 GPS gps (p28,p27);
00008 Serial xbee(p13,p14);
00009 TB6612 left(p25,p17,p16);
00010 TB6612 right(p26,p19,p18);
00011 
00012 
00013 int main() {
00014      double a;
00015      double b;
00016      double distance;
00017      int i = 0;
00018      
00019      pc.printf("GPS Start\n");
00020      xbee.printf("s\n");
00021      while(1) {
00022          if(gps.getgps()){
00023            
00024           pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示   
00025           i ++;
00026           if(i<29){
00027            }else if(i == 30){
00028            a = gps.latitude;
00029            b = gps.longitude;
00030            
00031            }else{
00032            break;
00033            }
00034          }else{
00035           pc.printf("NO DATA\r\n");//データ取得失敗
00036           wait(1);
00037             }
00038        }
00039        
00040         left = 100; //左モーター１００％
00041         right = 100;//右モーター１００％
00042         wait(30);
00043         left = 0; //左モーター１０％
00044         right = 0;//右モーター１０％(左折)
00045         wait(15);
00046         
00047       while(1){
00048          if(gps.getgps()) {
00049            
00050           pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示   
00051           
00052        // 球面三角法により、大円距離(メートル)を求める
00053        double c;
00054        double d; 
00055         c = gps.latitude;
00056         d = gps.longitude;
00057 
00058          const double pi = 3.14159265359; // 円周率
00059                            
00060             double ra = a * pi / 180;
00061             double rb = b * pi / 180;     // 緯度経度をラジアンに変換
00062             double rc = c * pi / 180;
00063             double rd = d * pi / 180;
00064         
00065             double e = sin(ra) * sin(rc) +  cos(ra) * cos(rc) * cos(rb - rd);  // 2点の中心角(ラジアン)を求める
00066             double rr = acos(e);
00067 
00068             const double earth_radius = 6378140;   // 地球赤道半径(m)
00069 
00070             distance = earth_radius * rr; // 2点間の距離(m)
00071              
00072 
00073          if (distance<5){
00074              }else{
00075              pc.printf("5m clear!");
00076              xbee.printf("5m clear!");
00077              break;
00078               }
00079           
00080           }else{
00081            xbee.printf("NO DATA\r\n");//データ取得失 
00082            pc.printf("NO DATA\r\n");  
00083            wait(1);
00084            }
00085         }
00086         return 0;
00087      }
00088      
00089      
00090  
00091 
00092  
00093 
00094 
00095 
00096 
00097 
00098      
00099      
00100      
00101     
00102