File content as of revision 4:bcc2d1bebb95:

#include "mbed.h"
#include "mbed.h"

Serial command(USBTX, USBRX);
DigitalOut led(LED1);
#define DEBUG 1

//*****************************************************************************
//  COMANDO  MOVER MOTOR
//  |POS 1|POS 2|POS 3|POS 4| POS 5|
//  |  <  | #M  |  ,  | #°G |  >   |
//
// #M  -> indica el motor que se va a mover
// #°G -> indica los grados a mover del  servomotor 
// <,> -> inicio, separdor  y fin de comando 
//  el inicio de comando no se almacena en el buffer
//*****************************************************************************

// VARIABLES PARA DEFINIR  EL  COMMANDO
#define BUFF_SIZE 4
#define COMM_NUM_MOTOR 0
#define COMM_SEPARADOR 1
#define COMM_GRADOS_MOTOR  2


uint8_t buffer_command[BUFF_SIZE]={0,0,0,0};


void print_num(uint8_t val)

{
if (val <10)
        command.putc(val+0x30);
    else 
        command.putc(val-9+0x40);
        
}
void print_bin2hex (uint8_t val)
{
    command.printf(" 0x");
    print_num(val>>4);
    print_num(val&0x0f);
        
        
}

// TODO : TIMEOUT UART SERIAL
void Read_command()
{
    for (uint8_t i=0; i<BUFF_SIZE;i++)
        buffer_command[i]=command.getc();
    
}

void echo_command()
{
    for (uint8_t i=0; i<BUFF_SIZE;i++)
        print_bin2hex(buffer_command[i]);
      
}


uint8_t check_command()
{
       if (buffer_command[BUFF_SIZE-1]== '>' && buffer_command[COMM_SEPARADOR]==','){
        
            #if DEBUG
                command.printf("\nMover Motor:");
                print_bin2hex(buffer_command[COMM_NUM_MOTOR]);
                command.printf(" -> ");
                print_bin2hex(buffer_command[COMM_GRADOS_MOTOR]);
                command.printf(" grados \n");  
            #endif
            return 1;
        }
        #if DEBUG
            command.printf("\n ERROR COMANDO -> ");
            echo_command();
        #endif
        return 0;        
        
     
}
int main() {
    #if DEBUG
    command.printf("inicio con debug\n");
    #else
    command.printf("inicio sin debug\n");
    #endif
    uint8_t val;
    while(1){
        val=command.getc();
        if (val== '<'){
            Read_command();
            if (check_command()){
                //EJEMPLO DE COMANDO 
                led=1;  
                wait(1);
                led=0;
            }
        }
        
    }
}