Revision 4:c60636c95b80, committed 2014-10-17

Comitter:

gerardo_carmona

Date:

Fri Oct 17 09:08:06 2014 +0000

Parent:

3:bd16e43ad7be

Child:

5:b384cf06de76

Commit message:

Mayu..

Changed in this revision

--- 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

--- a/my_libraries/magnometer.cpp	Fri Oct 17 06:21:52 2014 +0000
+++ b/my_libraries/magnometer.cpp	Fri Oct 17 09:08:06 2014 +0000
@@ -16,6 +16,10 @@
 //double average;
 
 // ----- Functions ------------------------------------------------------------------
+void init_magnometer(){
+    fxos.enable();
+}
+
 double get_mag_x(){
     fxos.get_data(&accel_data, &magn_data);
     return magn_data.x;
@@ -28,8 +32,6 @@
 
 double get_mag_angle(){
     double _angle;
-    int16_t _x=0;
-    int16_t _y=0;
     
     for (int i = 0; i < 10; i++){
         fxos.get_data(&accel_data, &magn_data);
@@ -39,10 +41,10 @@
         //pc.printf("%d\n", magn_data.x);
     }
     
-    avg_x = avg_x / 10;
-    avg_y = avg_y / 10;
+    avg_x = avg_x / 10.0;
+    avg_y = avg_y / 10.0;
     
     //pc.printf("%d\n", avg_x);
-    _angle = atan2((double)_x, (double)_y) * 180.0 / 3.14159;
+    _angle = atan2(-avg_y, avg_x) * 180.0 / 3.14159;
     return _angle;
 }
\ No newline at end of file

--- a/my_libraries/magnometer.h	Fri Oct 17 06:21:52 2014 +0000
+++ b/my_libraries/magnometer.h	Fri Oct 17 09:08:06 2014 +0000
@@ -8,5 +8,6 @@
 double get_mag_x();
 double get_mag_y();
 double get_mag_angle(); 
+void init_magnometer();
 
 #endif
\ No newline at end of file

--- a/my_libraries/ultrasonic.cpp	Fri Oct 17 06:21:52 2014 +0000
+++ b/my_libraries/ultrasonic.cpp	Fri Oct 17 09:08:06 2014 +0000
@@ -17,12 +17,15 @@
 
 // ----- Functions ------------------------------------------------------------------
 float ultrasonicos(int sensor){
+    double inch, volts;
     if (sensor == ULTRA_R) {
         read_sensor = ultra_rig;                 // read analog as a float         
     }else if (sensor == ULTRA_L) {
         read_sensor = ultra_lef;                 // read analog as a float    
     }
     // read_sensor * 3300mv / 6.4 mV/in * 2.54 in/cms
-    cm = read_sensor * 1309.6875;
+    volts = read_sensor * 3300;
+    inch = volts / 6.4;
+    cm = inch * 2.54;
     return cm;
 }
\ No newline at end of file