main.cpp

#include "mbed.h"
#include "SDFileSystem.h"
 
Ticker timer;
//SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
Serial gp(p28, p27);       // tx, rx
Serial pc(USBTX, USBRX);    // tx, rx 
PwmOut moter_l(p21);//左モーター
PwmOut moter_r(p22);//右モーター
DigitalOut fet3(p23);//ニクロム線
DigitalOut janpa1(p19);//パラシュートの開きを検知
DigitalIn janpa2(p20);//パラシュートの開きを検知
DigitalOut led1(LED1);
int ido_gosa,kei_gosa;
int ido_target=1000; //緯度の目標値　秒以下を入力　要設定
int kei_target=1000; //経度の目標値　秒以下を入力　要設定
int old_ido,old_kei;
int ido,kei;
char gpsc[500];
int a,b,c,d;
int i=0;
 
void gps_kakiko()
{
    led1=1;
    i=0;
    mkdir("/sd/gps", 0777);
    FILE *fp = fopen("/sd/gps/gps.txt", "w");
    if(fp == NULL) {
        error("Could not open file for write\n");
        } 
 
    while(i!=50)
     { fputc(gp.getc(),fp);//sdに50文字づつ書き込み
       i++; }
    
    fclose(fp);
    led1=0;
 
 
}
 
void nikuromu()
{
    led1=1;
    fet3=1;
    wait(60);//焼き切り時間
}
 
void moter()
{
    ido_gosa = kei_gosa = 20;
    while(ido_gosa >= 10 && kei_gosa >= 10)
    {
        
       a=50;
       gpsc[a]= gp.getc();
        
        if(gpsc[a-34]=='A')//GPGGAを見つける
         {
           if(gpsc[a-33]=='G')
            {
                if(gpsc[a-32]=='G')
                 {
                   old_ido=ido;
                   old_kei=kei; 
                   ido=gpsc[a-16]*1000+gpsc[a-15]*100+gpsc[a-14]*10+gpsc[a-13];
                   kei=gpsc[a-3]*1000+gpsc[a-2]*100+gpsc[a-1]*10+gpsc[a];
                   ido_gosa = ido_target-ido;
                   kei_gosa = kei_target-kei;
                   if(a>=50)
                    {a=25;}}}}    
    
       moter_l.period(0.02);      // 周期
       moter_r.period(0.02);          
       moter_l.pulsewidth(0.0020);  //パルス幅
       moter_r.pulsewidth(0.0020);
    
       if(kei_gosa >=10 && ido_gosa >=10)//先ず経度or緯度をあわせる
         {    moter_l.pulsewidth(0.0020);
              moter_r.pulsewidth(0.0);
              wait(5); //方向転換90°　要設定
              moter_l.pulsewidth(0.0020);
              moter_r.pulsewidth(0.0020);
              wait(10); //gpsの変化待ち
           
              if(kei_gosa >=10 && ido_gosa >=10)
                continue; //　方向転換をやりなおす
         }
      
       if(kei_gosa >=10 && ido_gosa <=10 || kei_gosa <=10 && ido_gosa >=10)
         {    moter_l.pulsewidth(0.0020);
              moter_r.pulsewidth(0.0020);
              if(kei_gosa >=10 && ido_gosa <=10 || kei_gosa <=10 && ido_gosa >=10)
                continue; //　進み続ける
         }
         
        }
    moter_l.pulsewidth(0.0);
    moter_r.pulsewidth(0.0);
    
    
}
    
int main()
{
    janpa1=1;
    gp.baud(9600);
    //timer.attach(&gps_kakiko, 2.0);//割り込み
    if(janpa2==0)//パラシュートが開き、ジャンパピンが抜けたらスタート
    {
    wait(60);//落下中　要設定
    nikuromu();
    moter();
    }
    return 0;
}