Diff: main.cpp

Revision:

0:02573593bcdc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jun 29 13:04:49 2016 +0000
@@ -0,0 +1,49 @@
+#include "mbed.h"
+#include "MBed_Adafruit_GPS.h"
+
+int main()
+{
+
+    int flag = 0;
+
+    Serial* gps_Serial = new Serial(p28,p27);
+    Adafruit_GPS myGPS(gps_Serial);
+    myGPS.begin(9600);
+
+    const double target_x = 37.524988, target_y = 139.938585;
+    double robot_x = 0.0, robot_y = 0.0;
+    double sub_x = 0.0, sub_y = 0.0;
+    double distance = 0.0;
+    Timer GPS_Timer;
+    GPS_Timer.start();
+    const int GPS_Time = 1000;
+
+    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
+    myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
+    myGPS.sendCommand(PGCMD_ANTENNA);
+
+    while(true) {
+
+        myGPS.read();
+        if ( myGPS.newNMEAreceived() ) {
+            if ( !myGPS.parse(myGPS.lastNMEA()) ) {
+                continue;
+            }
+        }
+
+        if (GPS_Timer.read_ms() >= GPS_Time) {
+            GPS_Timer.reset();
+            if(myGPS.fix) {
+                robot_x = (double)myGPS.latitudeH + (double)(myGPS.latitudeL / 10000.0 / 60.0);
+                robot_y = (double)myGPS.longitudeH +(double)(myGPS.longitudeL / 10000.0 / 60.0);
+            }
+
+            sub_x = (abs(target_x) - abs(robot_x)) * 111135.0;
+            sub_y = (abs(target_y) - abs(robot_y)) * 91191.0;
+            distance = sqrt(sub_x*sub_x - sub_y*sub_y);
+            printf("latitude:%f, longitude:%f, distance:%f\n", robot_x, robot_y, distance);
+
+        }
+    }
+    return 0;
+}
\ No newline at end of file