File content as of revision 5:5aa7223226df:

#define cansatB
#define min(x, y) ((x) < (y)) ? (x) : (y)
#include "mbed.h"       //mbed
#include "getGPS.h"         //GPS
#include "math.h"         //GPS
#include "TB6612.h"      //motorDriver
#include "JPEGCamera.h"//カメラ
#include "base64.h"//写真ーXBee
#include "us015.h"  // 超音波センサ
#include <stdio.h>
US015 hs(p12,p11);                  //P12 :超音波センサ トリガ出力  //p11 :超音波センサ  エコー入力
GPS gps (p28,p27);                 //GPS

DigitalOut FET(p21);                //FET
DigitalOut Ultra(p12);
DigitalIn thermo(p20);     //焦電センサ↓
DigitalOut led(LED1);     
Serial pc(USBTX,USBRX); // tx, rx  焦電センサ↑
Serial xbee(p13, p14);
TB6612 left(p25,p17,p16);//motor
TB6612 right(p26,p19,p18);//motor

void JPEGCamera(void);
void us015(void);
void motorFoward(void);
void motorFowardR(void);
void motorForwardL(void);
void motorStop(void);
void motorReverse(void);  
    LocalFileSystem local("local");                                       

int main() {   
    printf("CanSat-B_Start!\r\n");
         //FET
    FET = 0;
    SW = 1;  //p21をhigh(3.3V)にする。 
     while(1) {
    if(flight==1) {
        wait(10);
        }
        
    else{
        if(flight==1) {
        wait(10);
        }
        else{
        FET = 0;                       //FET、ニクロム線切断
        wait(20);
        FET = 1;
        wait(12);
        FET = 0;
        wait(10);
        FET = 1;
        wait(12);
        FET = 0; 
        SW = 0;  //p21をlow(0V)にする。 
    break;
            }
    }
    }
    
      //以下GPS
     double a;
     double b;
     double distance;
    
    pc.printf("GPS Start\r\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){
             printf("%lf\r\n",distance);   
             motorFoward();
             }else{
             motorStop();
             pc.printf("5m clear!");
             break;
              }
          
          }else{
           pc.printf("NO DATA\r\n");//データ取得失敗
           wait(1);
           }
        }
        //GPS End     

  float th;
  Timer tm;
  pc.printf("start\r\n");

  bool detected=false;
    thermo=0; //焦電off
    Ultra=1;//超音波on
   
         us015();
        if(distance<2000){//超音波反応
         Ultra=0;//超音波off
         thermo=1;//焦電on
         motorStop();
        
          if(true)
             th = thermo;
             if(th=1 && !detected) {//焦電反応ありの場合
               detected=true;
             pc.printf("human\r\n");
             tm.reset();
             tm.start();
             thermo=0;
    JPEGCamera();

    FILE *fp;
    base64 *bs;
    int c;
    
    xbee.printf("charizard!!!\r\n");
    bs = new base64();
    bs->Encode("/local/PICT000.jpg","/local/d.txt");
    
    
   if((fp=fopen("/local/d.txt","r"))!=NULL)
   {
       while ((c=fgetc(fp))!=EOF){
           xbee.printf("%c",c);
       }
       fclose(fp);
   }


            }else{//焦電反応なしの場合
             detected=false;
         thermo=0;
         Ultra=1;
             }
           

  //while(true)
  
  
//超音波センサー反応あり
//停止
//超音波センサーOFF
//焦電センサーON
       
   //焦電センサー反応あり
   //焦電センサーOFF
   //カメラ起動
   //カメラOFF
   //XBeeによりPCへ送信
       if(receiveOK()=true){
           printf("Unknown Creature has been descovered!\n");
           }
       //"OK"受信、ミッション終了
       else if(receiveOK()=false){
           printf("Continue!\n");
           }
       //"NO"受信、ミッション再開

    else if(detected=false){
        thermo=0;
        motorFowardR();
        wait(3);
        Ultra=1;
        motorFoward();
        }
    
    //焦電センサー反応無し
    //焦電センサーOFF
    //方向転換
    //超音波センサーON
    //直進
}
else if(distance>=2000){
    motorStop();
    wait(2);
    motorFowardR();
    wait(3);
}
//超音波センサー反応無し
//停止
//方向転換
//直進
}


void JPEGCamera(){
           const int RESPONSE_TIMEOUT = 500;
const int DATA_TIMEOUT = 1000;

JPEGCamera::JPEGCamera(PinName tx, PinName rx) : Serial(tx, rx) {
    printf("AA\r\n");
    baud(38400);
    state = READY;
}

bool JPEGCamera::setPictureSize(JPEGCamera::PictureSize size, bool doReset) {
    char buf[9] = {0x56, 0x00, 0x31, 0x05, 0x04, 0x01, 0x00, 0x19, (char) size};
    int ret = sendReceive(buf, sizeof buf, 5);

    if (ret == 5 && buf[0] == 0x76) {
        if (doReset)
            reset();
        return true;
    } else
        return false;
}

bool JPEGCamera::isReady() {
    return state == READY;
}

bool JPEGCamera::isProcessing() {
    return state == PROCESSING;
}

bool JPEGCamera::takePicture(char *filename) {
    if (state == READY) {
        fp = fopen(filename, "wb");
        if (fp != 0) {
            if (takePicture()) {
                imageSize = getImageSize();
                address = 0;
                state = PROCESSING;
            } else {
                fclose(fp);
                printf("takePicture(%s) failed", filename);
                state = ERROR;
            }
        } else {
            printf("fopen() failed");
            state = ERROR;
        }
    }
    return state != ERROR;
}

bool JPEGCamera::processPicture() {
    if (state == PROCESSING) {
        if (address < imageSize) {
            char data[1024];
            int size = readData(data, min(sizeof(data), imageSize - address), address);
            int ret = fwrite(data, size, 1, fp);
            if (ret > 0)
                address += size;
            if (ret == 0 || address >= imageSize) {
                stopPictures();
                fclose(fp);
                wait(0.1); // ????
                state = ret > 0 ? READY : ERROR;
            }
        }
    }

    return state == PROCESSING || state == READY;
}

bool JPEGCamera::reset() {
    char buf[4] = {0x56, 0x00, 0x26, 0x00};
    int ret = sendReceive(buf, sizeof buf, 4);
    if (ret == 4 && buf[0] == 0x76) {
        wait(4.0);
        state = READY;
    } else {
        state = ERROR;
    }
    return state == READY;
}

bool JPEGCamera::takePicture() {
    char buf[5] = {0x56, 0x00, 0x36, 0x01, 0x00};
    int ret = sendReceive(buf, sizeof buf, 5);

    return ret == 5 && buf[0] == 0x76;
}

bool JPEGCamera::stopPictures() {
    char buf[5] = {0x56, 0x00, 0x36, 0x01, 0x03};
    int ret = sendReceive(buf, sizeof buf, 5);

    return ret == 4 && buf[0] == 0x76;
}

int JPEGCamera::getImageSize() {
    char buf[9] = {0x56, 0x00, 0x34, 0x01, 0x00};
    int ret = sendReceive(buf, sizeof buf, 9);

    //The size is in the last 2 characters of the response.
    return (ret == 9 && buf[0] == 0x76) ? (buf[7] << 8 | buf[8]) : 0;
}

int JPEGCamera::readData(char *dataBuf, int size, int address) {
    char buf[16] = {0x56, 0x00, 0x32, 0x0C, 0x00, 0x0A, 0x00, 0x00,
                    address >> 8, address & 255, 0x00, 0x00, size >> 8, size & 255, 0x00, 0x0A
                   };
    int ret = sendReceive(buf, sizeof buf, 5);

    return (ret == 5 && buf[0] == 0x76) ? receive(dataBuf, size, DATA_TIMEOUT) : 0;
}

int JPEGCamera::sendReceive(char *buf, int sendSize, int receiveSize) {
    while (readable()) getc();

    for (int i = 0; i < sendSize; i++) putc(buf[i]);

    return receive(buf, receiveSize, RESPONSE_TIMEOUT);
}

int JPEGCamera::receive(char *buf, int size, int timeout) {
    timer.start();
    timer.reset();

    int i = 0;
    while (i < size && timer.read_ms() < timeout) {
        if (readable())
            buf[i++] = getc();
    }

    return i;
}
void us015(){
    hs.TrigerOut();
         wait(1);
         int distance;
         distance = hs.GetDistance();
         printf("%d\r\n",distance);//距離出力
         }

void motorFoward(){
    left = 100; 
        right = 100;
        printf("直進\n\r");
    }
void motorFowardR(){
    left = 100; 
        right = 20;
        printf("右折\n\r");
    }
void motorForwardL(){
    left = 20; 
        right = 100;
        printf("左折\n\r");}
void motorStop(){
    left = 0; 
        right = 0;
        printf("停止\n\r");}
void motorReverse(){
    left = -50;
        right = -50;
        printf("バック\n\r");
        }