Diff: main.cpp

Revision:

7:fae874e898b3

Parent:

6:db1a62608047

Child:

8:765a73e21907

--- a/main.cpp	Fri Nov 22 08:52:55 2019 +0000
+++ b/main.cpp	Wed Dec 04 11:49:44 2019 +0000
@@ -1,38 +1,56 @@
 #define cansatB
-#define min(x, y) ((x) < (y)) ? (x) : (y)
-#include "mbed.h"       //mbed
-#include "getGPS.h"         //GPS
-#include "math.h"         //GPS
-#include "TB6612.h"      //motorDriver
-#include "JPEGCamera.h"//カメラ
-#include "base64.h"//写真ーXBee
-#include "us015.h"  // 超音波センサ
+#include "mbed.h" 
+//mbed
+#include "getGPS.h"
+#include "math.h"
+//GPS
+#include "TB6612.h"
+ //motorDriver
+ #include "JPEGCamera.h"
+//カメラ
 #include <stdio.h>
-US015 hs(p12,p11);                  //P12 :超音波センサ トリガ出力  //p11 :超音波センサ  エコー入力
+#include <base64.h>
+//XBee
+#include "us015.h"
+// 超音波センサ
+
+
 GPS gps (p28,p27);                 //GPS
 
 DigitalOut FET(p21);                //FET
+DigitalOut myled(LED1);
+US015 hs(p12,p11);
+DigitalIn thermo(p20);
+DigitalOut Sb612switch(p15);         //焦電スイッチ
 DigitalOut Ultra(p12);
-DigitalOut thermo(p20);     //焦電センサ↓
-DigitalOut led(LED1);     
-Serial pc(USBTX,USBRX); // tx, rx  焦電センサ↑
-Serial xbee(p13, p14);
-TB6612 left(p25,p17,p16);//motor
-TB6612 right(p26,p19,p18);//motor
+Serial pc(USBTX,USBRX);              // tx, rx
+JPEGCamera camera(p9, p10);          // TX, RX
+TB6612 left1(p25,p17,p16);            //モーターピン
+TB6612 right1(p26,p19,p18);           //モーターピン
+Serial xbee(p13,p14);                //xbee
 
-void JPEGCamera(void);
-void us015(void);
-void motorFoward(void);
-void motorFowardR(void);
-void motorForwardL(void);
-void motorStop(void);
-void motorReverse(void);  
-    LocalFileSystem local("local");                                       
+
+                                    
 
-int main() {   
+int main()
+{
+    
+    
+    Sb612switch=0; //焦電off
+    wait(1);
+    Ultra=0;//超音波off
+       
     printf("CanSat-B_Start!\r\n");
     
-    printf("FETのコピーはここ");
+     //FET
+    
+    FET=1;
+    wait(10);
+    FET=0;
+    wait(10);
+    
+    
+    
       //以下GPS
      double a;
      double b;
@@ -40,8 +58,10 @@
     
     pc.printf("GPS Start\r\n");
     
-     while(1) {
-         if(gps.getgps()){
+     while(1)
+     {
+         if(gps.getgps())
+         {
            a = gps.latitude;
            b = gps.longitude;
            
@@ -50,304 +70,203 @@
            break;
            
          }else{
-          pc.printf("NO DATA\r\n");//データ取得失敗
-          wait(1);
-            }
-       }
-      while(1){
+               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;
+          // 球面三角法により、大円距離(メートル)を求める
+          double c;
+          double d; 
+          c = gps.latitude;
+          d = gps.longitude;
 
-         const double pi = 3.14159265359; // 円周率
+          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);   
-             motorFoward();
+             right1=100;
+             left1=100;
              }else{
-             motorStop();
+             right1=0;
+             left1=0;
              pc.printf("5m clear!");
              break;
-              }
+                  }
           
-          }else{
+             }else{
            pc.printf("NO DATA\r\n");//データ取得失敗
            wait(1);
            }
         }
         //GPS End     
 
+ int i=1;
   float th;
   Timer tm;
+  for(i=0;i<3;i++){
   pc.printf("start\r\n");
   
-  bool detected=false;
-    thermo=0; //焦電off
+  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
-         
-         us015();
-        if(distance<2000){//超音波反応
-         Ultra=0;//超音波off
-         thermo=1;//焦電on
-         motorStop();
-        
-          if(true)
-             th = thermo;
-             if(th=1 && !detected) {//焦電反応ありの場合
-               detected=true;
-             pc.printf("human\r\n");
-             tm.reset();
-             tm.start();
-             thermo=0;
-    JPEGCamera();
-
-    FILE *fp;
-    base64 *bs;
-    int c;
-    
-    xbee.printf("charizard!!!\r\n");
-    bs = new base64();
-    bs->Encode("/local/PICT000.jpg","/local/d.txt");
-    
-    
-   if((fp=fopen("/local/d.txt","r"))!=NULL)
-   {
-       while ((c=fgetc(fp))!=EOF){
-           xbee.printf("%c",c);
-       }
-       fclose(fp);
-   }
-
-
-            }else{//焦電反応なしの場合
-             detected=false;
-         thermo=0;
-         Ultra=1;
-             }
-           
-
-  //while(true)
-  
-  
-//超音波センサー反応あり
-//停止
-//超音波センサーOFF
-//焦電センサーON
-       
-   //焦電センサー反応あり
-   //焦電センサーOFF
-   //カメラ起動
-   //カメラOFF
-   //XBeeによりPCへ送信
-     printf("OK1はここに\r\n");
-       //"OK"受信、ミッション終了
-      printf("OK2はここに\r\n");
-       //"NO"受信、ミッション再開
-
-     printf("NOはここに");
+    wait(1);
     
-    //焦電センサー反応無し
-    //焦電センサーOFF
-    //方向転換
-    //超音波センサーON
-    //直進
-}
-else if(distance>=2000){
-    motorStop();
-    wait(2);
-    motorFowardR();
-    wait(3);
-}
-//超音波センサー反応無し
-//停止
-//方向転換
-//直進
-}
-
-
-void JPEGCamera(){
-           const int RESPONSE_TIMEOUT = 500;
-const int DATA_TIMEOUT = 1000;
-
-JPEGCamera::JPEGCamera(PinName tx, PinName rx) : Serial(tx, rx) {
-    printf("AA\r\n");
-    baud(38400);
-    state = READY;
-}
-
-bool JPEGCamera::setPictureSize(JPEGCamera::PictureSize size, bool doReset) {
-    char buf[9] = {0x56, 0x00, 0x31, 0x05, 0x04, 0x01, 0x00, 0x19, (char) size};
-    int ret = sendReceive(buf, sizeof buf, 5);
-
-    if (ret == 5 && buf[0] == 0x76) {
-        if (doReset)
-            reset();
-        return true;
-    } else
-        return false;
-}
-
-bool JPEGCamera::isReady() {
-    return state == READY;
-}
-
-bool JPEGCamera::isProcessing() {
-    return state == PROCESSING;
-}
-
-bool JPEGCamera::takePicture(char *filename) {
-    if (state == READY) {
-        fp = fopen(filename, "wb");
-        if (fp != 0) {
-            if (takePicture()) {
-                imageSize = getImageSize();
-                address = 0;
-                state = PROCESSING;
-            } else {
-                fclose(fp);
-                printf("takePicture(%s) failed", filename);
-                state = ERROR;
-            }
-        } else {
-            printf("fopen() failed");
-            state = ERROR;
-        }
-    }
-    return state != ERROR;
-}
-
-bool JPEGCamera::processPicture() {
-    if (state == PROCESSING) {
-        if (address < imageSize) {
-            char data[1024];
-            int size = readData(data, min(sizeof(data), imageSize - address), address);
-            int ret = fwrite(data, size, 1, fp);
-            if (ret > 0)
-                address += size;
-            if (ret == 0 || address >= imageSize) {
-                stopPictures();
-                fclose(fp);
-                wait(0.1); // ????
-                state = ret > 0 ? READY : ERROR;
-            }
-        }
-    }
-
-    return state == PROCESSING || state == READY;
-}
-
-bool JPEGCamera::reset() {
-    char buf[4] = {0x56, 0x00, 0x26, 0x00};
-    int ret = sendReceive(buf, sizeof buf, 4);
-    if (ret == 4 && buf[0] == 0x76) {
-        wait(4.0);
-        state = READY;
-    } else {
-        state = ERROR;
-    }
-    return state == READY;
-}
-
-bool JPEGCamera::takePicture() {
-    char buf[5] = {0x56, 0x00, 0x36, 0x01, 0x00};
-    int ret = sendReceive(buf, sizeof buf, 5);
-
-    return ret == 5 && buf[0] == 0x76;
-}
-
-bool JPEGCamera::stopPictures() {
-    char buf[5] = {0x56, 0x00, 0x36, 0x01, 0x03};
-    int ret = sendReceive(buf, sizeof buf, 5);
-
-    return ret == 4 && buf[0] == 0x76;
-}
-
-int JPEGCamera::getImageSize() {
-    char buf[9] = {0x56, 0x00, 0x34, 0x01, 0x00};
-    int ret = sendReceive(buf, sizeof buf, 9);
-
-    //The size is in the last 2 characters of the response.
-    return (ret == 9 && buf[0] == 0x76) ? (buf[7] << 8 | buf[8]) : 0;
-}
-
-int JPEGCamera::readData(char *dataBuf, int size, int address) {
-    char buf[16] = {0x56, 0x00, 0x32, 0x0C, 0x00, 0x0A, 0x00, 0x00,
-                    address >> 8, address & 255, 0x00, 0x00, size >> 8, size & 255, 0x00, 0x0A
-                   };
-    int ret = sendReceive(buf, sizeof buf, 5);
-
-    return (ret == 5 && buf[0] == 0x76) ? receive(dataBuf, size, DATA_TIMEOUT) : 0;
-}
-
-int JPEGCamera::sendReceive(char *buf, int sendSize, int receiveSize) {
-    while (readable()) getc();
-
-    for (int i = 0; i < sendSize; i++) putc(buf[i]);
-
-    return receive(buf, receiveSize, RESPONSE_TIMEOUT);
-}
-
-int JPEGCamera::receive(char *buf, int size, int timeout) {
-    timer.start();
-    timer.reset();
-
-    int i = 0;
-    while (i < size && timer.read_ms() < timeout) {
-        if (readable())
-            buf[i++] = getc();
-    }
-
-    return i;
-}
-void us015(){
-    hs.TrigerOut();
+    while(1) {
+         printf("超音波on\r\n 焦電off\r\n" )  ;
+         hs.TrigerOut();
          wait(1);
          int distance;
          distance = hs.GetDistance();
-         printf("%d\r\n",distance);//距離出力
+         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[32];
+                 sprintf(filename, "/local/pict%03d.jpg",r);
+                 printf("Picture: %s ", filename);
+                 if (camera.takePicture(filename)) { 
+                  while (camera.isProcessing()) {
+                    camera.processPicture();
+                    printf("take pictuer!");
+                   }
+                 }else{
+                 printf("take picture failed\r\n");
+                 }
+               }else{
+                 printf("camera is not ready\r\n");
+                }
+               
+               printf("time = %f\n", timer.read());
+               
+       
+               
+               xbee.printf("xbee connected!\r\n");
+               bs = new base64();
+               bs->Encode("/local/pict000.jpg","/local/data000.txt");
+               
+               if((fp=fopen("/local/data000.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");
          }
 
-void motorFoward(){
-    left = 100; 
-        right = 100;
-        printf("直進\n\r");
     }
-void motorFowardR(){
-    left = 100; 
-        right = 20;
-        printf("右折\n\r");
-    }
-void motorForwardL(){
-    left = 20; 
-        right = 100;
-        printf("左折\n\r");}
-void motorStop(){
-    left = 0; 
-        right = 0;
-        printf("停止\n\r");}
-void motorReverse(){
-    left = -50;
-        right = -50;
-        printf("バック\n\r");
-        }   
\ No newline at end of file
+
+ }
+}