Diff: main.cpp

Revision:

0:a56e760b5d55

Child:

1:c9f35ca3f74f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 19 06:24:19 2021 +0000
@@ -0,0 +1,62 @@
+#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;
+    double m, n;
+    double i,w;
+    gps.getgps();
+    x = gps.latitude; //経度の初期値
+    y = gps.longitude; //緯度の初期値
+    while(1){
+        driveMotor(1,1);
+        wait(30);
+        driveMotor(0,0);
+        m=gps.latitude; //現在の経度
+        n=gps.longitude; //現在の緯度
+        x=m-x;
+        y=n-y;
+        double a[2]={x,y};
+        double b[2]={0-m,0-n}; //目的地の座標を(0,0)とした
+        double p=a[0]*a[0]+a[1]*a[1];
+        double q=b[0]*b[0]+b[1]*b[1];
+        double pp=pow(p,0.5);
+        double qq=pow(q,0.5);
+        i=(a[0]*b[0]+a[1]*b[1])/pp*qq;
+        w=acos(i);
+        }
+     }
+    
+    
\ No newline at end of file