File content as of revision 21:548ac557ee95:

#define cansatB
#include "mbed.h" 
//mbed
#include "getGPS.h"
#include "math.h"
//GPS
#include "TB6612.h"
 //motorDriver
 #include "JPEGCamera.h"
//カメラ
#include <stdio.h>
#include <base64.h>
//XBee
#include "us015.h"
// 超音波センサ


GPS gps (p28,p27);                 //GPS
Timer time_gps;

DigitalOut FET(p21);                //FET
DigitalOut myled(LED1);
US015 hs(p12,p11);
DigitalIn thermo(p20);
DigitalOut Sb612switch(p15);         //焦電スイッチ
DigitalOut Ultra(p12);
Serial pc(USBTX,USBRX);              // tx, rx
JPEGCamera camera(p9, p10);          // TX, RX
TB6612 left1(p25,p17,p16);            //モーターピン
TB6612 right1(p26,p19,p18);           //モーターピン
Serial xbee(p13,p14);                //xbee
DigitalIn flight(p23);      //フライトピン(in)
DigitalOut SW(p24);         //フライトピン（out）           

int main()
{
    
    
   Sb612switch=0; //焦電off
    wait(1);
    Ultra=0;//超音波off
    SW=1;
    flight==1;//flight pin ついてる
    FET=0;//FET off  
    myled=1; 
    xbee.printf("you worry about me don't you?\r\n");

     //FET
     while(1){
     if(flight==1) {
        myled=1;
        wait(10);
        printf("mada\r\n");
        }
     else{
        if(flight==1) {
            myled=1;
        wait(10);
        xbee.printf("madamada\r\n");
        }
        else{
          myled=0;
          xbee.printf("yattar\r\n");
          wait(20);
          FET=1;
          wait(10);
          FET=0;
          wait(10);
          xbee.printf("FET End!\r\n");
          SW=0;
        
    break;
            }
    }   
    }
    
    
      //以下GPS
     double a=0;
     double b=0;
     double distance;
     double timeout_flag=0;
     int k = 0;
     int j = 0;
     
     pc.printf("GPS Start\n");
     xbee.printf("start\n");
     time_gps.start();
     while(a==0)
        {
            if(gps.getgps()){
                a=gps.latitude;
                b=gps.longitude;
                }
           
            pc.printf("%lf,%lf\n",a,b);/*着地地点の緯度と軽度を測る*/
            if(time_gps.read()>500){/*500経ったら次に進む*/
                xbee.printf("timeout!!\n");/*xbeeで時間切れの場合はtimeoutと出力する*/
                timeout_flag=1;
                break;
            }
        }
       printf("moter on%lf",a);
        left1 = 100; //左モーター１００％
        right1 = 100;//右モーター１００％
        
        wait(10);
        printf("moter off");
        left1 = 0; //左モーター０％
        right1 = 0;//右モーター０％
        printf("moter off");
        wait(15);
        
        while(!timeout_flag) {
         printf("GPS restart\n");
         if(gps.getgps()){
           
          pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示   
          j ++;
          if(j<10){
           }else{
            // 球面三角法により、大円距離(メートル)を求める
           double lat2;
           double lon2; 
           lat2 = gps.latitude;
           lon2 = gps.longitude;

            pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//動いた後の緯度と経度を表示   
            const double pi = 3.14159265359; // 円周率
                           
            double ra = a * pi / 180;
            double rb = b * pi / 180;     // 緯度経度をラジアンに変換
            double rc = lat2 * pi / 180;
            double rd = lon2 * pi / 180;
        
            double e = sin(ra) * sin(rc) +  cos(ra) * cos(rc) * cos(rd - rb);  // 2点の中心角(ラジアン)を求める
            double rr = acos(e);

            const double earth_radius = 6378140;   // 地球赤道半径(m)

            distance = earth_radius * rr; // 2点間の距離(m)
            printf("distance=%f\ne=%lf\nrr=%lf\n",distance,e,rr);
            if (distance<5){
             }else{
                 pc.printf("(a =%lf,b =%lf)\n\r",a,b);
             pc.printf("(c =%lf,d =%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示   
             pc.printf("5m clear!");
             xbee.printf("5m clear!");
              break;
           }  
            }
       }else{
         printf("No data");
          }
        } //GPS End */    

 int i=1;
 float th;
 Timer tm;
 for(i=0;i<3;i++){
   pc.printf("start\r\n");
  
   left1 = 100; //左モーター１００％
   right1 = 100;//右モーター１００％

   printf("Restart\r\n" );

   wait(4);
   left1=50;
   right1=50;
   wait(1);
   left1=0;
   right1=0;
   wait(1);

   printf("停止\n\r");

   Sb612switch=0; //焦電off
   wait(1);
   Ultra=1;//超音波on
   wait(1);
    
    while(1) {
      left1 = 0;
      right1 = 0;

      printf("超音波on\r\n 焦電off\r\n" )  ;

      hs.TrigerOut();
      wait(1);
      int distance;
      distance = hs.GetDistance();

      printf("distance=%dmm\r\n",distance);//距離出力
        
      if(distance<=2000){//超音波反応
       Ultra=0;//超音波off
       wait(1);
       Sb612switch=1; //焦電on
       wait(1);

      printf("焦電On!\r\n  ");
           bool detected=false;
             th = thermo;
             if(th==1 && !detected) {//焦電反応ありの場合
               i++; 
               detected=true;
               xbee.printf("human\r\n");
               tm.reset();
               tm.start();
          
               LocalFileSystem local("local");
               Timer timer;
               timer.start();
               camera.setPictureSize(JPEGCamera::SIZE320x240);
               
               FILE *fp;
               base64 *bs;
               int c;

            for (int r = 0; r < 1; r++) {
               if (camera.isReady()) {
                 char filename[32];
                 sprintf(filename, "/local/pict%03d.jpg",r);
                 printf("Picture: %s ", filename);
                 if (camera.takePicture(filename)) { 
                  while (camera.isProcessing()) {
                    camera.processPicture();
                    printf("take pictuer!");
                   }
                 }else{
                 printf("take picture failed\r\n");
                 }
               }else{
                 printf("camera is not ready\r\n");
                }
               
               printf("time = %f\n", timer.read());
               
       
               
               xbee.printf("i could take apicture!\r\n");
               bs = new base64();
               bs->Encode("/local/pict000.jpg","/local/data000.txt");
               
               if((fp=fopen("/local/data000.txt","r"))!=NULL)
               {
                   pc.printf("ok\r\n");
                   while((c=fgetc(fp))!=EOF){
                       xbee.printf("%c",c);
                    }
                    fclose(fp);
                }
                }
                while(1){
        
                    int received_data = xbee.getc();
   
                    if (received_data == 82 || received_data == 114){  //Rまたはｒ
                        xbee.printf("_________________________________________________________________________________________________________________________________\r\n");
                         if((fp=fopen("/local/data000.txt","r"))!=NULL)
                         {
                            while ((c=fgetc(fp))!=EOF){
                                xbee.printf("%c",c);                    //再送
                             }
                         fclose(fp);
                         }
                     }
                     else{
                         break;
                         }
                    }
     
               Sb612switch=0; //焦電off
               wait(1);
              }else{//焦電反応なしの場合
              printf("not found!\r\n");
               
               
               Sb612switch=0;
               wait(1);
               Ultra=0;
               wait(1);
               detected=false;
               printf("後退\r\n");
               left1 = -100; //左モーター-5０％
               right1 = -100;//右モーター-5０％
               wait(2.0);
               left1=-50;
               right1=-50;
               wait(1);
               left1=0;
               right1=0;
               wait(1);
               
               printf("右折\n\r");
               
               left1 = 60; //左モーター１００％
               right1 = 100;//右モーター１００％
               wait(2.0);
              }               
      }else{//超音波distance>2000
        printf("safety zone\r\n");
        Ultra=0;
        wait(1);
        left1 = 60; //左モーター50％
        right1 = 100;//右モーター50％
        printf("右折\r\n");
        wait(3);
        left1 = 100;
        right1 = 100;
        wait(5);
        left1 = 0;
        right1 = 0;
        wait(5);
      }

    }

  }
 }