Diff: Function.h

Revision:

2:0c73afb20925

Child:

4:135619de1646

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Function.h	Sun Oct 24 14:46:02 2021 +0000
@@ -0,0 +1,81 @@
+#include "mbed.h"
+#include "getGPS.h"
+#include "us015.h"
+#include "HMC6352.h"
+#include "Math.h"
+
+US015 hs(D2, D3);
+DigitalOut Ultra(D2);
+GPS gps(D1, D0);
+HMC6352 compass(D4, D5);
+Serial pc(USBTX, USBRX);
+double GPS_x, GPS_y;  // 現在地の座標
+double next_CP_x, next_CP_y;
+double CPs_x[100];  // = [];  //CPリスト(x座標)
+double CPs_y[100];  // = [];  // CPリスト(y座標)
+double *pGPS_x = &GPS_x, *pGPS_y = &GPS_y;
+
+int FrontGet() 
+{
+        Ultra = 1;  //超音波on
+        hs.TrigerOut();
+        wait(1);
+        int distance;
+        distance = hs.GetDistance();
+        pc.printf("distance=%d\r\n", distance);  //距離出力
+        Ultra=0;//超音波off
+        
+        if(distance < 200) {
+            return 1;
+        }
+        else {
+            return 0;
+        }
+}
+
+int catchGPS()
+{
+    pc.printf("GPS Start\n");
+    /* 1秒ごとに現在地を取得してターミナル出力 */
+    while(1) {
+        if(gps.getgps()) {  //現在地取得
+            pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
+            *pGPS_x = gps.latitude;
+            *pGPS_y = gps.longitude;
+            break;
+        }
+        else {
+            pc.printf("No data\n");//データ取得に失敗した場合
+            wait(1);
+        }
+    }
+    return 0;
+}
+
+
+int AngleGet() 
+{
+    while(1) {
+    double angle;
+    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    angle = compass.sample() / 10;
+
+        double theta;
+        double delta;
+        
+        if(gps.getgps()) {
+        pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);
+        next_CP_x = gps.latitude;
+        next_CP_y = gps.longitude;
+        theta = atan(next_CP_x - 0 / next_CP_y - 0) * 180 / M_PI;
+        printf("%f", theta);
+        delta = theta - angle;
+        printf("%f-%f=%f\r\n", theta, angle, delta); 
+        }
+        else {
+                pc.printf("No data\r\n");//データ取得に失敗した場合
+                wait(1);
+        }
+    wait(2);
+    }
+}
\ No newline at end of file