File content as of revision 6:cb6b868465c3:

#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|
//  |  <  | #C  | a | b | c  | d |  >   |
//
// #C -> indica el comando
// a,b,c,d parametros del comando
// <,> -> inicio,  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_N 0
#define INITPARAMETER  1


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]== '>'){
        
            #if DEBUG
                command.printf("\nComando:");
                print_bin2hex(buffer_command[COMM_N]);
                command.printf(" -> ");
            #endif
            return 1;
        }
        #if DEBUG
            command.printf("\n ERROR COMANDO -> ");
            echo_command();
        #endif
        return 0;        
        
     
}
void command_led(int tm)
{
  //EJEMPLO DE COMANDO 
                led=1;  
                wait(tm);
                led=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()){
                command_led(buffer_command[INITPARAMETER]);
                #if DEBUG
                 echo_command();
                #endif
        }
        }
        
    }
}