Diff: main.cpp

Revision:

3:b4f715167119

Parent:

2:591877e2c7d1

Child:

4:fca767ac5f69

--- a/main.cpp	Wed Oct 20 02:55:52 2021 +0000
+++ b/main.cpp	Wed Oct 20 07:34:03 2021 +0000
@@ -38,10 +38,11 @@
     double x, y;
     double m, n;
     double i,w;
+    double j,k,l;
     gps.getgps();
     x = gps.latitude; //経度の初期値
     y = gps.longitude; //緯度の初期値
-    while(1){
+    while(j>5){
         driveMotor(1,1);
         wait(30);
         driveMotor(0,0);
@@ -51,6 +52,9 @@
         y=n-y;
         double a[2]={x,y};
         double b[2]={0-m,0-n}; //目的地の座標を(0,0)とした
+        k=b[0]/0.000008983148616;
+        l=b[1]/0.000010966382364;
+        j=pow(k*k+l*l,0.5);
         double p=a[0]*a[0]+a[1]*a[1];
         double q=b[0]*b[0]+b[1]*b[1];
         double pp=pow(p,0.5);
@@ -62,7 +66,7 @@
         double c = (e-f)/(1+e*f);
         double d = m*m+n*n;
         double dd = pow(d,0.5); //目的地までの距離
-        double t[2] = {5*w/3.14159,2*dd/3*1}; //方向転換は5秒以内、1はモーターの1m/sを緯度、経度に変換した値。再度図るのは、全体の2/3だけ進んだときにしました。
+        double t[2] = {5*w/3.14159,2*j/3}; //方向転換は5秒以内、1はモーターの1m/sで進むための時間。再度図るのは、全体の2/3だけ進んだときにしました。
         if(c*w>0){
             driveMotor(1,0); //方向転換、単位は1秒あたり36度。
             wait(t[0]);
@@ -79,8 +83,11 @@
             driveMotor(1,1);
             wait(t[1]);
             }
+//ちゃんと前後確認するプログラムを書く。いったん、0度と180度かの判断はあきらめて、0度と180度の時は、違う方角を向くようにしました。
         else{
-            return 0;
+            driveMotor(1,0); //直線上にある時は、18度だけ向きを変更します。
+            wait(0.5);
+            driveMotor(0,0);
             }
         }
      }