Diff: Function.h

Revision:

16:b5a60a976bf7

Parent:

15:4779723a4f75

Child:

17:1b6a84ab4433

--- a/Function.h	Wed Oct 27 10:17:59 2021 +0000
+++ b/Function.h	Wed Oct 27 10:48:07 2021 +0000
@@ -120,15 +120,11 @@
     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
     angle = compass.sample() / 10;
 
-    double next_x;
-    double next_y;
     double theta;
     double delta;
 
-    pc.printf("%f, %f\r\n", 0, 0);  //gps.latitude=0, gps.longitude=0
-    next_x = 0;
-    next_y = 100;
-    theta = atan2(next_x - 0 , next_y - 0) * 180 / M_PI;
+    pc.printf("%f, %f\r\n", gps.latitude, gps.longitude);
+    theta = atan2(next_CP_x - gps.latitude , next_CP_y - gps.longitude) * 180 / M_PI;
     printf("theta=%f\r\n", theta);
     if(theta >= 0) {
         delta = angle - theta;
@@ -141,24 +137,3 @@
     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;
-    }
-    else {
-            pc.printf("No data\r\n");//データ取得に失敗した場合
-            wait(1);
-    }
-}
-*/
\ No newline at end of file