Diff: main.cpp

Revision:

4:c60636c95b80

Parent:

3:bd16e43ad7be

Child:

5:b384cf06de76

--- a/main.cpp	Fri Oct 17 06:21:52 2014 +0000
+++ b/main.cpp	Fri Oct 17 09:08:06 2014 +0000
@@ -77,6 +77,7 @@
 #define MOVE_STO        'q'
 #define MOVE_90L        'o'
 #define MOVE_90R        'p'
+#define GET_GPS         'g'
 // Otros
 #define reset_encoders  'r'
 #define request_data    'm'
@@ -88,6 +89,7 @@
 Serial bt(PTC15, PTC14);
 // Leds for status
 BusOut leds(LED_RED, LED_GREEN, LED_BLUE);
+Serial gps(PTC17, PTC16);
 
 // Sensors
 AnalogIn ultra_left(A2);
@@ -101,20 +103,25 @@
 
 // ----- Variables ------------------------------------------------------------------
 char bt_data;
+float lat_val, lon_val;
 
 // ----- Function prototypes --------------------------------------------------------
 void read_bt();
 void command_bt(char _command);
 void send_all_data();
+void gps_data();
 
 // ----- Main program ---------------------------------------------------------------
 int main(){
     init_encoders();
+    init_magnometer();
     bt.baud(9600); 
     pc.baud(9600);
+    gps.baud(4800);
     leds = LED_NORMAL;
     while (true){
-        read_bt(); 
+        read_bt();
+        gps_data();
     }
 }
 
@@ -159,6 +166,9 @@
         case MOVE_STO:
             motor_stop();
             break;
+        case GET_GPS:
+            gps_data();
+            break;
         //case MOVE_90L:
         //    move_90(1);
         //    break;
@@ -183,4 +193,74 @@
     bt.printf("Right encoder: %f cms\tLeft encoder: %f cms\n", encoder(encoder_rigth), encoder(encoder_rigth));
     bt.printf("Rigt distance: %fcms\tLeft distance: %fcms\n", ultrasonicos(ULTRA_R), ultrasonicos(ULTRA_L));
     bt.printf("Angle: %f\n", get_mag_angle());
+}
+
+void gps_data(){
+    leds = LED_BUSY;
+    
+    if (gps.readable()){
+        
+        char lat[10];
+        char lon[10];
+        char c;
+        char str[200];
+        c = gps.getc();
+        if (c == '$') {
+            gps.scanf ("%199s",str);  // Get all the data
+            //pc.printf("%s \n",str);
+            // Checking if its the nmea string that we need
+            if (str[2] == 'G' && str[3] == 'G' && str[4] == 'A'){
+                int i = 0;
+                int j = 0;
+                c = ' ';
+                int array_size = 0;
+                array_size = sizeof(str);
+                // Searching for the second comma
+                for (i = 6; i < array_size; i++){
+                    c = str[i];
+                    if (c == ',') break;
+                }
+                // Latitud
+                c = ' ';
+                i++;
+                j = 0;
+                while ( c != ','){
+                    c = str[i+j];
+                    if (c != ',')
+                        lat[j] = c;
+                    j++;
+                }
+                // N
+                for (i = j; i < array_size; i++){
+                    c = str[i];
+                    if (c == 'N') break;
+                }
+                
+                // Obtenemos el valor de longitud
+                c = ' ';
+                i = i + 2;
+                j = 0;
+                while ( c != ','){
+                    c = str[i+j];
+                    if (c != ',')
+                        lon[j] = c;
+                    j++;
+                }
+                //pc.printf("sLatitud %s \n",lat);
+                //pc.printf("sLongitud %s \n",lon);
+                
+                lat_val = atof(lat);
+                lon_val = atof(lon);
+                
+                pc.printf("Latitud %f \n",lat_val);
+                pc.printf("Longitud %f \n",lon_val);
+                wait(0.001);
+            }
+            
+        }
+    }else{
+        //pc.printf("No gps data available \n");
+        //leds = LED_ERROR; wait(0.1);
+    }
+    leds = LED_NORMAL;
 }
\ No newline at end of file