File content as of revision 6:586f877b5aa9:

#include "mbed.h"
#include "JPEGCamera.h"
#include "us015.h"
#include "TB6612.h"
#include <stdio.h>
#include <base64.h>
#include "getGPS.h"
#include "math.h"

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

GPS gps (p28,p27);                   //GPS


int main(){
  int i=1;
  float th;
  Timer tm;
  double a;
  double b;
  double distance;
  
    pc.printf("GPS Start\n");
    
    while(1) {
         if(gps.getgps()){
           a = gps.latitude;
           b = gps.longitude;
           
          pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示   
           break;
           
         }else{
          pc.printf("NO DATA\r\n");//データ取得失敗
          wait(1);
            }
       }
      while(1){
         if(gps.getgps()) {
           
          pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示   
          
       // 球面三角法により、大円距離(メートル)を求める
       double c;
       double d; 
        c = gps.latitude;
        d = gps.longitude;

         const double pi = 3.14159265359; // 円周率
                           
            double ra = a * pi / 180;
            double rb = b * pi / 180;     // 緯度経度をラジアンに変換
            double rc = c * pi / 180;
            double rd = d * pi / 180;
        
            double e = sin(ra) * sin(rc) +  cos(ra) * cos(rc) * cos(rb - rd);  // 2点の中心角(ラジアン)を求める
            double rr = acos(e);

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

            distance = earth_radius * rr; // 2点間の距離(m)
             

         if (distance<5){
             }else{
             pc.printf("5m clear!");
             xbee.printf("5m clear!");
             break;
              }
          
          }else{
           xbee.printf("NO DATA\r\n");//データ取得失 
           pc.printf("NO DATA\r\n");  
           wait(1);
           }
        }
  
  for(i=0;i<3;i++){
  pc.printf("start\r\n");
  
  left1 = 100; //左モーター１００％
  right1 = 100;//右モーター１００％
 
    Sb612switch=0; //焦電off
    Ultra=1;//超音波on
    
    while(1) {
         printf("超音波on\r\n 焦電off\r\n" )  ;
         hs.TrigerOut();
         wait(1);
         int distance;
         distance = hs.GetDistance();
         printf("distance=%d\r\n",distance);//距離出力
        
        if(distance<=2000){//超音波反応
         left1 = 0; //左モーター０％
         right1 = 0;//右モーター０％
         printf("停止\n\r");
         wait(5.0);
         
         Ultra=0;//超音波off
         Sb612switch=1; //焦電on
          printf("超音波off\r\n焦電on\r\n  ");
           bool detected=false;
             th = thermo;
             if(th==1 && !detected) {//焦電反応ありの場合
               i++; 
               detected=true;
               pc.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("xbee connected!\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);
                }
                }
               
              }else{//焦電反応なしの場合
              printf("not found!\r\n");
               
               left1 = 70; //左モーター１０％
               right1 = 100;//右モーター１００％(右折)
               Sb612switch=0;
               Ultra=1;
               detected=false;
               printf("右折\n\r");
               wait(3.0);
               left1 = 100; //左モーター１００％
               right1 = 100;//右モーター１００％
              }           
       }else{//超音波distance>2000
          printf("safety zone\r\n");
          left1 = 100; //左モーター100％
          right1 = 70;//右モーター70％(左折)
          printf("左折\r\n");
         }

    }

}
  }