cansat-e_2021
/
GPSDRV8833CanSat
connected GPS and motor driver
Diff: main.cpp
- Revision:
- 2:591877e2c7d1
- Parent:
- 1:c9f35ca3f74f
- Child:
- 3:b4f715167119
diff -r c9f35ca3f74f -r 591877e2c7d1 main.cpp --- 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; + } } }