File content as of revision 0:0454d5b5905b:

#include "mbed.h"
#include "Function.h"

int main() 
{
    //直進
    Move('2', 1);     // 前進
    wait(10);
    Move('1', 0);     // 停止
    
    //回転
    Move('4', 0.5);   // 時計回りに回転
    wait(10);
    Move('1', 0);     // 停止
    
    //FrontGetの距離調整
    while(1) {
        if (FrontGet()) {
            Move('1', 0);      //停止
            Move('4', 0.5);    //回転
            wait(1);
            Move('1', 0);      //停止
        } 
        else {
        Move('2', 1);
        }
    }
    
/* main    
    double GPS_x, GPS_y;  // 現在地の座標
    double direction;     // 次CPへの向き
    double CPs_x[3]={34.54608391847546, 34.545845666047306, 34.545666059919796};  //CPリスト(x座標)
    double CPs_y[3]={135.50338843400897, 135.50368659080985, 135.50347298593758};    // CPリスト(y座標)
    double next_CP_x, next_CP_y;
    int cp_max = sizeof(CPs_x);
    
    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);
            pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);
            
                while (1) {
                // 角度調整
                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);
                
                double lati = 111132.8715;    //1度あたりの緯度の距離(m)
                double longi = 91535.79099;    //1度あたりの経度の距離(m)
                    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 < 100) { // 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;
    }
*/
}