File content as of revision 2:e2b803e3bcbc:

#define cansatB
#include "mbed.h"       //mbed
#include "getGPS.h"         //GPS
#include "math.h"         //GPS
#include "XBee.h"        //XBee
#include <SoftwareSerial.h>   //カメラ
#include <SD.h>//SDカード
#include <JPEGCamera.h>//カメラ
#include "us015.h"  // 超音波センサ
#include <stdio.h>
US015 hs(p12,p11);                  //P12 :超音波センサ トリガ出力  //p11 :超音波センサ  エコー入力
Serial pc(USBTX,USBRX);            //GPs
GPS gps (p28,p27);                 //GPS
PwmOut motorSpeedR(p26);  //モーター
PwmOut motorSpeedL(p25);  //モーター
DigitalIn flight(p22,p23);      //フライトピン
DigitalOut FET(p21);                //FET
XBee xbee(p13, p14);               // XBee
DigitalIn thermo(p20);     //焦電センサ↓
DigitalOut led(LED1);     
Serial pc(USBTX, USBRX); // tx, rx  焦電センサ↑
Serial pc (p9);     //カメラ
//Motor motor1(p22, p16, p17, 1); // pwm, fwd, rev, can brake     モーター
//Motor motor2(p22, p19, p20, 1); // pwm, fwd, rev, can brake

int main() {            //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;
            }
    }
    }
    }

int main() {       //以下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);   
             }else{
             pc.printf("5m clear!");
             break;
              }
          
          }else{
           pc.printf("NO DATA\r\n");//データ取得失敗
           wait(1);
           }
        }
        return 0;          //注意！void()に変えること.このままだとここで終わる
     }
      

void motorForwardR(void);
void motorStopR(void);
void motorReverseR(void);
void motorForwardL(void);
void motorStopL(void);
void motorReverseL(void);
LocalFileSystem local(“local”);               // Create the local filesystem under the name “local”　　 データ保存
float culculate_distance_3(float a,float b);

   motorStopR();
   motorStopL();
   wait(20);     //確認用//デフォ20秒
//    FILE *fp = fopen(“/local/gps.txt”, “w”);  // Open “gps.txt” on the local file system for writing
         motorSpeedR.period_ms(4);              //モーター調節
         motorSpeedR = 0.655;
         motorSpeedL.period_ms(4);              //モーター調節
motorSpeedL = 0.755;
 motorForwardL(); //車体を時計回りに３秒回転
   motorReverseR();
   wait(1.6);
   motorStopR();
   motorStopL();
   wait(1);
   motorReverseL(); //車体を反時計回りに３秒回転
   motorForwardR();
   wait(1.6);
   motorStopR();
   motorStopL();
   wait(1);
   printf(“compass carriblation\r\n”); //キャリブレーション終了
//float mcn1,mcn2;

if(distance<2000){
    motorStopR();
    motorStopL();
    stopUS015();
    startsb612a();
//超音波センサー反応あり
//停止
//超音波センサーOFF
//焦電センサーON
  int main()
{
  float th;
  Timer tm;
  pc.printf("start\r\n");
  led=0;
  bool detected=false;
  while(true)
  {
    th = thermo;
    if(th==1 && !detected) {
      led = 1;
      detected=true;
      pc.printf("human\r\n");
      tm.reset();
      tm.start();
    }
    if(tm.read_ms()>10000) {
      printf("Time out!\r\n");
      led = 0;
      detected=false;
    }
  }
}
   if(detected=true){
       stopsb612a();
       startCamera();
       stopCamera();
       sendSD();
       sendPC();
       
   //焦電センサー反応あり
   //焦電センサーOFF
   //カメラ起動
   //カメラOFF
   //データをSDカードに保存
   //保存データを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){
        stopsb612a();
        motorForwardL();
        motorStopL();
        motorForwardR();
        motorForwardL();
        startUS015();
        }
    } 
    //焦電センサー反応無し
    //焦電センサーOFF
    //方向転換
    //超音波センサーON
    //直進
}
else if(distance>=2000){
    motorStopL();
    motorStopR();
    motorForwardL();
    motorStopL();
    motorForwardR();
    motorForwardL();
}
//超音波センサー反応無し
//停止
//方向転換
//直進