Revision 5:9ff2f6cd54d2, committed 2019-12-08

Comitter:

KINU

Date:

Sun Dec 08 13:28:08 2019 +0000

Parent:

4:88f7837f947f

Commit message:

GPSXBeecompleted

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TB6612.cpp	Sun Dec 08 13:28:08 2019 +0000
@@ -0,0 +1,57 @@
+/**
+ * Motor Driver TB6612 Control Library
+ *
+ * -- TB6612 is a device of the TOSHIBA. 
+ *
+ * Copyright (C) 2012 Junichi Katsu (JKSOFT) 
+ */
+ 
+ 
+#include "TB6612.h"
+ 
+// TB6612 Class Constructor
+TB6612::TB6612(PinName pwm, PinName fwd, PinName rev):
+        _pwm(pwm), _fwd(fwd), _rev(rev) {
+ 
+    _fwd = 0;
+    _rev = 0;
+    _pwm = 0.0;
+    _pwm.period(0.001);
+}
+ 
+// Speed Control
+//  arg
+//   int speed -100 -- 0 -- 100
+void TB6612::speed(int speed) {
+        
+    if( speed > 0 )
+    {
+        _pwm = ((float)speed) / 100.0;
+        _fwd = 1;
+        _rev = 0;
+    }
+    else if( speed < 0 )
+    {
+        _pwm = -((float)speed) / 100.0;
+        _fwd = 0;
+        _rev = 1;
+    }
+    else
+    {
+        _fwd = 1;
+        _rev = 1;
+    }
+}
+ 
+ 
+// Speed Control with time-out
+//  arg
+//   int speed -100 -- 0 -- 100
+//   int time  0
+void TB6612::move(int sspeed , int time)
+{
+    speed(sspeed);
+    wait_ms(time);
+}
+ 
+            
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TB6612.h	Sun Dec 08 13:28:08 2019 +0000
@@ -0,0 +1,30 @@
+/**
+ * Motor Driver TB6612 Control Library
+ *
+ * -- TB6612 is a device of the rohm. 
+ *
+ * Copyright (C) 2012 Junichi Katsu (JKSOFT) 
+ */
+ 
+#ifndef MBED_TB6612_H
+#define MBED_TB6612_H
+ 
+#include "mbed.h"
+ 
+class TB6612 {
+public:
+    TB6612(PinName pwm, PinName fwd, PinName rev);
+    void speed(int speed);
+    void move(int speed , int time);
+    void operator= ( int value )
+    {
+        speed(value);
+    }
+    
+protected:
+    PwmOut _pwm;
+    DigitalOut _fwd;
+    DigitalOut _rev;
+};
+ 
+#endif
\ No newline at end of file

--- a/gps.cpp	Sun Nov 10 10:41:19 2019 +0000
+++ b/gps.cpp	Sun Dec 08 13:28:08 2019 +0000
@@ -1,44 +1,67 @@
 #include "mbed.h"
 #include "getGPS.h"
 #include "math.h"
+#include "TB6612.h"
 
 Serial pc(USBTX,USBRX);
 GPS gps (p28,p27);
 Serial xbee(p13,p14);
-
+TB6612 left(p25,p17,p16);
+TB6612 right(p26,p19,p18);
 
 int main() {
      double a;
      double b;
      double distance;
-    
-    pc.printf("GPS Start\n");
-    
+     int i = 0;
+     int j = 0;
+     
+     pc.printf("GPS Start\n");
+     xbee.printf("s\n");
      while(1) {
          if(gps.getgps()){
+           
+          pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示   
+          i ++;
+          if(i<59){
+           }else{
            a = gps.latitude;
            b = gps.longitude;
-           
-          pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示   
+           printf("(a =%lf,b =%lf)\r\n",gps.latitude,gps.longitude);//a,bを表示   
            break;
-           
+           }
          }else{
           pc.printf("NO DATA\r\n");//データ取得失敗
           wait(1);
             }
        }
-      while(1){
-         if(gps.getgps()) {
+       printf("moter on");
+        left = 100; //左モーター１００％
+        right = 100;//右モーター１００％
+        
+        wait(30);
+        printf("moter off");
+        left = 0; //左モーター１０％
+        right = 0;//右モーター１０％(左折)
+        printf("moter off");
+        wait(15);
+        printf("GPS restart\n");
+        
+        while(1) {
+         if(gps.getgps()){
            
-          pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//緯度と経度を表示   
-          
-       // 球面三角法により、大円距離(メートル)を求める
-       double c;
-       double d; 
-        c = gps.latitude;
-        d = gps.longitude;
+          pc.printf("(%lf,%lf)\r\n",gps.latitude,gps.longitude);//緯度と経度を表示   
+          j ++;
+          if(j<29){
+           }else{
+            // 球面三角法により、大円距離(メートル)を求める
+           double c;
+           double d; 
+           c = gps.latitude;
+           d = gps.longitude;
 
-         const double pi = 3.14159265359; // 円周率
+            pc.printf("(%lf,%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示   
+            const double pi = 3.14159265359; // 円周率
                            
             double ra = a * pi / 180;
             double rb = b * pi / 180;     // 緯度経度をラジアンに変換
@@ -51,27 +74,18 @@
             const double earth_radius = 6378140;   // 地球赤道半径(m)
 
             distance = earth_radius * rr; // 2点間の距離(m)
-             
-
-         if (distance<5){
+            
+            if (distance<5){
              }else{
+             pc.printf("(c =%lf,d =%lf)\n\r",gps.latitude,gps.longitude);//c,dを表示   
              pc.printf("5m clear!");
              xbee.printf("5m clear!");
-             break;
-              }
-          
-          }else{
-           xbee.printf("NO DATA\r\n");//データ取得失 
-           pc.printf("NO DATA\r\n");  
-           wait(1);
-           }
+              break;
+           }  
+            }
+       }else{
+         printf("No data");
+          }
         }
-        return 0;
-     }
-     
-     
-     
-     
-     
-    
-
+       return 0;      
+}
\ No newline at end of file