Diff: main.cpp

Revision:

2:591877e2c7d1

Parent:

1:c9f35ca3f74f

Child:

3:b4f715167119

--- a/main.cpp	Tue Oct 19 07:51:56 2021 +0000
+++ b/main.cpp	Wed Oct 20 02:55:52 2021 +0000
@@ -1,5 +1,6 @@
 #include <mbed.h>
 #include <getGPS.h>
+//イレギュラーへの対応がまだできてません
 
 PwmOut pinAFin(A1);
 PwmOut pinARin(D10);
@@ -56,15 +57,31 @@
         double qq=pow(q,0.5);
         i=(a[0]*b[0]+a[1]*b[1])/pp*qq;
         w=acos(i);
+        double e = (0-n)/(0-m);//目的地の座標を(0,0)とした
+        double f = (n-y)/(m-x);
+        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だけ進んだときにしました。
-        driveMotor(1,0); //方向転換、単位は1秒あたり72度。左回転にしたい。
-        wait(t[0]);
-        driveMotor(0,0);
-        wait(0.5);
-        driveMotor(1,1);
-        wait(t[1]);
+        if(c*w>0){
+            driveMotor(1,0); //方向転換、単位は1秒あたり36度。
+            wait(t[0]);
+            driveMotor(0,0);
+            wait(0.5);
+            driveMotor(1,1);
+            wait(t[1]);
+            }
+        else if(c*w<0){
+            driveMotor(0,1);
+            wait(t[0]);
+            driveMotor(0,0);
+            wait(0.5);
+            driveMotor(1,1);
+            wait(t[1]);
+            }
+        else{
+            return 0;
+            }
         }
      }