Diff: main.cpp

Revision:

1:5f317aa38c12

Parent:

0:c51bac912336

Child:

2:3b1b6e825546

--- a/main.cpp	Wed Nov 17 07:55:22 2021 +0000
+++ b/main.cpp	Wed Nov 24 06:49:52 2021 +0000
@@ -1,12 +1,13 @@
 #include <mbed.h>
 #include <getGPS.h>
 
-PwmOut pinAFin(A1);
-PwmOut pinARin(D10);
+PwmOut pinAFin(D10);
+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);
 
 void driveMotor(int speedA,int speedB) {
   float outputA = abs(speedA);
@@ -36,26 +37,40 @@
 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;
+    s = 0;
+    
+    xbee.printf("XBee Connected\r\n");
+    while(gps.getgps() != true){
+        }    
+    
+//基準値を記録する
     gps.getgps();
     x = gps.latitude;
     y = gps.longitude;
-    while(j>10){
-//30秒前進する
-        driveMotor(1,1);
-        wait(30);//メモ：ただ直進してる
-        driveMotor(0,0);
+    xbee.printf("%f,%f\n", x, y);
+//10秒前進する
+    driveMotor(1,1);
+    wait(10);//メモ：ただ直進してる
+    driveMotor(0,0);
 //m,nに現在の経度、緯度を記録する        
-        gps.getgps();
-        m=gps.latitude;
-        n=gps.longitude;
+    gps.getgps();
+    m=gps.latitude;
+    n=gps.longitude;
+    xbee.printf("%f,%f\n", m, n);
 //a,bの配列に、それぞれ緯度、経度の現在地と初期位置の差、jに目標までの距離を記録する        
-        double a[2]={m-x,n-y};
-        double b[2]={u-m,v-n};
-        j=pow(b[0]*b[0]/0.000008983148616/0.000008983148616+b[1]*b[1]/0.000010966382364/0.000010966382364,0.5);
+    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);
+    
+    
+    while(j>10){
+
 //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;
+        i=(a[0]*b[0]+a[1]*b[1])/p/q;
         w=acos(i);
 //tanの値を出す        
         e = (v-n)/(u-m);
@@ -64,6 +79,7 @@
 //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);
         
 //実際に動くときのプログラム。1.2m/s計算、1.8s進み、0.4秒周りを見る。平均速度1.0m/s、基準を進行方向にしたとき見れる範囲:-43.5~43.5の予定
         
@@ -75,14 +91,14 @@
                 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++;
                 }
             }
@@ -95,14 +111,14 @@
                 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++;
                 }
             }
@@ -113,6 +129,30 @@
             wait(0.1);
             driveMotor(0,0);
             }
+            
+        //基準値を記録する
+        gps.getgps();
+        x = gps.latitude;
+        y = gps.longitude;
+//5秒前進する
+        driveMotor(1,1);
+        wait(5);//メモ：ただ直進してる
+        driveMotor(0,0);
+//m,nに現在の経度、緯度を記録する        
+        gps.getgps();
+        m=gps.latitude;
+        n=gps.longitude;
+        
+        xbee.printf("%f,%f\n", x, y);
+        xbee.printf("%f,%f\n", m, n);
+        
+//a,bの配列に、それぞれ緯度、経度の現在地と初期位置の差、jに目標までの距離を記録する        
+        a[0]=m-x;
+        a[1]=n-y;
+        b[0]=u-m;
+        b[1]=v-n;
+        j=pow(b[0]/0.000008983148616*b[0]/0.000008983148616+b[1]/0.000010966382364*b[1]/0.000010966382364,0.5);
+        
         }
      }