Diff: main.cpp

Revision:

0:a01fda36fde8

Child:

1:10af8aaa5b40

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 10 14:10:51 2018 +0000
@@ -0,0 +1,137 @@
+#include "mbed.h"
+#include "gps.h"
+#include "ultrasonic.h"
+#include "motordriver.h"
+
+Serial pc(USBTX, USBRX);
+GPS gps(p28, p27); 
+
+Motor motor1(p22, p16, p17, 1); // pwm, fwd, rev, can brake
+Motor motor2(p22, p19, p20, 1); // pwm, fwd, rev, can brake
+
+void dist(int distance)
+{
+    //put code here to happen when the distance is changed
+    printf("Distance changed to %dmm\r\n", distance);
+}
+
+ultrasonic mu(p11, p12, .1, 1, &dist);    //Set the trigger pin to D8 and the echo pin to D9
+                                          //have updates every .1 seconds and a timeout after 1
+                                          //second, and call dist when the distance changes
+ 
+
+ 
+LocalFileSystem local("local");               // Create the local filesystem under the name "local"
+ 
+int main() {
+    
+    /*FILE *fp = fopen("/local/gps.txt", "w");  // Open "out.txt" on the local file system for writing
+    fprintf(fp, "GPS Start\r\n");
+    int n;
+    for(n=0;n<45;n++)  //GPSの取得を45回行う
+    {
+        if(gps.getgps()) //現在地取得
+            fprintf(fp,"(%lf, %lf)\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
+        
+        else
+            fprintf(fp,"No data\r\n");//データ取得に失敗した場合
+        
+        wait(1);
+    }    
+    fprintf(fp,"GPS finish\r\n");
+    fclose(fp);*/
+
+
+    motor1.stop(0);
+    motor2.stop(0);
+    
+   
+    FILE *fp = fopen("/local/gps.txt", "w");  // Open "gps.txt" on the local file system for writing
+    fprintf(fp, "\r\n\GPS Start\r\n");
+    int n;
+    for(n=0;n<45;n++)  //GPSの取得を45回行う
+    {
+        printf("%d\n",n);
+        
+        if(gps.getgps()) //現在地取得
+            fprintf(fp,"(%lf, %lf)\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
+        
+        else
+            fprintf(fp,"No data\r\n");//データ取得に失敗した場合
+        
+        wait(1);
+    }    
+    fprintf(fp,"GPS finish\r\n");
+    fclose(fp);                        //GPSの測定終了  
+  
+        mu.startUpdates();//start mesuring the distance(超音波センサー)
+        int distance;
+        
+        while(1)
+    {
+        
+        motor1.speed(0.5);    //通常走行
+        motor2.speed(0.5);
+        //Do something else here
+        // mu.checkDistance();     //call checkDistance() as much as possible, as this is where
+        //the class checks if dist needs to be called.
+
+        distance=mu.getCurrentDistance();
+        
+
+        printf("%d\r\n",distance);
+        
+        if(100<distance && distance < 500)
+         {
+            
+            printf("if success!\r\n");
+            
+            float ms1,ms2,msj1,msj2;
+            ms1=0.5;            //直進の時モーターをどれだけ回せばいいかわからないのでとりあえず0.5にしておく
+            ms2=0.5;
+    
+            msj1=0.5;           //回転の時
+            msj2=0.5;
+
+            motor1.stop(0);
+            motor2.stop(0);
+            wait(1);
+            printf("mortor stop\r\n");
+
+            motor1.speed(msj1);       //機体を時計回りに９０度回転
+            motor2.speed(-msj2);
+            wait(1);
+            printf("mortor rotation\r\n");
+
+            motor1.speed(ms1);       //直進
+            motor2.speed(ms2);
+            wait(1);
+            printf("mortor straight\r\n");
+
+            motor1.speed(-msj1);      //機体を反時計回りに９０度回転
+            motor2.speed(msj2);
+            wait(1);
+            printf("mortor rotation\r\n");
+
+            motor1.speed(ms1);       //直進
+            motor2.speed(ms2);
+            wait(1);
+            printf("mortor straight\r\n");            
+
+            motor1.speed(-msj1);      //機体を反時計回りに９０度回転
+            motor2.speed(msj2);
+            wait(1);
+            printf("mortor rotation\r\n");
+
+            motor1.speed(ms1);       //直進
+            motor2.speed(ms2);
+            wait(1);
+            printf("mortor straight\r\n");
+
+            motor1.speed(msj1);       //機体を時計回りに９０度回転
+            motor2.speed(-msj2);
+            wait(1);   
+            printf("mortor rotation\r\n");
+        }
+   }
+}