cansat-e_2021
/
GPSDRV8833CanSat
connected GPS and motor driver
Diff: main.cpp
- Revision:
- 1:c9f35ca3f74f
- Parent:
- 0:a56e760b5d55
- Child:
- 2:591877e2c7d1
diff -r a56e760b5d55 -r c9f35ca3f74f main.cpp --- a/main.cpp Tue Oct 19 06:24:19 2021 +0000 +++ b/main.cpp Tue Oct 19 07:51:56 2021 +0000 @@ -56,6 +56,15 @@ double qq=pow(q,0.5); i=(a[0]*b[0]+a[1]*b[1])/pp*qq; w=acos(i); + 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]); } }