File content as of revision 5:4e79ad68f3e3:

#include "mbed.h"
#include "SDFileSystem.h"
#include "math.h"

Ticker timer;
SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
Serial gps(p13, p14);       // tx, rx
Serial pc(USBTX, USBRX);    // tx, rx 
PwmOut moterl(p21);//左モーター
PwmOut moterr(p22);//右モーター
DigitalOut led1(LED1);
DigitalOut fet3(p23);//ニクロム線
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
DigitalOut janpa1(p19);//パラシュートの開きを検知
DigitalIn janpa2(p20);//パラシュートの開きを検知 

float palse=0.02;
char gps_data[256];
float longitude,latitude,gpstime,knot,angle;
//緯度longitude　経度latitude,
int i=0;
char *gps_target = "$GPRMC,230412.000,A,3022.5297,N,13057.6164,E,0.00,168.46,030316,,,A*6D";
float high_target = 7.0;
float longitude_target,latitude_target,gpstime_target,knot_target,angle_target,high;
int count=0;
int gomidata1,gomidata2,gomidata3,gomidata4,gomidata5,gomidata6;
float longitudegosa;
float latitudegosa;
int angle_targetmath = 0;

void gps_rx()//GPSデータ受信割り込み
{
    pc.printf("gps_rx");
    gps_data[i] = gps.getc();
    //gps_data[i] = pc.getc();
    if( gps_data[i] == '$' )
    {
        //$から受信データを保持する
        gps_data[0] = '$';
        i = 1;
    }
    else if( gps_data[i-1] == '\r' && gps_data[i] == '\n' )
    {
        //改行コードまでのデータを解析する
        gps_data[i+1] = '\0';
        if(memcmp(gps_data, "$GPRMC",6) == 0)
        {
            // $GPRMCで始ってればデータを分けて格納
            sscanf(gps_data,"$GPRMC,%f,A,%f,N,%f,E,%f,%f",&gpstime,&longitude,&latitude,&knot,&angle);
            pc.printf("GPRMC %f %f %f %f %f \n",gpstime,longitude,latitude,knot,angle);
        }
        if(memcmp(gps_data, "$GPGGA",6) == 0)
        {
            // $GPGGAで始ってればデータを分けて格納
            sscanf(gps_data,"$GPGGA,%d,%d,N,%d,E,%d,%d,%d,%f,M",&gomidata1,&gomidata2,&gomidata3,&gomidata4,&gomidata5,&gomidata6,&high);
            pc.printf("high %f",high);
        }
        //データをSDに書き込み
        FILE *fp = fopen("/sd/gpsdata.txt", "a");
        if(fp == NULL)
        {
            error("Could not open file for write\n");
        }
        fprintf(fp,"%s \n",gps_data);
        fclose(fp);
        //free(fp);
        
        //PCにデータ送信
        pc.printf("%s \n",gps_data);
        i = 0;
    }
    else
    {
        //改行コードが来るまでカウントを続ける
        i++;
        if(i==255)
        {
            printf("*** Error! ***\n");
        }
    }

}

void move()
{
    //動作
    switch (count)
    {
        case 0:
            //パラシュートを焼き切るまで待つ
            //焼き切ったらcount+1
            //緯度longitude　経度latitude,
            //pc.printf("case0");
            if( (high_target+1.0)>= high )
            {
                pc.printf("hight_START");
                //左右のモーターを動かす
                moterl.pulsewidth(palse);  //パルス幅
                moterr.pulsewidth(palse);
                //焼き切り開始
                led1=1;
                //fet3=1;
                wait(80);//焼き切り時間1分
                led1=0;
                angle_target = 90-atan2(sin(latitude_target-latitude),cos(longitude)*tan(longitude_target)-sin(longitude)*cos(latitude_target-latitude));
                if((angle_target+angle)>360){angle_targetmath = (int(angle_target)+int(angle))%360;}
                else{angle_targetmath = angle_target+angle;}
                pc.printf("%f %d \n",angle_target,angle_targetmath);
                count++;
            }
            break;
        case 1:
            //落ちてパラシュートを焼き切った後
            pc.printf("case1");
            if( (angle_targetmath+5)>=int(angle) && (angle_targetmath-5)<=int(angle) )//3は適当な数字。誤差範囲
            {
                //角度が合ったらcount+1
                count++;
                
            }
            else
            {
                //角度が合うまで回転
                if(angle_targetmath>=angle)
                {
                    //differ_angle=angle_target-angle;
                    //moterlを動かす
                    moterl.pulsewidth(palse);
                    pc.printf("L \n");
                }
                else if(angle>=angle_targetmath)
                {
                    //differ_angle=angle-angle_target;
                    //moterrを動かす
                    moterr.pulsewidth(palse);
                    pc.printf("R \n");
                }
            }
            break;
        case 2:
            //pc.printf("case2");
            //目標地点の近くまで走行
            //目標地点に近づいたらcount+1
            //緯度longitude　経度latitude,            
            longitudegosa = longitude_target-longitude;
            latitudegosa = latitude_target-latitude;
            
            if( abs(longitudegosa)<=50 && abs(latitudegosa)<=50 )
            {
                //longitude =;
                //latitude =;
                count++;
            }
            else
            {
                pc.printf("front \n");
                moterl.pulsewidth(palse);  //パルス幅
                moterr.pulsewidth(palse);
            }
            break;
        case 3:
            pc.printf("case3");
            //動作を停止。割り込みも停止させ、回収待ち
            //moterl=moterr=0;
            //ngps.detach();
            count++;
            break;
        default:
            moterl.pulsewidth(0.0);
            moterr.pulsewidth(0.0);
            break;
        }
}
 
int main()
{
    pc.printf("START! \n");
    gps.baud(9600);
    gps.attach(gps_rx, Serial::RxIrq);//ＧＰＳ割り込み
    //pc.attach(gps_rx, Serial::RxIrq);
    sscanf(gps_target,"$GPRMC,%f,A,%f,N,%f,E,%f,%f",&gpstime_target,&longitude_target,&latitude_target,&knot_target,&angle_target);
    pc.printf("%f %f %f %f %f \n",gpstime_target,longitude_target,latitude_target,knot_target,angle_target);
    janpa1=1;
    while(janpa2!=0);
    pc.printf("PINOK");
    moterl.period(0.02);      // 周期
    moterr.period(0.02); 
    while(1)
    {
        pc.putc(gps.getc());
        pc.printf(",");
        moterl.pulsewidth(0.0020);  //パルス幅
        moterr.pulsewidth(0.0020);
        //move();
    }

}