main.cpp

00001 #define cansatB
00002 #include "mbed.h"       //mbed
00003 #include "getGPS.h"         //GPS
00004 #include "math.h"         //GPS
00005 #include "TB6612.h"      //motorDriver
00006 #include "XBee.h"        //XBee
00007 #include <SoftwareSerial.h>   //カメラ
00008 #include <SD.h>//SDカード
00009 #include <JPEGCamera.h>//カメラ
00010 #include "us015.h"  // 超音波センサ
00011 #include <stdio.h>
00012 US015 hs(p12,p11);                  //P12 :超音波センサ トリガ出力  //p11 :超音波センサ  エコー入力
00013 Serial pc(USBTX,USBRX);            //GPs
00014 GPS gps (p28,p27);                 //GPS
00015 PwmOut motorSpeedR(p26);  //モーター
00016 PwmOut motorSpeedL(p25);  //モーター
00017 DigitalIn flight(p22,p23);      //フライトピン
00018 DigitalOut FET(p21);                //FET
00019 XBee xbee(p13, p14);               // XBee
00020 DigitalIn thermo(p20);     //焦電センサ↓
00021 DigitalOut led(LED1);     
00022 Serial pc(USBTX, USBRX); // tx, rx  焦電センサ↑
00023 Serial pc (p9);     //カメラ
00024 TB6612 left(p25,p17,p16);
00025 TB6612 right(p26,p19,p18);
00026 
00027 int main() {            //FET
00028     FET = 0;
00029     SW = 1;  //p21をhigh(3.3V)にする。 
00030      while(1) {
00031     if(flight==1) {
00032         wait(10);
00033         }
00034         
00035     else{
00036         if(flight==1) {
00037         wait(10);
00038         }
00039         else{
00040         FET = 0;                       //FET、ニクロム線切断
00041         wait(20);
00042         FET = 1;
00043         wait(12);
00044         FET = 0;
00045         wait(10);
00046         FET = 1;
00047         wait(12);
00048         FET = 0; 
00049         SW = 0;  //p21をlow(0V)にする。 
00050     break;
00051             }
00052     }
00053     }
00054     }
00055 
00056 int main() {       //以下GPS
00057      double a;
00058      double b;
00059      double distance;
00060     
00061     pc.printf("GPS Start\r\n");
00062     
00063      while(1) {
00064          if(gps.getgps()){
00065            a = gps.latitude;
00066            b = gps.longitude;
00067            
00068           pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示
00069            
00070            break;
00071            
00072          }else{
00073           pc.printf("NO DATA\r\n");//データ取得失敗
00074           wait(1);
00075             }
00076        }
00077       while(1){
00078          if(gps.getgps()) {
00079            
00080           pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示   
00081           
00082        // 球面三角法により、大円距離(メートル)を求める
00083        double c;
00084        double d; 
00085         c = gps.latitude;
00086         d = gps.longitude;
00087 
00088          const double pi = 3.14159265359; // 円周率
00089                            
00090             double ra = a * pi / 180;
00091             double rb = b * pi / 180;     // 緯度経度をラジアンに変換
00092             double rc = c * pi / 180;
00093             double rd = d * pi / 180;
00094         
00095             double e = sin(ra) * sin(rc) +  cos(ra) * cos(rc) * cos(rb - rd);  // 2点の中心角(ラジアン)を求める
00096             double rr = acos(e);
00097 
00098             const double earth_radius = 6378140;   // 地球赤道半径(m)
00099 
00100             distance = earth_radius * rr; // 2点間の距離(m)
00101             
00102            
00103              
00104 
00105          if (distance<5){
00106              printf("%lf\r\n",distance);   
00107              }else{
00108              pc.printf("5m clear!");
00109              break;
00110               }
00111           
00112           }else{
00113            pc.printf("NO DATA\r\n");//データ取得失敗
00114            wait(1);
00115            }
00116         }
00117         return 0;          //注意！void()に変えること.このままだとここで終わる
00118      }
00119       
00120 int main() {
00121     while(1) {
00122         left = 100; //左モーター１００％
00123         right = 100;//右モーター１００％
00124         wait(1.0);
00125         left = 30;//左３０％
00126         right = 30;//右３０％
00127         wait(1.0);
00128         printf("OK");
00129     }
00130 }
00131 
00132 if(distance<2000){
00133     motorStopR();
00134     motorStopL();
00135     stopUS015();
00136     startsb612a();
00137 //超音波センサー反応あり
00138 //停止
00139 //超音波センサーOFF
00140 //焦電センサーON
00141   int main()
00142 {
00143   float th;
00144   Timer tm;
00145   pc.printf("start\r\n");
00146   led=0;
00147   bool detected=false;
00148   while(true)
00149   {
00150     th = thermo;
00151     if(th==1 && !detected) {
00152       led = 1;
00153       detected=true;
00154       pc.printf("human\r\n");
00155       tm.reset();
00156       tm.start();
00157     }
00158     if(tm.read_ms()>10000) {
00159       printf("Time out!\r\n");
00160       led = 0;
00161       detected=false;
00162     }
00163   }
00164 }
00165    if(detected=true){
00166        stopsb612a();
00167        startCamera();
00168        stopCamera();
00169        sendSD();
00170        sendPC();
00171        
00172    //焦電センサー反応あり
00173    //焦電センサーOFF
00174    //カメラ起動
00175    //カメラOFF
00176    //データをSDカードに保存
00177    //保存データをXBeeによりPCへ送信
00178        if(receiveOK()=true){
00179            printf("Unknown Creature has been descovered!\n");
00180            }
00181        //"OK"受信、ミッション終了
00182        else if(receiveOK()=false){
00183            printf("Continue!\n");
00184            }
00185        //"NO"受信、ミッション再開
00186 
00187     else if(detected=false){
00188         stopsb612a();
00189         motorForwardL();
00190         motorStopL();
00191         motorForwardR();
00192         motorForwardL();
00193         startUS015();
00194         }
00195     } 
00196     //焦電センサー反応無し
00197     //焦電センサーOFF
00198     //方向転換
00199     //超音波センサーON
00200     //直進
00201 }
00202 else if(distance>=2000){
00203     motorStopL();
00204     motorStopR();
00205     motorForwardL();
00206     motorStopL();
00207     motorForwardR();
00208     motorForwardL();
00209 }
00210 //超音波センサー反応無し
00211 //停止
00212 //方向転換
00213 //直進