File content as of revision 1:59be7d7e1b54:

#include "mbed.h"
#include <Timer.h>
#include "main.h"
#define MEM_SIZE 5
#define MEM_TYPE char
#define COLOR_SIZE 1  // cantidad de lecturas iguales
#define TIEMPO_SIZE 0.1 //tiempo de captura color
MEM_TYPE buffer[MEM_SIZE];
Serial command(USBTX, USBRX); // Usar Serial PC USB
Serial Bluetooth(PC_10,PC_11); //Tx, RX. Usar Bluetooth HC-06.
DigitalOut Led(LED1); // digital output 
DigitalIn button(USER_BUTTON);
void Rx_interrupt();
void Rx_interruptBT();

/* //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() {
    //command.baud(9600);
    //Bluetooth.baud(9600);
    init_serial();
// Setup a serial interrupt function to receive data
    command.attach(&Rx_interrupt, Serial::RxIrq);
    Bluetooth.attach(&Rx_interruptBT, Serial::RxIrq);
    init_servo();
    
    debug_m("inicio \n");

    while(1)
    {
       //caminar(7);
       
       //activar en produccion
       int action = leer_color();
    switch ( action ) {
  
        case 1: //Gira Derecha
       caminar(action);
       break; 
       case 2: //Gira Izq
        caminar(action);
        break;
        case 3: //Adelante
        caminar(action);
        break;
        case 4: //atras
        caminar(action);
        break;
        case 5: //Saludar 1
        caminar(action);
        break;
        case 6: //Saludar 2
        caminar(action);
        break; 
      default: 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();
    debug_m("inicio matriz\n");
    buffer[0] = command.getc();
    debug_m("comando matriz\n");
 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;
 
}
*/
uint32_t read_command2()
{
   // retorna los byte recibidos concatenados en un entero, 
   // Formato | FF | TC | NM | GR | FD
   // Datos  | Inicio | Tele-comando | Numero Motor | Grados | Cierre
   inicio2:  
    debug_m("inicio matriz2\n");
    buffer[0] = Bluetooth.getc();
    debug_m("comando matriz2\n");
  
  
    while(buffer[0] != 0xFF ){ //7B y 7D  son { y }
    
        buffer[0] = Bluetooth.getc();
        debug_m("P1: %x \n",  buffer[0]); 
        debug_m("comando inicio invalido \n");
        }
        debug_m("comando inicio Valido \n");
        char ini = buffer[0];
        debug_m("inicio encontrado: %x \n",  ini); 
        
            buffer[1] = Bluetooth.getc();
            debug_m("P2: %x \n",  buffer[1]);
        if (buffer[1] != 0xFF && buffer[1] != 0xFD){     
            buffer[2] = Bluetooth.getc();
            }else{
                buffer[0] = buffer[1];
                goto inicio2; 
            }
            debug_m("P3: %x \n",  buffer[2]);
        if (buffer[2] != 0xFF && buffer[2] != 0xFD){     
            buffer[3] = Bluetooth.getc();
            }else{
                buffer[0] = buffer[2];
                goto inicio2; 
            }
            debug_m("P4: %x \n",  buffer[3]);
        if (buffer[3] != 0xFF && buffer[3] != 0xFD){     
            buffer[4] = Bluetooth.getc();
            }else{
                buffer[0] = buffer[3];
                goto inicio2; 
            }
     //para que evalue el cierre 
        debug_m("P5: %x \n",  buffer[4]);
        if (buffer[4] != 0xFF && buffer[4] == 0xFD){     
            debug_m("comando Cierre valido \n");
            }else{
                buffer[0] = buffer[4];
                goto inicio2; 
            }
            

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


    return tele;
 
}
void init_serial()
{
    command.baud(9600);
    Bluetooth.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(int w){
    debug_m("Caminando..\n"); 
        switch ( w ) {
  
        case 1: //Gira Derecha
       direccion(0x01);
       break; 
       case 2: //Gira Izq
        direccion(0x02);
        break;
        case 3: //Adelante
        direccion(0x03);
        break;
        case 4: //atras
        direccion(0x06);
        break;
        case 5: //Saludar 1
        direccion(0x05);
        break;
        case 6: //Saludar 2
        direccion(0x06);
        break; 
        case 7: //pruebas
        direccion(0x07);
        break; 
      default: break;
}
    
      
      
      
      
    }
    
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);
    Bluetooth.printf(s);
    #endif  
}
void Rx_interruptBT() {
    Led=!Led;
    //char d = Bluetooth.getc();
    //Bluetooth.printf("blue %x\n",d);
    //command.printf("bluetooth %x\n",d);
        debug_m("Entrando TC...\n");
      uint32_t read_cc;
      read_cc=read_command2();
      
        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(3);
            debug_m("Tc 3 Finalizado\n");
            break;
            default: debug_m("Error de comando. \nSe espera  entre 0x01 - 0x08 \n");break ;
            }
            wait(3);
    return;
}
void Rx_interrupt() {
    
    char d = command.getc();
    command.printf("Serial %c\n",d);
    if (d=='i')
    {
        command.printf("Arri1\n");
        direccion(0x03);
    }else if(d=='d')
    {
        command.printf("Abaj1\n");
        direccion(0x04);
    }else if(d=='o')
    {
        command.printf("Izq1\n");
        direccion(0x02);
    }else if(d=='p')
    {
        command.printf("Der1\n");
        direccion(0x01);
    }else if(d=='1')
    {
        command.printf("Izq2\n");
        direccion(0x05);
    }else if(d=='2')
    {
        command.printf("Der2\n");
        direccion(0x06);
    }else if(d=='3')
    {
        command.printf("Arri2\n");
         arrancar();
    }else if(d=='4')
    {
        command.printf("Abaj2\n");
    }else if(d=='Y')
    {
        command.printf("Y\n");
        centro(0x01);
        
    }else if(d=='X')
    {
        command.printf("X\n");
        centro(0x02);
    }else if(d=='B')
    {
        command.printf("B\n");
        centro(0x03);
    }else if(d=='A')
    {
        command.printf("A\n");   
        centro(0x04); 
    }else{
        command.printf("Sin especificar\n");
        Led=!Led;
        }
    
    
    /*//telecomandos desde serial
     uint32_t read_cc;
      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 ;      
            }
           */ 
    return;
    
}