Diff: main.cpp

Revision:

8:8aca0c7419ab

Parent:

7:9ec83a204ae5

--- a/main.cpp	Thu Dec 09 07:29:57 2021 +0000
+++ b/main.cpp	Sat Dec 18 23:52:04 2021 +0000
@@ -1,12 +1,14 @@
 #include <mbed.h>
 #include <getGPS.h>
 
+DigitalOut pin(D7);
+DigitalIn pin1(D6);
 PwmOut pinAFin(D10);
 PwmOut pinARin(A1);
 PwmOut pinBFin(D11);
 PwmOut pinBRin(D12);
 GPS gps(D1, D0);// tx,rx
-Serial xbee(A7,A2);
+Serial xbee(SERIAL_TX, SERIAL_RX);
 
 void driveMotor(double speedA,double speedB) {
   float outputA = abs(speedA);
@@ -34,13 +36,28 @@
 }
 
 int main(){
+    pin1.mode(PullUp);
+    while(1){
+        if(pin1 == 1){
+            break;
+            }
+        }
+        
+    wait(50);
+    pin = 1;
+    wait(17);
+    pin = 0;
+    wait(10);
+    
+    
     double x,y,u,v,m,n,i,w,j,p,q,e,f,c;
-    int r,s,k;
-    u = 34.546205;
-    v = 135.509195;
+    int r,s;
+    u = 34.546385;
+    v = 135.502085;
     s = 0;
+    double t[2] = {0,0};
     
-    xbee.printf("XBee Connected\r\n");
+//    xbee.printf("XBee Connected\r\n");
     while(gps.getgps() != true){
         }    
     
@@ -48,7 +65,8 @@
     gps.getgps();
     x = gps.latitude;
     y = gps.longitude;
-    xbee.printf("x:%f,y:%f\r\n", x, y);
+//    xbee.printf("x:%f,y:%f\r\n", x, y);
+
 //10秒前進する
     driveMotor(1,1);
     wait(5);//メモ：ただ直進してる
@@ -61,9 +79,8 @@
     driveMotor(0,0);
 //m,nに現在の経度、緯度を記録する
     while(gps.getgps() != true){
-        xbee.printf("false\r\n");
     }
-
+    gps.getgps();
     m=gps.latitude;
     n=gps.longitude;
     xbee.printf("m:%f,n:%f\r\n", m, n);
@@ -88,31 +105,28 @@
         c = (e-f)/(1+e*f);
         xbee.printf("c:%f\r\n",c);
 //t[0]は回る時間、t[1]はかかる時間、rに何回進むプログラムを動かすかを決める        
-        double t[2] = {w/3.14159/0.9,2*j/3/1.2}; //再度図るのは、全体の2/3だけ進んだとき
-        r = t[1]/2.2;
+        t[0] = w/3.14159/0.9;
+        t[1] = 2.0*j/3.0/1.2;
+        xbee.printf("t[0]:%f\n",t[0]);
+        xbee.printf("t[1]:%f\n",t[1]);
         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の予定
         
-        if(c*i>0){
+        if(m-x==0 && n-y==0){
+            driveMotor(0,1); //直線上にある時は、18度だけ向きを変更
+            wait(0.1);
+            driveMotor(0,0);
+            wait(3);
+            }
+        else if(c*i>0){
             driveMotor(0.5,0); //メモ：右回りがどっちか測定する
             wait(t[0]);
             driveMotor(0,0);
-            while(s == r-5){
-                driveMotor(1,1);
-                wait(1.8);
-                driveMotor(0,0);
-/*超音波センサの起動。首を振る
-                driveMotor(0,1);
-                wait(0.1);
-                driveMotor(1,0);
-                wait(0.2);
-                driveMotor(0,1);
-                wait(0.1);
-//超音波センサ終了*/
-                s++;
-                }
+            wait(1);
+            driveMotor(1,1);
+            wait(t[1]);
             driveMotor(0.8,0.8);
             wait(1);
             driveMotor(0.5,0.5);
@@ -126,20 +140,9 @@
             driveMotor(0,0.5);
             wait(t[0]);
             driveMotor(0,0);
-            while(s == r-5){
-                driveMotor(1,1);
-                wait(1.8);
-                driveMotor(0,0);
-/*超音波センサの起動。首を振る
-                driveMotor(0,1);
-                wait(0.1);
-                driveMotor(1,0);
-                wait(0.2);
-                driveMotor(0,1);
-                wait(0.1);
-//超音波センサ終了*/
-                s++;
-                }
+            wait(1);
+            driveMotor(1,1);
+            wait(t[1]);
             driveMotor(0.8,0.8);
             wait(1);
             driveMotor(0.5,0.5);
@@ -161,6 +164,7 @@
         while(gps.getgps() != true){
 //            xbee.printf("false\r\n");
         }
+        gps.getgps();
         x = gps.latitude;
         y = gps.longitude;
 //5秒前進する
@@ -173,28 +177,16 @@
         driveMotor(0,0);
 //m,nに現在の経度、緯度を記録する
 
+        while(gps.getgps() != true){
+//            xbee.printf("false\r\n");
+        }
+        gps.getgps();
         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){     5mでは測れないかもと思って作ったプログラム
-            driveMotor(0.5,0.5);　　　　　　　　　　　　　　内容:5mで測れなかった時５回だけ1m進むプログラム
-            wait(2);
-            driveMotor(0,0);
-            wait(2);
-            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("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;