File content as of revision 18:42cbba4fb371:

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

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

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

int main() {
    // 変数宣言
    /*int CP_num;           // CPリストのインデックス
    int last_num;         // CPリストの最後の要素のインデックス*/
    double GPS_x, GPS_y;  // 現在地の座標
    double direction;     // 次CPへの向き
    double CPs_x[3]={34.545658, 34.545837, 34.546052};  //CPリスト(x座標)
    double CPs_y[3]={135.503469, 135.503676, 135.503503};    // CPリスト(y座標)
    double next_CP_x, next_CP_y;
    
    // 落下検知
    // パラシュート分離
    
    wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん
    while(flight_pin){
        pc.printf("flight_pin nuketa");
        wait(35);//ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s
        nichrome=1;
        pc.printf("nichrome in");
        wait(30);
        nichrome=0;
    }
    // 落下終了
    
    
    // 行動フロー開始
    //last_num = sizeof(CPs_x) / sizeof(double) - 1;  // ゴール地点のインデックスを算出
    while (next_CP_x != CPs_x[cp_max-1] && next_CP_y != CPs_y[cp_max-1]) { // ゴール判定
        for (int i = 0; i<=cp_max-1 ; i++) {
            // 移動
        next_CP_x = CPs_x[i];
        next_CP_y = CPs_y[i];
        
        pc.printf("i=%d\r\n", i);
        catchGPS();
        pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);
            
            while (1) {
                speak();
                
                direction = AngleGet();
                pc.printf("direction=%f\n", direction);
                while(1) {
                    if(direction < 5 || direction > 355) {  //角度判定
                        Move('2', 1);
                        break;
                    }
                    else {
                        Move('1', 0);
                        Move('4', 0.5);
                    }
                }
                
                if (FrontGet()) {
                    Move('1', 0);      //停止
                    Move('4', 0.5);    //回転
                    wait(1);
                    Move('1', 0);      //回転停止
                    continue;
                } 
                else {
                    Move('2', 0.5);
                }
                catchGPS();
                pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);
                if ((next_CP_x - GPS_x)*(next_CP_x - GPS_x) + (next_CP_y = GPS_y)*(next_CP_y = GPS_y) < 0.01) { // CP到着判定 //試験で調整
                    pc.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y);
                    break;
                }
                
            }
        }
    // 行動フロー終了
    pc.printf("End\r\n");
    Move('1', 0);      //停止
    return 0;
    }
}