File content as of revision 5:b384cf06de76:

/* ==================================================================================
    --- TEST 1 ---
    OBJETIVE:
    Move the motor via bluetooth and program 90° turns to the right and left using 
    the magnometer. It will report data from the encoders and limit the movement by
    the ultrasonic sensors
    
    
    COMPONENTS:
    * Bluetooth
    * Magnometer            (FXOS8700CQ & magnometer.h)
    * Motors                (motors.h)
    * Encoder               (encoder.h)
    * Ultrasonic sensors    (motors.h)
    * SD Card               (sd_card.h & SDFileSystem)
    * xbee                  (xbee.h)
    
    
    CONTROLS:
    w: foward
    s: reverse
    a: left turn
    d: right turn
    o: 90° left turn
    p: 90° right turn
    
    
    Credits:
    * Mayumi Haro
    * Sarahí Morán
    * Gerardo Carmona
    
    VERSION:
    1.3 - 10/17/14
    
    
    HISTORY:
    1.3 - 10/17/14
    (+) Added nuw functions
    
    1.2 - 10/16/14
    (+) Added xbee, ultrasonic, sd_card
    
    1.1 - 10/16/14
    (*) Some bug fixes
    (+) Added encoder.h and encpoder.cpp (empty files)
    
    1.0 - 10/14/14
    (+) Initial release.
    
    
================================================================================== */

// ----- Libraries ------------------------------------------------------------------
#include "mbed.h"
#include "motors.h"
#include "magnometer.h"
#include "sd_card.h"
#include "xbee.h"
#include "encoders.h"
#include "ultrasonic.h"

// ----- Constants ------------------------------------------------------------------
#define LED_ERROR       6
#define LED_NORMAL      5
#define LED_BUSY        3
// Motores
#define MOVE_FWD_F      'W'
#define MOVE_REV_F      'S'
#define MOVE_LEF_F      'A'
#define MOVE_RIG_F      'D'
#define MOVE_FWD_S      'w'
#define MOVE_REV_S      's'
#define MOVE_LEF_S      'a'
#define MOVE_RIG_S      'd'
#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'
#define encoder_rigth   1
#define encoder_left    2

// ----- I/O Pins -------------------------------------------------------------------
Serial pc(USBTX, USBRX);
Serial bt(PTC15, PTC14);
// Leds for status
BusOut leds(LED_RED, LED_GREEN, LED_BLUE);
Serial gps(PTC17, PTC16);

// Sensors
AnalogIn ultra_left(A2);
AnalogIn ultra_right(A3);
//DigitalIn encoder_left(D3);
//DigitalIn encoder_right(D4);

// ----- Others ---------------------------------------------------------------------
Timer bt_timer;
Timer gps_timer;

// ----- 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();
        gps_data();
    }
}

// ----- Functions ------------------------------------------------------------------
void read_bt(){
    char c = ' ';
    if (bt.readable()){
        c = bt.getc();
        bt.printf("%c\n\r", c);
    //}
    //if (c != '\n'){
        command_bt(c);
    }
}

void command_bt(char _command){
    switch (_command){
        case MOVE_FWD_F:
            motor_fwd(FAST, FAST);
            break;
        case MOVE_REV_F:
            motor_rev(FAST, FAST);
            break;
        case MOVE_LEF_F:
            motor_left(FAST, FAST);
            break;
        case MOVE_RIG_F:
            motor_right(FAST, FAST);
            break;
        case MOVE_FWD_S:
            motor_fwd(SLOW, SLOW);
            break;
        case MOVE_REV_S:
            motor_rev(SLOW, SLOW);
            break;
        case MOVE_LEF_S:
            motor_left(SLOW, SLOW);
            break;
        case MOVE_RIG_S:
            motor_right(SLOW, SLOW);
            break;
        case MOVE_STO:
            motor_stop();
            break;
        case GET_GPS:
            gps_data();
            break;
        //case MOVE_90L:
        //    move_90(1);
        //    break;
        //case MOVE_90R:
        //    move_90(2);
        //    break;
        case reset_encoders:
            encoders_to_zero();
            break;
        case request_data:
            send_all_data();
            break;
        default:
            //motor_stop();
        //    bt.printf("%f\r\n", get_mag_angle());
        break;
    }
}

void send_all_data(){
    pc.printf("sending all data...\r\n");
    pc.printf("Right encoder: %f cms\tLeft encoder: %f cms\r\n", encoder(encoder_rigth), encoder(encoder_rigth));
    pc.printf("Rigt distance: %fcms\tLeft distance: %fcms\r\n", ultrasonicos(ULTRA_R), ultrasonicos(ULTRA_L));
    pc.printf("Angle: %f\r\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;
}