File content as of revision 5:6ca97bfe9406:

#include "mbed.h"
#include "main.h"
#define MEM_SIZE 5
#define MEM_TYPE char
MEM_TYPE buffer[MEM_SIZE];
Serial command(USBTX, USBRX); // Usar Serial PC USB
//Serial command(PC_10,PC_11); //Tx, RX. Usar Bluetooth HC-06.

DigitalIn button(USER_BUTTON);

int main() {
    init_servo();
    init_serial();
    debug_m("inicio \n");
    uint32_t read_cc;
    while(1)
    {
        read_cc=read_command();
        debug_m("Entrando TC...\n");

        switch (read_cc) {

            case  0x01: 
            debug_m("Tc 1 Iniciado..\n");
            moving();
            debug_m("Tc 1 Finalizado\n");
            break;
            case  0x02: 
            debug_m("Tc 2 Iniciado.. \n");
            pares();
            debug_m("Tc 2 Finalizado\n");
            break;
            case  0x03: 
            debug_m("Tc 3 Iniciado.. \n");
            caminar();
            debug_m("Tc 3 Finalizado\n");
            break;
            case  0x05: 
            debug_m("Tc 5 Centrar Iniciado.. \n");
            centro(01);
            centro(02);
            centro(03);
            centro(04);
            debug_m("Tc 5 Centrar Finalizado\n");
            break;
            default: debug_m("Error de comando. \nSe espera  entre 0x01 - 0x08 \n");break ;      
        }
    }
}



uint32_t read_command()
{
   // retorna los byte recibidos concatenados en un entero, 
   // Formato | FF | TC | NM | GR | FD
   // Datos  | Inicio | Tele-comando | Numero Motor | Grados | Cierre
    //char intc=command.getc();
    buffer[0] = command.getc();

 inicio:   
  
    while(buffer[0] != 0xFF ){ //7B y 7D  son { y }
    
        buffer[0] = command.getc();
        
        debug_m("comando inicio invalido \n");
        }
        debug_m("comando inicio Valido \n");
        char ini = buffer[0];
        printf("inicio encontrado: %x \n",  ini); 
        
            buffer[1] = command.getc();
        if (buffer[1] != 0xFF && buffer[1] != 0xFD){     
            buffer[2] = command.getc();
            }else{
                buffer[0] = buffer[1];
                goto inicio; 
            }
        if (buffer[2] != 0xFF && buffer[2] != 0xFD){     
            buffer[3] = command.getc();
            }else{
                buffer[0] = buffer[2];
                goto inicio; 
            }
        if (buffer[3] != 0xFF && buffer[3] != 0xFD){     
            buffer[4] = command.getc();
            }else{
                buffer[0] = buffer[3];
                goto inicio; 
            }
     //para que evalue el cierre 
        if (buffer[4] != 0xFF && buffer[4] == 0xFD){     
            debug_m("comando Cierre valido \n");
            }else{
                buffer[0] = buffer[4];
                goto inicio; 
            }

          
          
        //intc=command.getc();
        int tele = buffer[1];
        char telec = buffer[1];
        printf("Tele comando: %x \n",  telec ); 


    return tele;
 
}


void init_serial()
{
    command.baud(9600);
    debug_m("Serial 9600 \n");    
}


void moving(){
    debug_m("se inicia el comado mover..\n");    
    
    char nmotor = buffer[2];
    printf("Motor: %x \n",  nmotor ); 
    char grados = buffer[3];
    printf("Grados: %x \n",  grados ); 
    //char endc = buffer[4];
    //printf("Cierre: %c \n",  endc ); 
    mover_ser(nmotor,grados); 
    
    debug_m("fin del comado mover..\n");    
    
}
void pares(){
    debug_m("se inicia el comado pares..\n");    
    
    char par = buffer[2];
    printf("Par: %x \n",  par ); 
    char dire = buffer[3];
    printf("Dir: %x \n",  dire ); 
    char endcc = buffer[4];
    printf("Cierre: %x \n",  endcc ); 
    while(button == 0) {
    mover_par(par,dire); 
    }
    centro(par);
    debug_m("fin del comado pares..\n"); 
    }
    
void caminar(){
    debug_m("Caminando..\n");    
    
    char par = buffer[2];
    printf("Par: %x \n",  par ); 
    char dire = buffer[3];
    printf("Dir: %x \n",  dire ); 
    char endcc = buffer[4];
    printf("Cierre: %x \n",  endcc ); 
    while(button == 0) {
    mover_par(par,dire); 
    }
    centro(par);
    debug_m("Stop..\n"); 
    }
void debug_m(char *s , ... ){
    #if DEBUG
    command.printf(s);
    #endif  
}