Diff: main.cpp

Revision:

2:e2b803e3bcbc

Parent:

1:edc264954800

Child:

3:5d0c4b13f4e8

--- a/main.cpp	Fri Nov 01 15:02:54 2019 +0000
+++ b/main.cpp	Fri Nov 08 16:16:43 2019 +0000
@@ -1,56 +1,122 @@
 #define cansatB
-#include “mbed.h”        //mbed
-#include “getgps.h”         //GPS
-#include “motordriver.h” //モータードライバ
-#include “XBee.h”        //XBee
+#include "mbed.h"       //mbed
+#include "getGPS.h"         //GPS
+#include "math.h"         //GPS
+#include "XBee.h"        //XBee
 #include <SoftwareSerial.h>   //カメラ
 #include <SD.h>//SDカード
 #include <JPEGCamera.h>//カメラ
-#include “US015.h”  // 超音波センサ
-#include "sb612a.h"//焦電センサー
+#include "us015.h"  // 超音波センサ
 #include <stdio.h>
-#include <wiringPi.h>
-#include <mcp23s17.h>
-#include <sys/time.h>
-#include <unistd.h>
-DigitalOut USSTriger (p12);         //P12 :超音波センサ トリガ出力
-InterruptIn USSEcho (p11);          //p11 :超音波センサ  エコー入力
-Serial pc(USBTX, USBRX);            //GPS
-GPS gps(p28, p27);           //GPS
+US015 hs(p12,p11);                  //P12 :超音波センサ トリガ出力  //p11 :超音波センサ  エコー入力
+Serial pc(USBTX,USBRX);            //GPs
+GPS gps (p28,p27);                 //GPS
 PwmOut motorSpeedR(p26);  //モーター
 PwmOut motorSpeedL(p25);  //モーター
 DigitalIn flight(p22,p23);      //フライトピン
-DigitalOut FET(p21);        //FET
-Timer ActiveTime;
+DigitalOut FET(p21);                //FET
 XBee xbee(p13, p14);               // XBee
-AnalogIn  (p15,p16); //焦電センサ
+DigitalIn thermo(p20);     //焦電センサ↓
+DigitalOut led(LED1);     
+Serial pc(USBTX, USBRX); // tx, rx  焦電センサ↑
 Serial pc (p9);     //カメラ
 //Motor motor1(p22, p16, p17, 1); // pwm, fwd, rev, can brake     モーター
 //Motor motor2(p22, p19, p20, 1); // pwm, fwd, rev, can brake
-void dist(int distance)
-{
-   //put code here to happen when the distance is changed
-   printf(“Distance changed to %dmm\r\n”, distance);
-}
-  printf(“GPS start\r\n”);//GPS 第一回目
-  FILE *fp = fopen(“/local/gps,foto.txt”, “w”);  // Open “gps.txt” on the local file system for writing
-   fprintf(fp, “GPS Start\r\n”);
-   int n;
-   for(n=0;n<45;n++)  //GPSの取得を45回行う(これで大体1分半)
-   {
-       printf(“gps for\r\n”);
-       if(gps.getgps()) //現在地取得
-           fprintf(fp,“(%lf, %lf)\r\n”, gps.latitude, gps.longitude);//緯度と経度を出力
-       else
-           fprintf(fp,“No data\r\n”);//データ取得に失敗した場合
-       wait(1);
-       printf(“%d\r\n”,n);              //今何回目かを出力（本番ではいらない）
-   }
-   fprintf(fp,“GPS finish\r\n”);
-  // fclose(fp);                     // GPSの測定終了   */
-US015 mu(p11, p12, .1, 1, &dist);    //Set the trigger pin to D8 and the echo pin to D9              超音波センサー
-                                         //have updates every .1 seconds and a timeout after 1
-                                         //second, and call dist when the distance changes
+
+int main() {            //FET
+    FET = 0;
+    SW = 1;  //p21をhigh(3.3V)にする。 
+     while(1) {
+    if(flight==1) {
+        wait(10);
+        }
+        
+    else{
+        if(flight==1) {
+        wait(10);
+        }
+        else{
+        FET = 0;                       //FET、ニクロム線切断
+        wait(20);
+        FET = 1;
+        wait(12);
+        FET = 0;
+        wait(10);
+        FET = 1;
+        wait(12);
+        FET = 0; 
+        SW = 0;  //p21をlow(0V)にする。 
+    break;
+            }
+    }
+    }
+    }
+
+int main() {       //以下GPS
+     double a;
+     double b;
+     double distance;
+    
+    pc.printf("GPS Start\r\n");
+    
+     while(1) {
+         if(gps.getgps()){
+           a = gps.latitude;
+           b = gps.longitude;
+           
+          pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示
+           
+           break;
+           
+         }else{
+          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;
+
+         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 e = sin(ra) * sin(rc) +  cos(ra) * cos(rc) * cos(rb - rd);  // 2点の中心角(ラジアン)を求める
+            double rr = acos(e);
+
+            const double earth_radius = 6378140;   // 地球赤道半径(m)
+
+            distance = earth_radius * rr; // 2点間の距離(m)
+            
+           
+             
+
+         if (distance<5){
+             printf("%lf\r\n",distance);   
+             }else{
+             pc.printf("5m clear!");
+             break;
+              }
+          
+          }else{
+           pc.printf("NO DATA\r\n");//データ取得失敗
+           wait(1);
+           }
+        }
+        return 0;          //注意！void()に変えること.このままだとここで終わる
+     }
+      
+
 void motorForwardR(void);
 void motorStopR(void);
 void motorReverseR(void);
@@ -59,37 +125,7 @@
 void motorReverseL(void);
 LocalFileSystem local(“local”);               // Create the local filesystem under the name “local”　　 データ保存
 float culculate_distance_3(float a,float b);
-int main() {
-   printf(“cansat start\r\n”);
-   flight==1;
-   FET = 0;
-   SW = 1;  //p23をhigh(3.3V)にする。
-    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;                       //FET、ニクロム線切断
-       wait(20);
-       FET = 1;
-       wait(12);
-       FET = 0;
-       wait(10);
-       FET = 1;
-       wait(12);
-       FET = 0;
-       SW = 0;                         //p23をlow(0V)にする。
-   break;
-           }
-   }
-   }
+
    motorStopR();
    motorStopL();
    wait(20);     //確認用//デフォ20秒
@@ -112,24 +148,8 @@
    wait(1);
    printf(“compass carriblation\r\n”); //キャリブレーション終了
 //float mcn1,mcn2;
-  printf(“GPS start\r\n”);//GPS第２回目（移動後）
-  FILE *fp = fopen(“/local/gps,foto.txt”, “w”);  // Open “gps.txt” on the local file system for writing
-   fprintf(fp, “GPS Start\r\n”);
-   int n;
-   for(n=0;n<45;n++)  //GPSの取得を45回行う(これで大体1分半)
-   {
-       printf(“gps for\r\n”);
-       if(gps.getgps()) //現在地取得
-           fprintf(fp,“(%lf, %lf)\r\n”, gps.latitude, gps.longitude);//緯度と経度を出力
-       else
-           fprintf(fp,“No data\r\n”);//データ取得に失敗した場合
-       wait(1);
-       printf(“%d\r\n”,n);              //今何回目かを出力（本番ではいらない）
-   }
-   fprintf(fp,“GPS finish\r\n”);
-  // fclose(fp);                     // GPSの測定終了   */
 
-if(distance<4000){
+if(distance<2000){
     motorStopR();
     motorStopL();
     stopUS015();
@@ -138,6 +158,30 @@
 //停止
 //超音波センサーOFF
 //焦電センサーON
+  int main()
+{
+  float th;
+  Timer tm;
+  pc.printf("start\r\n");
+  led=0;
+  bool detected=false;
+  while(true)
+  {
+    th = thermo;
+    if(th==1 && !detected) {
+      led = 1;
+      detected=true;
+      pc.printf("human\r\n");
+      tm.reset();
+      tm.start();
+    }
+    if(tm.read_ms()>10000) {
+      printf("Time out!\r\n");
+      led = 0;
+      detected=false;
+    }
+  }
+}
    if(detected=true){
        stopsb612a();
        startCamera();
@@ -175,7 +219,7 @@
     //超音波センサーON
     //直進
 }
-else if(distance>=4000){
+else if(distance>=2000){
     motorStopL();
     motorStopR();
     motorForwardL();