File content as of revision 3:bd16e43ad7be:

/* ==================================================================================
    --- 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 "encoder.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'
// 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);

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

// ----- Function prototypes --------------------------------------------------------
void read_bt();
void command_bt(char _command);
void send_all_data();

// ----- Main program ---------------------------------------------------------------
int main(){
    init_encoders();
    bt.baud(9600); 
    pc.baud(9600);
    leds = LED_NORMAL;
    while (true){
        read_bt(); 
    }
}

// ----- Functions ------------------------------------------------------------------
void read_bt(){
    char c = ' ';
    if (bt.readable()){
        c = bt.getc();
        bt.printf("%c\n\r", c);
    //}
    //if (c != ' '){
        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 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(){
    bt.printf("sending all data...\n");
    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());
}