Diff: main.cpp

Revision:

2:3b1b6e825546

Parent:

1:5f317aa38c12

Child:

3:6033c3517dc7

--- a/main.cpp	Wed Nov 24 06:49:52 2021 +0000
+++ b/main.cpp	Wed Dec 01 05:42:15 2021 +0000
@@ -5,7 +5,6 @@
 PwmOut pinARin(A1);
 PwmOut pinBFin(D11);
 PwmOut pinBRin(D12);
-Serial pc(SERIAL_TX, SERIAL_RX);
 GPS gps(D1, D0);// tx,rx
 Serial xbee(A7,A2);
 
@@ -49,37 +48,44 @@
     gps.getgps();
     x = gps.latitude;
     y = gps.longitude;
-    xbee.printf("%f,%f\n", x, y);
+    xbee.printf("x:%f,y:%f\r\n", x, y);
 //10秒前進する
     driveMotor(1,1);
     wait(10);//メモ：ただ直進してる
     driveMotor(0,0);
-//m,nに現在の経度、緯度を記録する        
-    gps.getgps();
+//m,nに現在の経度、緯度を記録する
+    while(gps.getgps() != true){
+        xbee.printf("false\r\n");
+    }
+
     m=gps.latitude;
     n=gps.longitude;
-    xbee.printf("%f,%f\n", m, n);
+    xbee.printf("m:%f,n:%f\r\n", m, n);
 //a,bの配列に、それぞれ緯度、経度の現在地と初期位置の差、jに目標までの距離を記録する        
     double a[2]={m-x,n-y};
     double b[2]={u-m,v-n};
     j=pow(b[0]/0.000008983148616*b[0]/0.000008983148616+b[1]/0.000010966382364*b[1]/0.000010966382364,0.5);
+    xbee.printf("距離:%f\r\n", j);
     
     
-    while(j>10){
+    while(j>5){
 
 //cosと角度の値を出す        
         p=pow(a[0]*a[0]+a[1]*a[1],0.5);
         q=pow(b[0]*b[0]+b[1]*b[1],0.5);
         i=(a[0]*b[0]+a[1]*b[1])/p/q;
+        xbee.printf("i:%f\r\n",i);
         w=acos(i);
 //tanの値を出す        
         e = (v-n)/(u-m);
         f = (n-y)/(m-x);
         c = (e-f)/(1+e*f);
+        xbee.printf("c:%f\r\n",c);
 //t[0]は回る時間、t[1]はかかる時間、rに何回進むプログラムを動かすかを決める        
         double t[2] = {w/3.14159/2,2*j/3/1.2}; //再度図るのは、全体の2/3だけ進んだとき
         r = t[1]/2.2;
         wait(1);
+        xbee.printf("c*i:%f\r\n", c*i);
         
 //実際に動くときのプログラム。1.2m/s計算、1.8s進み、0.4秒周りを見る。平均速度1.0m/s、基準を進行方向にしたとき見れる範囲:-43.5~43.5の予定
         
@@ -128,10 +134,13 @@
             driveMotor(1,0); //直線上にある時は、18度だけ向きを変更
             wait(0.1);
             driveMotor(0,0);
+            wait(3);
             }
             
         //基準値を記録する
-        gps.getgps();
+        while(gps.getgps() != true){
+//            xbee.printf("false\r\n");
+        }
         x = gps.latitude;
         y = gps.longitude;
 //5秒前進する
@@ -139,12 +148,15 @@
         wait(5);//メモ：ただ直進してる
         driveMotor(0,0);
 //m,nに現在の経度、緯度を記録する        
-        gps.getgps();
+        
+        while(gps.getgps() != true){
+            xbee.printf("false\r\n");
+        }
         m=gps.latitude;
         n=gps.longitude;
         
-        xbee.printf("%f,%f\n", x, y);
-        xbee.printf("%f,%f\n", m, n);
+        xbee.printf("x:%f,y:%f\r\n", x, y);
+        xbee.printf("m:%f,n:%f\r\n", m, n); 
         
 //a,bの配列に、それぞれ緯度、経度の現在地と初期位置の差、jに目標までの距離を記録する        
         a[0]=m-x;