main.cpp

00001 #include "mbed.h"
00002 #include "Function.h"   // 自作関数
00003 #define cp_max 2        //CPの数を入力する
00004 
00005 
00006 int main() {
00007     // 変数宣言
00008     double GPS_x, GPS_y;  // 現在地の座標
00009 //    double direction;     // 次CPへの向き
00010     double CPs_x[cp_max]={34.545588426585454, 34.5454484832847};  //CPリスト(x座標)
00011     double CPs_y[cp_max]={135.50282053551706, 135.50262437335775};    // CPリスト(y座標)
00012     double next_CP_x, next_CP_y;
00013     
00014     
00015     catchGPS();
00016     xbee.printf("XBee Connected\r\n");
00017     xbee.printf("Fall point(lati,long)=(%lf , %lf)\r\n", gps.latitude, gps.longitude);
00018     for (int i = 0; i<=cp_max-1; i++) {//最後のcp=goalまで移動
00019         next_CP_x = CPs_x[i];
00020         next_CP_y = CPs_y[i];
00021         xbee.printf("next_i=%d\r\n", i);
00022             while(1) {
00023                 Move('2', 0.3);
00024     /*    
00025                 while(FrontGet()) {
00026                 Move('1', 0);      //停止
00027                 Move('4', 0.3);    //時計回り回転
00028                 wait(0.5);
00029                 Move('1', 0);      //回転停止
00030     */
00031                 catchGPS();
00032                 double lati = 111132.8715;    //1度あたりの緯度の距離(m)
00033                 double longi = 91535.79099;    //1度あたりの経度の距離(m)
00034                 GPS_x = gps.latitude;
00035                 GPS_y = gps.longitude;
00036                     if ((next_CP_x - GPS_x)*(next_CP_x - GPS_x)*lati*lati + (next_CP_y - GPS_y)*(next_CP_y - GPS_y)*longi*longi < 10) { // CP到着判定 //試験で調整
00037                         xbee.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y);
00038                         break;
00039                     }
00040             }
00041         }
00042     xbee.printf("\n\rEnd\r\n");
00043     Move('1', 0);      //停止
00044     return 0;
00045 }