送信のところを変えました。

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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(p25,p17,p16);            //モーターピン
00030 TB6612 right1(p26,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     xbee.printf("you worry about me don't you?\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         xbee.printf("madamada\r\n");
00060         }
00061         else{
00062           myled=0;
00063           xbee.printf("yattar\r\n");
00064           wait(20);
00065           FET=1;
00066           wait(10);
00067           FET=0;
00068           wait(10);
00069           xbee.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");
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()>500){/*500経ったら次に進む*/
00098                 xbee.printf("timeout!!\n");/*xbeeで時間切れの場合はtimeoutと出力する*/
00099                 timeout_flag=1;
00100                 break;
00101             }
00102         }
00103        printf("moter on%lf",a);
00104         left1 = 100; //左モーター100%
00105         right1 = 100;//右モーター100%
00106         
00107         wait(10);
00108         printf("moter off");
00109         left1 = 0; //左モーター0%
00110         right1 = 0;//右モーター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);//動いた後の緯度と経度を表示   
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; //左モーター100%
00164    right1 = 100;//右モーター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                xbee.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                  for (int r = 0; r < 1; r++) {
00221             if (camera.isReady()) {
00222              char filename[25];
00223              snprintf(filename,25,"%s%03d%s","/local/pict",r,".jpg");
00224              printf("Picture: %s ", filename);
00225               if (camera.takePicture(filename)) { 
00226                 while (camera.isProcessing()) {
00227                  camera.processPicture();
00228 
00229                  xbee.printf("take pictuer!");
00230 
00231                  xbee.printf("xbee connected!!\r\n");
00232 
00233                  bs = new base64();
00234                  bs->Encode(filename,"/local/data.txt");
00235                
00236                  printf("time = %f\n", timer.read());
00237 
00238                
00239                   if((fp=fopen("/local/d.txt","r"))!=NULL){
00240                    pc.printf("ok\r\n");
00241                    while((c=fgetc(fp))!=EOF){
00242                      xbee.printf("%c",c);
00243                     }
00244                     fclose(fp);
00245                   }
00246                 }
00247               }else{
00248                 printf("take picture failed\r\n");
00249                   }
00250             }else{
00251               printf("camera is not ready\r\n");
00252                  }
00253             while(1){
00254         
00255               int received_data = xbee.getc();
00256    
00257               if (received_data == 82 || received_data == 114){  //Rまたはr
00258 
00259                 xbee.printf("_________________________________________________________________________________________________________________________________\r\n");
00260                 if((fp=fopen("/local/d.txt","r"))!=NULL) {
00261                   while ((c=fgetc(fp))!=EOF){
00262 
00263                    xbee.printf("%c",c);                    //再送
00264                   }
00265                   fclose(fp);
00266                 }
00267               }else{
00268                 break;
00269               }
00270             }
00271      
00272             Sb612switch=0; //焦電off
00273             wait(1);
00274           }
00275         }else{//焦電反応なしの場合
00276 
00277            
00278               printf("not found!\r\n");
00279                
00280                
00281                Sb612switch=0;
00282                wait(1);
00283                Ultra=0;
00284                wait(1);
00285                detected=false;
00286                printf("後退\r\n");
00287                left1 = -100; //左モーター-50%
00288                right1 = -100;//右モーター-50%
00289                wait(2.0);
00290                left1=-50;
00291                right1=-50;
00292                wait(1);
00293                left1=0;
00294                right1=0;
00295                wait(1);
00296                
00297                printf("右折\n\r");
00298                
00299                left1 = 60; //左モーター100%
00300                right1 = 100;//右モーター100%
00301                wait(2.0);
00302               }               
00303       }else{//超音波distance>2000
00304         printf("safety zone\r\n");
00305         Ultra=0;
00306         wait(1);
00307         left1 = 60; //左モーター50%
00308         right1 = 100;//右モーター50%
00309         printf("右折\r\n");
00310         wait(3);
00311         left1 = 100;
00312         right1 = 100;
00313         wait(5);
00314         left1 = 0;
00315         right1 = 0;
00316         wait(5);
00317       }
00318 
00319     }
00320 
00321   }
00322  }