File content as of revision 1:db6b55732559:

#include "mbed.h"
#include "Function.h"   // 自作関数
#define cp_max 2        //CPの数を入力する


int main() {
    // 変数宣言
    double GPS_x, GPS_y;  // 現在地の座標
//    double direction;     // 次CPへの向き
    double CPs_x[cp_max]={34.545588426585454, 34.5454484832847};  //CPリスト(x座標)
    double CPs_y[cp_max]={135.50282053551706, 135.50262437335775};    // CPリスト(y座標)
    double next_CP_x, next_CP_y;
    
    
    catchGPS();
    xbee.printf("XBee Connected\r\n");
    xbee.printf("Fall point(lati,long)=(%lf , %lf)\r\n", gps.latitude, gps.longitude);
    for (int i = 0; i<=cp_max-1; i++) {//最後のcp=goalまで移動
        next_CP_x = CPs_x[i];
        next_CP_y = CPs_y[i];
        xbee.printf("next_i=%d\r\n", i);
            while(1) {
                Move('2', 0.3);
    /*    
                while(FrontGet()) {
                Move('1', 0);      //停止
                Move('4', 0.3);    //時計回り回転
                wait(0.5);
                Move('1', 0);      //回転停止
    */
                catchGPS();
                double lati = 111132.8715;    //1度あたりの緯度の距離(m)
                double longi = 91535.79099;    //1度あたりの経度の距離(m)
                GPS_x = gps.latitude;
                GPS_y = gps.longitude;
                    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到着判定 //試験で調整
                        xbee.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y);
                        break;
                    }
            }
        }
    xbee.printf("\n\rEnd\r\n");
    Move('1', 0);      //停止
    return 0;
}