Diff: main.cpp

Revision:

0:c51bac912336

Child:

1:5f317aa38c12

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Nov 17 07:55:22 2021 +0000
@@ -0,0 +1,119 @@
+#include <mbed.h>
+#include <getGPS.h>
+
+PwmOut pinAFin(A1);
+PwmOut pinARin(D10);
+PwmOut pinBFin(D11);
+PwmOut pinBRin(D12);
+Serial pc(SERIAL_TX, SERIAL_RX);
+GPS gps(D1, D0);// tx,rx
+
+void driveMotor(int speedA,int speedB) {
+  float outputA = abs(speedA);
+  float outputB = abs(speedB);
+  if (speedA > 0) {
+    pinAFin=outputA;
+    pinARin=0;
+  } else if (speedA < 0) {
+    pinAFin=0;
+    pinARin=outputA;
+  } else {
+    pinAFin=0;
+    pinARin=0;
+  }
+  if (speedB > 0) {
+    pinBFin=outputB;
+    pinBRin=0;
+  } else if (speedB < 0) {
+    pinBFin=0;
+    pinBRin=outputB;
+  } else {
+    pinBFin=0;
+    pinBRin=0;
+  }
+}
+
+int main(){
+    double x,y,u,v,m,n,i,w,j,p,q,e,f,c;
+    int r,s;
+    gps.getgps();
+    x = gps.latitude;
+    y = gps.longitude;
+    while(j>10){
+//30秒前進する
+        driveMotor(1,1);
+        wait(30);//メモ：ただ直進してる
+        driveMotor(0,0);
+//m,nに現在の経度、緯度を記録する        
+        gps.getgps();
+        m=gps.latitude;
+        n=gps.longitude;
+//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);
+//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;
+        w=acos(i);
+//tanの値を出す        
+        e = (v-n)/(u-m);
+        f = (n-y)/(m-x);
+        c = (e-f)/(1+e*f);
+//t[0]は回る時間、t[1]はかかる時間、rに何回進むプログラムを動かすかを決める        
+        double t[2] = {w/3.14159/2,2*j/3/1.2}; //再度図るのは、全体の2/3だけ進んだとき
+        r = t[1]/2.2;
+        
+//実際に動くときのプログラム。1.2m/s計算、1.8s進み、0.4秒周りを見る。平均速度1.0m/s、基準を進行方向にしたとき見れる範囲:-43.5~43.5の予定
+        
+        if(c*i>0){
+            driveMotor(1,0); //メモ：右回りがどっちか測定する
+            wait(t[0]);
+            driveMotor(0,0);
+            while(s == r){
+                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++;
+                }
+            }
+        
+        else if(c*i<0){
+            driveMotor(0,1);
+            wait(t[0]);
+            driveMotor(0,0);
+            while(s == r){
+                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++;
+                }
+            }
+            
+//0度と180度かの判断はあきらめて、0度と180度の時は、違う方角を向くようにした。
+        else{
+            driveMotor(1,0); //直線上にある時は、18度だけ向きを変更
+            wait(0.1);
+            driveMotor(0,0);
+            }
+        }
+     }
+    
+    
\ No newline at end of file