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