![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
zahyou kime hukume
Diff: main.cpp
- Revision:
- 3:6033c3517dc7
- Parent:
- 2:3b1b6e825546
- Child:
- 4:962ecf61540b
--- a/main.cpp Wed Dec 01 05:42:15 2021 +0000 +++ b/main.cpp Wed Dec 01 07:10:14 2021 +0000 @@ -8,7 +8,7 @@ GPS gps(D1, D0);// tx,rx Serial xbee(A7,A2); -void driveMotor(int speedA,int speedB) { +void driveMotor(double speedA,double speedB) { float outputA = abs(speedA); float outputB = abs(speedB); if (speedA > 0) { @@ -35,9 +35,9 @@ int main(){ double x,y,u,v,m,n,i,w,j,p,q,e,f,c; - int r,s; - u = 34.546402; - v = 135.502126; + int r,s,k; + u = 34.546366; + v = 135.502015; s = 0; xbee.printf("XBee Connected\r\n"); @@ -51,7 +51,13 @@ xbee.printf("x:%f,y:%f\r\n", x, y); //10秒前進する driveMotor(1,1); - wait(10);//メモ:ただ直進してる + wait(5);//メモ:ただ直進してる + driveMotor(0.8,0.8); + wait(1); + driveMotor(0.5,0.5); + wait(2); + driveMotor(0.2,0.2); + wait(2); driveMotor(0,0); //m,nに現在の経度、緯度を記録する while(gps.getgps() != true){ @@ -93,7 +99,7 @@ driveMotor(1,0); //メモ:右回りがどっちか測定する wait(t[0]); driveMotor(0,0); - while(s == r){ + while(s == r-5){ driveMotor(1,1); wait(1.8); driveMotor(0,0); @@ -107,13 +113,20 @@ //超音波センサ終了*/ s++; } + driveMotor(0.8,0.8); + wait(1); + driveMotor(0.5,0.5); + wait(2); + driveMotor(0.2,0.2); + wait(2); + driveMotor(0,0); } else if(c*i<0){ driveMotor(0,1); wait(t[0]); driveMotor(0,0); - while(s == r){ + while(s == r-5){ driveMotor(1,1); wait(1.8); driveMotor(0,0); @@ -127,6 +140,13 @@ //超音波センサ終了*/ s++; } + driveMotor(0.8,0.8); + wait(1); + driveMotor(0.5,0.5); + wait(2); + driveMotor(0.2,0.2); + wait(2); + driveMotor(0,0); } //0度と180度かの判断はあきらめて、0度と180度の時は、違う方角を向くようにした。 @@ -144,17 +164,35 @@ x = gps.latitude; y = gps.longitude; //5秒前進する - driveMotor(1,1); - wait(5);//メモ:ただ直進してる + driveMotor(0.8,0.8); + wait(1); + driveMotor(0.5,0.5); + wait(2); + driveMotor(0.2,0.2); + wait(2); driveMotor(0,0); -//m,nに現在の経度、緯度を記録する - - while(gps.getgps() != true){ - xbee.printf("false\r\n"); - } +//m,nに現在の経度、緯度を記録する + m=gps.latitude; n=gps.longitude; - + a[0]=m-x; + a[1]=n-y; + k = 0; + while(a[0] == 0 || a[1] == 0 || k < 5){ + driveMotor(1,1); + wait(1); + driveMotor(0,0); + wait(1); + while(gps.getgps() != true){ + xbee.printf("false\r\n"); + } + m=gps.latitude; + n=gps.longitude; + a[0]=m-x; + a[1]=n-y; + k++; + } + xbee.printf("x:%f,y:%f\r\n", x, y); xbee.printf("m:%f,n:%f\r\n", m, n); @@ -166,6 +204,9 @@ j=pow(b[0]/0.000008983148616*b[0]/0.000008983148616+b[1]/0.000010966382364*b[1]/0.000010966382364,0.5); } + driveMotor(1,0); + wait(3); + driveMotor(0,0); } \ No newline at end of file