File content as of revision 3:cad76afeff0f:

/*ライブラリ*/
#include "mbed.h"

// 自作関数
#include "Function.h"

// フライトピン・ニクロム線関係
DigitalIn flight_pin(A0);
DigitalOut nichrome(D13);
// 
#define cp_max 3    //CPの数を入力する

int main() {
    // 変数宣言
    double GPS_x, GPS_y;  // 現在地の座標
    double direction;     // 次CPへの向き
    double CPs_x[3]={1,2,3};  //CPリスト(x座標)
    double CPs_y[3]={1,2,3};    // CPリスト(y座標)
    double next_CP_x, next_CP_y;
    
    // 落下検知
    // パラシュート分離
    
    wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん
    while(flight_pin){}
        xbee.printf("flight_pin nuketa\n\r");
        wait(3);//ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s
        nichrome=1;
        xbee.printf("nichrome in\n\r");
        wait(3);
        nichrome=0;
    // 落下終了
    
    
    // 行動フロー開始
    Calibration();
    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) {
                speak();
                direction = AngleGet();
                xbee.printf("\n\rdirection=%f\n\rdirection start", direction);
                int df=1;
                //角度調節
                while(1) {
                    if(direction < 5 || direction > 355) {  //角度判定
                        xbee.printf("direction finish\n\r");
                        break;
                    }
                    else {
                        Move('1', 0);//停止
                        if(df==1){
                        xbee.printf("mortor mode:1 speed:0\n\r");
                        }
                        Move('4', 0.5);//時計回りに回転
                        if(df==1){
                        xbee.printf("mortor mode:4 speed:0.5\n\r");
                        df++;
                        direction = AngleGet();
                        break;
                        }
                    }
                }
                while(FrontGet()) {
                    xbee.printf("front get\n\r");
                    Move('1', 0);      //停止
                    xbee.printf("mortor mode:1 speed:0\n\r");
                    Move('4', 0.5);    //時計回り回転
                    xbee.printf("mortor mode:4 speed:0.5\n\r");
                    wait(1);
                    Move('1', 0);      //回転停止
                    xbee.printf("mortor mode:1 speed:0\n\r");
                } 
                xbee.printf("speed flag=");
                wait(5);
                float as[2];//advance speed
                if(xbee.readable()){
                    xbee.printf("advance speed=");
                    xbee.scanf("%f",&as[1]);
                    }else{
                        as[1]=0.5;
                        }
                Move('2', as[1]);
                xbee.printf("mortor mode:2 speed:%f\n\r",as[1]);
                catchGPS();
                xbee.printf("GPS_x=");
                xbee.scanf("%lf",&GPS_x);
                xbee.printf("\n\rGPS_y=");
                xbee.scanf("%lf",&GPS_y);
                //xbee.printf("now point(lati, long)=%lf , %lf\r\n", gps.latitude, gps.longitude);
                xbee.printf("\n\rX=%d\n\rY=%d\n\r",GPS_x,GPS_y);
                
                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 < 25) { // CP到着判定 //試験で調整
                    xbee.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y);
                    break;
                }
                
            }//while(1){}
        }//for(){}
    // 行動フロー終了
    xbee.printf("\n\rEnd\r\n");
    Move('1', 0);      //停止
    xbee.printf("mortor mode:1 speed:0");
    return 0;
}