File content as of revision 18:bd0b2394fa48:

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

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

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

int main() {
    // 変数宣言
    double GPS_x, GPS_y;  // 現在地の座標
    double direction;     // 次CPへの向き
    double CPs_x[5]={34.54556786858123, 34.545733563643864, 34.54594565284262, 34.54614448597558, 34.54632012151458};  //CPリスト(x座標)
    double CPs_y[5]={135.51009581235357, 135.51000193504115, 135.5098705068037, 135.50974444298416, 135.50962106137328};    // CPリスト(y座標)
    double next_CP_x, next_CP_y;
    //B6棟裏セット 34.545588426585454, 34.5454484832847 | 135.50282053551706, 135.50262437335775
    //グラウンド道路近くの直線 34.54556786858123, 34.545733563643864, 34.54594565284262, 34.54614448597558, 34.54632012151458
    //                     135.51009581235357, 135.51000193504115, 135.5098705068037, 135.50974444298416, 135.50962106137328
    
    //落下開始
    while(flight_pin){}
        xbee.printf("flight_pin nuketa\n");     // 落下検知
        wait(5);//!ホントは35!           //ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s
        //nichrome=1;       // パラシュート分離
        xbee.printf("nichrome in\n");
        wait(10);
        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();
                
            //回避開始
            while(FrontGet()) {
                xbee.printf("frontget\r\n");
                Move('1', 0);      //停止
                Move('4', 0.18);    //時計回り回転
                wait(0.5);
                Move('1', 0);      //回転停止
                xbee.printf("front_avoid_rotate\r\n");
            }
                //障害物よけて走ってから目的地に回頭、走らないと障害物に向くかも
                Move('2', 0.1);
                wait(1);
                Move('2', 0.39);
                wait(2);
                Move('1', 0);       //走りながらanglegetできたら、止まらない
                //ちょっと走るのおわり
            //回避終了
                
            //角度調節開始
                direction = AngleGet();
                xbee.printf("direction=%f\r\nrotation_start", direction);
                while(1) {
                    if(direction < 20 || direction > 340) {
                        xbee.printf("rotation finish\r\n");
                        Move('1', 0);   //停止
                        Move('2', 0.39);
                        xbee.printf("now_angle=%f\r\n", direction);
                        break;
                        }
                    else {
                        Move('4', 0.18);//時計回りに回転
                        xbee.printf("now_angle=%lf\r\n", direction);
                        direction = AngleGet();
                        }
                }   
            //角度調節終了
            
            //速度変更開始     
            xbee.printf("speed flag=");
            wait(3);
            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",as[1]);
            catchGPS();
            xbee.printf("now point(lati, long)=%lf , %lf\r\n", gps.latitude, gps.longitude);
            //速度変更終了
                
            //座標判定
            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("End\r\n");
    Move('1', 0);
    return 0;
}