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