File content as of revision 0:73e805ecf765:

#include "mbed.h"
#include <Timer.h>
#include "main.h"
#define MEM_SIZE 5
#define MEM_TYPE char
#define COLOR_SIZE 3
#define TIEMPO_SIZE 0.2
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);
/* //Puertos Sensor color
//433
DigitalIn OUT(PC_0);//A5 BC1
DigitalOut S2(PC_1);//A4 VD1
DigitalOut S3(PC_2);//A3 NG
DigitalOut S1(PC_3);//A2 BC2
DigitalOut S0(PA_1);//A1 VD2
*/
//446
DigitalIn OUT(PC_0);//A5 BC1
DigitalOut S2(PC_1);//A4 VD1
DigitalOut S3(PB_0);//A3 NG
DigitalOut S1(PA_4);//A2 BC2
DigitalOut S0(PA_1);//A1 VD2
//
Timer tiempo;
     //Variables de Trabajo Colores identificados
     int Rojo = 0;
     int Azul = 0;
     int Verde = 0;
     int Amarillo = 0;
     int Negro = 0;
     int Blanco = 0;
 int color = 0;
 // color
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 == 1) { // 433 0 // 446 1
    mover_par(par,dire); 
    }
    centro(par);
    debug_m("fin del comado pares..\n"); 
    }
    
void caminar(){
    debug_m("Caminando..\n");    
    
    char col = buffer[2];
    printf("Color: %x \n",  col ); 
    char acc = buffer[3];
    printf("Accion: %x \n",  acc ); 
    if (buffer[2] > 0x00 && buffer[2] < 0x06){
    int salir = 0;
    while(salir == 0){
        if (color != buffer[2]){
            command.printf("Programa Color \n");
            leer:
            color = leer_color();
            command.printf("Color encontrado %i \n",color);
            if(color == buffer[2]){
                if(color > 0){
                    command.printf("Color Aceptado %i \n",color);
                    command.printf("Ejecutando Accion %x \n",  acc );
                     switch ( buffer[3] ) {
  
                        case 0x01: mover_ser(0x01,0x00); break;
                        case 0x02: mover_ser(0x02,0x00);break;
                        case 0x03: mover_ser(0x03,0x00);break;
                        case 0x04: mover_ser(0x04,0x00);break;
                        case 0x05: mover_ser(0x01,0xB4);break;
                        case 0x06: mover_ser(0x02,0xB4);break;
                        case 0x07: mover_ser(0x03,0xB4);break;
                        case 0x08: mover_ser(0x04,0x4B);break;
                        case 0x09: mover_ser(0x01,0x00);break;
                        case 0x10: mover_ser(0x02,0x00);break;
                        case 0x11: mover_ser(0x03,0x00);break;
                        case 0x12: mover_ser(0x04,0x00);break;
                        case 0x13: mover_ser(0x01,0xB4);break;
                        case 0x14: mover_ser(0x02,0xB4);break;
                        case 0x15: mover_ser(0x03,0xB4);break;
                        case 0x16: mover_ser(0x04,0xB4);break;
                        default: break;
                    }
                    
                    
                    
                    
                    salir = 1;
                }
            }else{
            goto leer;
            }
        }
    }
   /* 
    while(button == 0) {
    mover_par(par,dire); 
    }
    centro(par);
    */
    debug_m("Stop..\n"); }else{
        debug_m("Fuera de rango color..\n");
        }
    }
    
unsigned int leer_color(void){
    //Variables de Lectura
    unsigned int Red = 0;
    unsigned int Blue = 0;
    unsigned int Green = 0;
    int salida = 0;
  while(salida == 0){ 
    S0 = 1;
    S1 = 1; 
    //Leer Rojo
    S2 = 0;
    S3 = 0;
    Red = lectura();
    //Leer Azul
    S2 = 0;
    S3 = 1;
    Blue = lectura();
    //Leer Verde
    S2 = 1;
    S3 = 1;
    Green = lectura();
    
    command.printf("Red: %d, Green: %d, Blue: %d ",Red,Green,Blue);
    wait(TIEMPO_SIZE);
    /*
    if( Red <= 9 && Red >= 5 && Green <= 7 && Green >= 3 && Blue <= 9 && Blue >= 6 ){ // Limite Inferior && Limite Superior por R G B
    //ROJO
    Rojo++;
    command.printf("R++ %i ",Rojo);
    
        }else if( Red <= 51 && Red >= 45 && Green <= 6 && Green >= 3 && Blue <= 51 && Blue >= 48 ){
            //AZUL
            Azul++;
            command.printf("Az++ %i ",Azul);
            
            }else if( Red <= 29 && Red >= 23 && Green <= 6 && Green >= 3 && Blue <= 30 && Blue >= 23 ){
                //VERDE
                Verde++;
                command.printf("V++ %i ",Verde);
                
                }else if( Red <= 6 && Red >= 4 && Green <= 4 && Green >= 2 && Blue <= 7 && Blue >= 4 ){
                    //AMARILLO
                    Amarillo++;
                    command.printf("Am++ %i ",Amarillo);
                    
                    }else if( Red <= 67 && Red >= 63 && Green <= 18 && Green >= 15 && Blue <= 68 && Blue >= 64  ){
                        //NEGRO
                        Negro++;
                        command.printf("N++ %i ",Negro);
                        
                        }else if( Red <= 1 && Red >= 1 && Green <= 18 && Green >= 15 && Blue <= 68 && Blue >= 64 ){
                            //BLANCO
                            Blanco++;
                            command.printf("B++ %i ",Blanco);
                            
                            }else{
                                //INDEFINIDO
                                command.printf("Nada ");
                         
                                }
                                */
    if( Red <= 9 && Red >= 5 && Green <= 38 && Green >= 25 && Blue <= 25 && Blue >= 18 ){ // Limite Inferior && Limite Superior por R G B
    //ROJO
    Rojo++;
    command.printf("R++ %i ",Rojo);
    
        }else if( Red <= 46 && Red >= 35 && Green <= 25 && Green >= 18 && Blue <= 11 && Blue >= 5 ){
            //AZUL
            Azul++;
            command.printf("Az++ %i ",Azul);
            
            }else if( Red <= 27 && Red >= 20 && Green <= 12 && Green >= 8 && Blue <= 15 && Blue >= 10 ){
                //VERDE
                Verde++;
                command.printf("V++ %i ",Verde);
                
                }else if( Red <= 10 && Red >= 3 && Green <= 10 && Green >= 4 && Blue <= 13 && Blue >= 10 ){
                    //AMARILLO
                    Amarillo++;
                    command.printf("Am++ %i ",Amarillo);
                    
                    }else if( Red <= 66 && Red >= 50 && Green <= 65 && Green >= 60 && Blue <= 45 && Blue >= 40 ){
                        //NEGRO
                        Negro++;
                        command.printf("N++ %i ",Negro);
                        
                        }else if( Red <= 6 && Red >= 3 && Green <= 6 && Green >= 3 && Blue <= 6 && Blue >= 3 ){
                            //BLANCO
                            Blanco++;
                            command.printf("B++ %i ",Blanco);
                            
                            }else{
                                //INDEFINIDO
                                command.printf("Nada ");
                         
                                }                             
    if( Rojo > COLOR_SIZE ){ // 
    //ROJO
    command.printf("Rojo\n");
    borrar();
    salida = 1;
        }else if( Azul > COLOR_SIZE ){
            //AZUL
            command.printf("Azul\n");
            borrar();
            salida = 2;
            }else if( Verde > COLOR_SIZE ){
                //VERDE
                command.printf("Verde\n");
                borrar();
                salida = 3;
                }else if( Amarillo > COLOR_SIZE ){
                    //AMARILLO
                    command.printf("Amarillo\n");
                    borrar();
                    salida = 4;
                    }else if( Negro > COLOR_SIZE ){
                        //NEGRO
                        command.printf("Negro\n");
                        borrar();
                        salida = 5;
                        }else if( Blanco > COLOR_SIZE ){
                            //BLANCO
                            command.printf("Blanco\n");
                            borrar();
                            salida = 6;
                            }else{
                                //INDEFINIDO
                                command.printf("Contando \n");
                                }   
                                                        
    }
    //command.printf("Color encontrado %i \n",salida);
    return salida;
}
 
unsigned int lectura(void){
    unsigned int Resultado = 0;
    while(OUT){} 
    while(!OUT){} 
    while(OUT){}
    tiempo.start();
    while(!OUT){}
    tiempo.stop();
    Resultado = tiempo.read_us();
    tiempo.reset();
    return(Resultado);
}
void borrar(void){

    Rojo = 0;
    Azul = 0;
    Verde = 0;
    Amarillo = 0;
    Negro = 0;
    Blanco = 0;
    
    return;
    }    
    
void debug_m(char *s , ... ){
    #if DEBUG
    command.printf(s);
    #endif  
}