Diff: Function.h

Revision:

4:135619de1646

Parent:

2:0c73afb20925

--- a/Function.h	Sun Oct 24 14:48:59 2021 +0000
+++ b/Function.h	Mon Oct 25 11:21:06 2021 +0000
@@ -14,6 +14,8 @@
 double CPs_x[100];  // = [];  //CPリスト(x座標)
 double CPs_y[100];  // = [];  // CPリスト(y座標)
 double *pGPS_x = &GPS_x, *pGPS_y = &GPS_y;
+double theta;
+double delta;
 
 int FrontGet() 
 {
@@ -35,7 +37,7 @@
 
 int catchGPS()
 {
-    pc.printf("GPS Start\n");
+    pc.printf("GPS Start\r\n");
     /* 1秒ごとに現在地を取得してターミナル出力 */
     while(1) {
         if(gps.getgps()) {  //現在地取得
@@ -45,7 +47,7 @@
             break;
         }
         else {
-            pc.printf("No data\n");//データ取得に失敗した場合
+            pc.printf("No data\r\n");//データ取得に失敗した場合
             wait(1);
         }
     }
@@ -53,29 +55,58 @@
 }
 
 
-int AngleGet() 
+double 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);
-        }
+    double next_x;
+    double next_y;
+    double theta;
+    double delta;
+    //if(gps.getgps()) {
+    pc.printf("%f, %f\r\n", 0, 0);  //gps.latitude=0, gps.longitude=0
+    next_x = 0;
+    next_y = 100;
+    theta = atan(next_x - 0 / next_y - 0) * 180 / M_PI;
+    printf("theta=%f\r\n", theta);
+    if(theta >= 0) {
+        delta = angle - theta;
+    }
+    else {
+        theta = theta + 360;
+        delta = angle - theta;
+    }
+    printf("%f-%f=%f\r\n", angle, theta, delta); 
+    //}
+    /*
+    else {
+            pc.printf("No data\r\n");//データ取得に失敗した場合
+            wait(1);
+    }
+    */
     wait(2);
+    return delta;
+}
+/*
+    double angle;
+    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    angle = compass.sample() / 10;
+            
+    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); 
+    return delta;
     }
-}
\ No newline at end of file
+    else {
+            pc.printf("No data\r\n");//データ取得に失敗した場合
+            wait(1);
+    }
+}
+*/
\ No newline at end of file