main.cpp

00001 #define cansatB
00002 #include "mbed.h" 
00003 //mbed
00004 #include "getGPS.h"
00005 #include "math.h"
00006 //GPS
00007 #include "TB6612.h"
00008  //motorDriver
00009  #include "JPEGCamera.h"
00010 //カメラ
00011 #include <stdio.h>
00012 #include <base64.h>
00013 //XBee
00014 #include "us015.h"
00015 // 超音波センサ
00016 
00017 
00018 GPS gps (p28,p27);                 //GPS
00019 Timer time_gps;
00020 
00021 DigitalOut FET(p21);                //FET
00022 DigitalOut myled(LED1);
00023 US015 hs(p12,p11);
00024 DigitalIn thermo(p20);
00025 DigitalOut Sb612switch(p15);         //焦電スイッチ
00026 DigitalOut Ultra(p12);
00027 Serial pc(USBTX,USBRX);              // tx, rx
00028 JPEGCamera camera(p9, p10);          // TX, RX
00029 TB6612 left1(p26,p17,p16);            //モーターピン
00030 TB6612 right1(p25,p19,p18);           //モーターピン
00031 Serial xbee(p13,p14);                //xbee
00032 DigitalIn flight(p23);      //フライトピン(in)
00033 DigitalOut SW(p24);         //フライトピン（out）           
00034 
00035 int main()
00036 {
00037     
00038     
00039    Sb612switch=0; //焦電off
00040     wait(1);
00041     Ultra=0;//超音波off
00042     SW=1;
00043     flight==1;//flight pin ついてる
00044     FET=0;//FET off  
00045     myled=1; 
00046     printf("CanSat-B_Start!\r\n");
00047 
00048      //FET
00049      while(1){
00050      if(flight==1) {
00051         myled=1;
00052         wait(10);
00053         printf("mada\r\n");
00054         }
00055      else{
00056         if(flight==1) {
00057             myled=1;
00058         wait(10);
00059         printf("madamada\r\n");
00060         }
00061         else{
00062           myled=0;
00063           printf("yattar\r\n");
00064           wait(20);
00065           FET=1;
00066           wait(10);
00067           FET=0;
00068           wait(10);
00069           printf("FET End!\r\n");
00070           SW=0;
00071         
00072     break;
00073             }
00074     }   
00075     }
00076     
00077     
00078       //以下GPS
00079      double a=0;
00080      double b=0;
00081      double distance;
00082      double timeout_flag=0;
00083      int k = 0;
00084      int j = 0;
00085      
00086      pc.printf("GPS Start\n");
00087      xbee.printf("start\n");//printf("1");
00088      time_gps.start();
00089      while(a==0)
00090         {
00091             if(gps.getgps()){
00092                 a=gps.latitude;
00093                 b=gps.longitude;
00094                 }
00095            
00096             pc.printf("%lf,%lf\n",a,b);
00097             if(time_gps.read()>30){
00098                 xbee.printf("timeout!!\n");
00099                 timeout_flag=1;
00100                 break;
00101             }
00102         }
00103        printf("moter on%lf",a);
00104         left1 = 100; //左モーター１００％
00105         right1 = 100;//右モーター１００％
00106         
00107         wait(10);
00108         printf("moter off");
00109         left1 = 0; //左モーター０％
00110         right1 = 0;//右モーター０％
00111         printf("moter off");
00112         wait(15);
00113         
00114         while(!timeout_flag) {
00115          printf("GPS restart\n");
00116          if(gps.getgps()){
00117            
00118           pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示   
00119           j ++;
00120           if(j<10){
00121            }else{
00122             // 球面三角法により、大円距離(メートル)を求める
00123            double lat2;
00124            double lon2; 
00125            lat2 = gps.latitude;
00126            lon2 = gps.longitude;
00127 
00128             pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示   
00129             const double pi = 3.14159265359; // 円周率
00130                            
00131             double ra = a * pi / 180;
00132             double rb = b * pi / 180;     // 緯度経度をラジアンに変換
00133             double rc = lat2 * pi / 180;
00134             double rd = lon2 * pi / 180;
00135         
00136             double e = sin(ra) * sin(rc) +  cos(ra) * cos(rc) * cos(rd - rb);  // 2点の中心角(ラジアン)を求める
00137             double rr = acos(e);
00138 
00139             const double earth_radius = 6378140;   // 地球赤道半径(m)
00140 
00141             distance = earth_radius * rr; // 2点間の距離(m)
00142             printf("distance=%f\ne=%lf\nrr=%lf\n",distance,e,rr);
00143             if (distance<5){
00144              }else{
00145                  pc.printf("(a =%lf,b =%lf)\n\r",a,b);
00146              pc.printf("(c =%lf,d =%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示   
00147              pc.printf("5m clear!");
00148              xbee.printf("5m clear!");
00149               break;
00150            }  
00151             }
00152        }else{
00153          printf("No data");
00154           }
00155         } //GPS End */    
00156 
00157  int i=1;
00158  float th;
00159  Timer tm;
00160  for(i=0;i<3;i++){
00161    pc.printf("start\r\n");
00162   
00163    left1 = 100; //左モーター１００％
00164    right1 = 100;//右モーター１００％
00165 
00166    printf("Restart\r\n" );
00167 
00168    wait(4);
00169    left1=50;
00170    right1=50;
00171    wait(1);
00172    left1=0;
00173    right1=0;
00174    wait(1);
00175 
00176    printf("停止\n\r");
00177 
00178    Sb612switch=0; //焦電off
00179    wait(1);
00180    Ultra=1;//超音波on
00181    wait(1);
00182     
00183     while(1) {
00184       left1 = 0;
00185       right1 = 0;
00186 
00187       printf("超音波on\r\n 焦電off\r\n" )  ;
00188 
00189       hs.TrigerOut();
00190       wait(1);
00191       int distance;
00192       distance = hs.GetDistance();
00193 
00194       printf("distance=%dmm\r\n",distance);//距離出力
00195         
00196       if(distance<=2000){//超音波反応
00197        Ultra=0;//超音波off
00198        wait(1);
00199        Sb612switch=1; //焦電on
00200        wait(1);
00201 
00202       printf("焦電On!\r\n  ");
00203            bool detected=false;
00204              th = thermo;
00205              if(th==1 && !detected) {//焦電反応ありの場合
00206                i++; 
00207                detected=true;
00208                pc.printf("human\r\n");
00209                tm.reset();
00210                tm.start();
00211           
00212                LocalFileSystem local("local");
00213                Timer timer;
00214                timer.start();
00215                camera.setPictureSize(JPEGCamera::SIZE320x240);
00216                
00217                FILE *fp;
00218                base64 *bs;
00219                int c;
00220 
00221             for (int r = 0; r < 1; r++) {
00222                if (camera.isReady()) {
00223                  char filename[32];
00224                  sprintf(filename, "/local/pict%03d.jpg",r);
00225                  printf("Picture: %s ", filename);
00226                  if (camera.takePicture(filename)) { 
00227                   while (camera.isProcessing()) {
00228                     camera.processPicture();
00229                     printf("take pictuer!");
00230                    }
00231                  }else{
00232                  printf("take picture failed\r\n");
00233                  }
00234                }else{
00235                  printf("camera is not ready\r\n");
00236                 }
00237                
00238                printf("time = %f\n", timer.read());
00239                
00240        
00241                
00242                xbee.printf("xbee connected!\r\n");
00243                bs = new base64();
00244                bs->Encode("/local/pict000.jpg","/local/data000.txt");
00245                
00246                if((fp=fopen("/local/data000.txt","r"))!=NULL)
00247                {
00248                    pc.printf("ok\r\n");
00249                    while((c=fgetc(fp))!=EOF){
00250                        xbee.printf("%c",c);
00251                     }
00252                     fclose(fp);
00253                 }
00254                 }
00255                 while(1){
00256         
00257                     int received_data = xbee.getc();
00258    
00259                     if (received_data == 82 || received_data == 114){  //Rまたはｒ
00260                         xbee.printf("_________________________________________________________________________________________________________________________________\r\n");
00261                          if((fp=fopen("/local/data000.txt","r"))!=NULL)
00262                          {
00263                             while ((c=fgetc(fp))!=EOF){
00264                                 xbee.printf("%c",c);                    //再送
00265                              }
00266                          fclose(fp);
00267                          }
00268                      }
00269                      else{
00270                          break;
00271                          }
00272                     }
00273      
00274                Sb612switch=0; //焦電off
00275                wait(1);
00276               }else{//焦電反応なしの場合
00277               printf("not found!\r\n");
00278                
00279                
00280                Sb612switch=0;
00281                wait(1);
00282                Ultra=0;
00283                wait(1);
00284                detected=false;
00285                printf("後退\r\n");
00286                left1 = -100; //左モーター-5０％
00287                right1 = -100;//右モーター-5０％
00288                wait(2.0);
00289                left1=-50;
00290                right1=-50;
00291                wait(1);
00292                left1=0;
00293                right1=0;
00294                wait(1);
00295                
00296                printf("右折\n\r");
00297                
00298                left1 = 60; //左モーター１００％
00299                right1 = 100;//右モーター１００％
00300                wait(2.0);
00301               }               
00302       }else{//超音波distance>2000
00303         printf("safety zone\r\n");
00304         Ultra=0;
00305         wait(1);
00306         left1 = 60; //左モーター50％
00307         right1 = 100;//右モーター50％
00308         printf("右折\r\n");
00309         wait(3);
00310         left1 = 100;
00311         right1 = 100;
00312         wait(5);
00313         left1 = 0;
00314         right1 = 0;
00315         wait(5);
00316       }
00317 
00318     }
00319 
00320   }
00321  }