Diff: main.cpp

Revision:

10:6e02396abaf5

Parent:

9:435ce4946f6c

Child:

11:6f553aa95d08

diff -r 435ce4946f6c -r 6e02396abaf5 main.cpp
--- a/main.cpp	Fri Dec 06 09:09:42 2019 +0000
+++ b/main.cpp	Fri Dec 13 11:25:52 2019 +0000
@@ -28,248 +28,287 @@
 TB6612 left1(p25,p17,p16);            //モーターピン
 TB6612 right1(p26,p19,p18);           //モーターピン
 Serial xbee(p13,p14);                //xbee
-
-
-                                    
+DigitalIn flight(p23);      //フライトピン(in)
+DigitalOut SW(p24);         //フライトピン（out）           
 
 int main()
 {
     
     
-    Sb612switch=0; //焦電off
+   /* Sb612switch=0; //焦電off
     wait(1);
     Ultra=0;//超音波off
-       
+    SW=1;
+    flight==1;//flight pin ついてる
+    FET=1;//FET off   
     printf("CanSat-B_Start!\r\n");
-    
+
      //FET
-    
-    FET=1;
-    wait(10);
-    FET=0;
-    wait(10);
-    
+     while(1){
+     if(flight==1) {
+        wait(10);
+        printf("mada\r\n");
+        }
+     else{
+        if(flight==1) {
+        wait(10);
+        printf("madamada\r\n");
+        }
+        else{
+          printf("yattar\r\n");
+          FET=0;
+          wait(10);
+          FET=1;
+          wait(10);
+          printf("FET End!\r\n");
+          SW=0;
+        
+    break;
+            }
+    }   
+    }
     
     
       //以下GPS
      double a;
      double b;
      double distance;
-    
-    pc.printf("GPS Start\r\n");
-    
-     while(1)
-     {
-         if(gps.getgps())
-         {
+     int k = 0;
+     int j = 0;
+     
+     pc.printf("GPS Start\n");
+     xbee.printf("s\n");
+     while(1){
+         if(gps.getgps()){
+           
+          pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示   
+          k ++;
+          if(k<59){
+           }else{
            a = gps.latitude;
            b = gps.longitude;
-          pc.printf("位置情報取得成功！\r\n"); 
-          pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示
-          
+           printf("(a =%lf,b =%lf)\r\n",gps.latitude,gps.longitude);//a,bを表示   
            break;
-           
+           }
          }else{
-               pc.printf("NO DATA\r\n");//データ取得失敗
-               wait(1);
-               }
-      }
-
-      while(1)
-      { printf("移動距離測定開始\r\n");
-        left1=100;
-        right1=100;
-        wait(5);
-        left1=0;
-        right1=0;
-        wait(3);
-         if(gps.getgps()) {
+          pc.printf("NO DATA\r\n");//データ取得失敗
+          wait(1);
+            }
+       }
+       printf("moter on");
+        left1 = 100; //左モーター１００％
+        right1 = 100;//右モーター１００％
+        
+        wait(30);
+        printf("moter off");
+        left1 = 0; //左モーター０％
+        right1 = 0;//右モーター０％
+        printf("moter off");
+        wait(15);
+        printf("GPS restart\n");
+        
+        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;
+          pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示   
+          j ++;
+          if(j<29){
+           }else{
+            // 球面三角法により、大円距離(メートル)を求める
+           double c;
+           double d; 
+           c = gps.latitude;
+           d = gps.longitude;
 
-          const double pi = 3.14159265359; // 円周率
+            pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示   
+            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 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);
+            double e = sin(ra) * sin(rc) +  cos(ra) * cos(rc) * cos(rb - rd);  // 2点の中心角(ラジアン)を求める
+            double rr = acos(e);
 
-          const double earth_radius = 6378140;   // 地球赤道半径(m)
+            const double earth_radius = 6378140;   // 地球赤道半径(m)
 
-          distance = earth_radius * rr; // 2点間の距離(m)
+            distance = earth_radius * rr; // 2点間の距離(m)
             
-           
-             
-
-         if (distance<5){
-             printf("走行距離=%lf\r\n",distance);   
+            if (distance<5){
              }else{
+             pc.printf("(c =%lf,d =%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示   
              pc.printf("5m clear!");
-             break;
-                  }
-          
-             }else{
-           pc.printf("NO DATA\r\n");//データ取得失敗
-           wait(1);
-           }
-        }
-        //GPS End     
+             xbee.printf("5m clear!");
+              break;
+           }  
+            }
+       }else{
+         printf("No data");
+          }
+        } //GPS End */    
 
  int i=1;
-  float th;
-  Timer tm;
-  for(i=0;i<3;i++){
-  pc.printf("start\r\n");
+ float th;
+ Timer tm;
+ for(i=0;i<3;i++){
+   pc.printf("start\r\n");
   
-  left1 = 100; //左モーター１００％
-  right1 = 100;//右モーター１００％
-    printf("Restart\r\n" );
-         wait(4);
-         left1=50;
-         right1=50;
+   left1 = 100; //左モーター１００％
+   right1 = 100;//右モーター１００％
+
+   printf("Restart\r\n" );
+
+   wait(4);
+   left1=50;
+   right1=50;
+   wait(1);
+   left1=0;
+   right1=0;
+   wait(1);
+
+   printf("停止\n\r");
+
+   Sb612switch=0; //焦電off
+   wait(1);
+   Ultra=1;//超音波on
+   wait(1);
+    
+    while(1) {
+      left1 = 0;
+      right1 = 0;
+
+      printf("超音波on\r\n 焦電off\r\n" )  ;
+
+      hs.TrigerOut();
+      wait(1);
+      int distance;
+      distance = hs.GetDistance();
+
+      printf("distance=%dmm\r\n",distance);//距離出力
+        
+      if(distance<=2000){//超音波反応
+       Ultra=0;//超音波off
+       wait(1);
+       Sb612switch=1; //焦電on
+       wait(1);
+
+       printf("焦電On!\r\n  ");
+
+       bool detected=false;
+       th = thermo;
+
+       if(th==1 && !detected) {//焦電反応ありの場合
+         i++; 
+         detected=true;
+         pc.printf("human\r\n");
+         tm.reset();
+         tm.start();
+           
+         LocalFileSystem local("local");
+         Timer timer;
+         timer.start();
+         camera.setPictureSize(JPEGCamera::SIZE320x240);
+               
+         FILE *fp;
+         base64 *bs;
+         int c;
+
+          for (int r = 0; r < 1; r++) {
+            if (camera.isReady()) {
+             char filename[25];
+             snprintf(filename,25,"%s%03d%s","/local/pict",r,".jpg");
+             printf("Picture: %s ", filename);
+              if (camera.takePicture(filename)) { 
+                while (camera.isProcessing()) {
+                 camera.processPicture();
+
+                 printf("take pictuer!");
+
+                 xbee.printf("xbee connected!!\r\n");
+
+                 bs = new base64();
+                 bs->Encode(filename,"/local/data.txt");
+               
+                 printf("time = %f\n", timer.read());
+
+               
+                  if((fp=fopen("/local/d.txt","r"))!=NULL){
+                   pc.printf("ok\r\n");
+                   while((c=fgetc(fp))!=EOF){
+                     xbee.printf("%c",c);
+                    }
+                    fclose(fp);
+                  }
+                }
+              }else{
+                printf("take picture failed\r\n");
+                  }
+            }else{
+              printf("camera is not ready\r\n");
+                 }
+            while(1){
+        
+              int received_data = xbee.getc();
+   
+              if (received_data == 82 || received_data == 114){  //Rまたはｒ
+
+                xbee.printf("_________________________________________________________________________________________________________________________________\r\n");
+                if((fp=fopen("/local/d.txt","r"))!=NULL) {
+                  while ((c=fgetc(fp))!=EOF){
+
+                   xbee.printf("%c",c);                    //再送
+                  }
+                  fclose(fp);
+                }
+              }else{
+                break;
+              }
+            }
+     
+            Sb612switch=0; //焦電off
+            wait(1);
+          }
+        }else{//焦電反応なしの場合
+         printf("not found!\r\n");
+         Sb612switch=0;
+         wait(1);
+         Ultra=0;
+         wait(1);
+         detected=false;
+         printf("後退\r\n");
+         left1 = -100; //左モーター-50％
+         right1 = -100;//右モーター-50％
+         wait(2.0);
+         left1=-50;
+         right1=-50;
          wait(1);
          left1=0;
          right1=0;
          wait(1);
-          printf("停止\n\r");
-    Sb612switch=0; //焦電off
-    wait(1);
-    Ultra=1;//超音波on
-    wait(1);
-    
-    while(1) {
-         printf("超音波on\r\n 焦電off\r\n" )  ;
-         hs.TrigerOut();
-         wait(1);
-         int distance;
-         distance = hs.GetDistance();
-         printf("distance=%dmm\r\n",distance);//距離出力
-        
-        if(distance<=2000){//超音波反応
-         
-         Ultra=0;//超音波off
-         wait(1);
-         Sb612switch=1; //焦電on
-         wait(1);
-          printf("焦電On!\r\n  ");
-           bool detected=false;
-             th = thermo;
-             if(th==1 && !detected) {//焦電反応ありの場合
-               i++; 
-               detected=true;
-               pc.printf("human\r\n");
-               tm.reset();
-               tm.start();
-          
-               LocalFileSystem local("local");
-               Timer timer;
-               timer.start();
-               camera.setPictureSize(JPEGCamera::SIZE320x240);
                
-               FILE *fp;
-               base64 *bs;
-               int c;
-
-            for (int r = 0; r < 1; r++) {
-               if (camera.isReady()) {
-                 
-                 char filename[25];
-                 snprintf(filename,25,"%s%03d%s", "/local/pict",r,".jpg");
-                 printf("Picture: %s ", filename);
-                 if (camera.takePicture(filename)) { 
-                  while (camera.isProcessing()) {
-                    camera.processPicture();
-                    printf("take picture!");
-                    
-                    xbee.printf("xbee connnected!\r\n");
-                    bs = new base64();
-                    bs->Encode(filename,"/local/data.txt");
-                   }
-                 }else{
-                 printf("take picture failed\r\n");
-                 }
-               }else{
-                 printf("camera is not ready\r\n");
-                }
-               
-               printf("time = %f\n", timer.read());
-            
-               
-               if((fp=fopen("/local/data.txt","r"))!=NULL)
-               {
-                   pc.printf("ok\r\n");
-                   while((c=fgetc(fp))!=EOF){
-                       xbee.printf("%c",c);
-                    }
-                    fclose(fp);
-                }
-                }
-                while(1){
-        
-                    int received_data = xbee.getc();
-   
-                    if (received_data == 82 || received_data == 114){  //Rまたはｒ
-                        xbee.printf("_________________________________________________________________________________________________________________________________\r\n");
-                         if((fp=fopen("/local/data000.txt","r"))!=NULL)
-                         {
-                            while ((c=fgetc(fp))!=EOF){
-                                xbee.printf("%c",c);                    //再送
-                             }
-                         fclose(fp);
-                         }
-                     }
-                     else{
-                         break;
-                         }
-                    }
-     
-               Sb612switch=0; //焦電off
-               wait(1);
-              }else{//焦電反応なしの場合
-              printf("not found!\r\n");
-               
-               
-               Sb612switch=0;
-               wait(1);
-               Ultra=0;
-               wait(1);
-               detected=false;
-               printf("後退\r\n");
-               left1 = -100; //左モーター-5０％
-               right1 = -100;//右モーター-5０％
-               wait(2.0);
-               left1=-50;
-               right1=-50;
-               wait(1);
-               left1=0;
-               right1=0;
-               wait(1);
-               
-               printf("右折\n\r");
-               
-               left1 = 60; //左モーター１００％
-               right1 = 100;//右モーター１００％
-               wait(2.0);
-              }           
-       }else{//超音波distance>2000
-         printf("safety zone\r\n");
-          Ultra=0;
-          wait(1);
-          left1 = 60; //左モーター50％
-          right1 = 100;//右モーター50％
-          printf("右折\r\n");
-         }
+         printf("右折\n\r");
+         left1 = 60; //左モーター100％
+         right1 = 100;//右モーター100％
+         wait(2.0);
+        }           
+      }else{//超音波distance>2000
+        printf("safety zone\r\n");
+        Ultra=0;
+        wait(1);
+        left1 = 60; //左モーター50％
+        right1 = 100;//右モーター50％
+        printf("右折\r\n");
+        wait(3);
+        left1 = 100;
+        right1 = 100;
+        wait(5);
+        left1 = 0;
+        right1 = 0;
+        wait(5);
+      }
 
     }
 
+  }
  }
-}