File content as of revision 6:8d7f6fe73ed1:

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

 DigitalOut s0(PB_13);
 DigitalOut s1(PB_14);
 DigitalOut s2(PB_15);
 DigitalOut s3(PB_1);
 DigitalIn  out (PB_2);
 
 DigitalOut LED(LED1);
 InterruptIn pulse(PC_13);
 float retardo = 0;

    
void ISR_Poff()

    {
    
    retardo = (0.05);
    
    }  
 
 
 
 Timer tiempo;
 
 unsigned int lectura (void);
 
Serial command(USBTX, USBRX);

int main() {
    init_servo();
    init_serial();
    pulse.fall (&ISR_Poff);

    debug_m("inicio \n");
    uint32_t read_cc;
    while(1)
    {
        LED = !LED;
        wait (retardo);
        
        read_cc=read_command();
        
        switch (read_cc)
        {
            case  0x01: moving();
                        break;
                        
            case  0x02: movimiento();
                        break;
                        
            case  0x03: sensor();
                        break;
                        
            case  0x04: saludo();
                        break;  
                        
            default:    debug_m("error de comando.\n");
                        break ;      
        }
        
    }
    
}



uint32_t read_command()
{
    char intc=command.getc();
    
    while(intc != 0xff)
        intc=command.getc();
    return command.getc();
}



void init_serial()
{
    command.baud(9600);    
}


void moving()
{
    command.printf("se inicia el 1er comado mover servos..\n");    
    char nmotor=command.getc();
    char grados=command.getc();
    char endc=command.getc();
    mover_ser(nmotor,grados); 
    debug_m("Moviendo servos...\n");       
}


void movimiento()
{
    command.printf("se inicia el 2do comado mover patas..\n");   
    char lado=command.getc();
    char movi=command.getc();
    char endc=command.getc();
    
    switch (lado)
        {
            case  0x01: switch (movi)                                           //Pata Derecha_delantera
                        {
                            case 0x01:  mover_ser(0x01,0x01);                  
                                        command.printf("moviendo pata derecha delantera arriba..\n");
                                        break;  
                                    
                            case 0x02:  mover_ser(0x01,0x2d);                   
                                        command.printf("moviendo pata derecha delantera abajo..\n");                  
                                        break; 
                        
                            case 0x03:  mover_ser(0x02,0x5a);                  
                                        command.printf("moviendo pata izquierda delantera adelante..\n");
                                        break; 
                                    
                            case 0x04:  mover_ser(0x02,0x0a);                  
                                        command.printf("moviendo pata izquierda delantera atras..\n");
                                        break; 
                        }
                        break;
                        
            case  0x02: switch (movi)                                           //Pata Derecha_trasera
                        {
                            case 0x01:  mover_ser(0x03,0x01);                   
                                        command.printf("moviendo pata derecha trasera arriba..\n");
                                        break;  
                                    
                            case 0x02:  mover_ser(0x03,0x2d); 
                                        command.printf("moviendo pata derecha trasera abajo..\n");                  
                                        break; 
                                    
                            case 0x03:  mover_ser(0x04,0xaa); 
                                        command.printf("moviendo pata derecha trasera adelante..\n");                  
                                        break; 
                                    
                            case 0x04:  mover_ser(0x04,0x5a); 
                                        command.printf("moviendo pata derecha trasera atras..\n");                  
                                        break;   
                        }
                        break;
                        
            case  0x03: switch (movi)                                           //Pata Izquierda_delantera
                        {
                            case 0x01:  mover_ser(0x05,0x01);                    
                                        command.printf("moviendo pata izquierda delantera arriba..\n");
                                        break;  
                                    
                            case 0x02:  mover_ser(0x05,0x2d);                   
                                        command.printf("moviendo pata izquierda delantera abajo..\n");
                                        break; 
                        
                            case 0x03:  mover_ser(0x06,0x5a);                   
                                        command.printf("moviendo pata izquierda delantera adelante..\n");
                                        break; 
                                    
                            case 0x04:  mover_ser(0x06,0xaa);                   
                                        command.printf("moviendo pata izquierda delantera atras..\n");
                                        break;   
                        }
                        break;
                        
            case  0x04: switch (movi)                                           //Pata Izquierda_trasera
                        {
                            case 0x01:  mover_ser(0x07,0x01);                   
                                        command.printf("moviendo pata izquierda trasera arriba..\n");
                                        break;  
                                    
                            case 0x02:  mover_ser(0x07,0x2d);                   
                                        command.printf("moviendo pata izquierda trasera abajo..\n");
                                        break; 
                        
                            case 0x03:  mover_ser(0x08,0x0a);                   
                                        command.printf("moviendo pata izquierda trasera adelante..\n");
                                        break; 
                                    
                            case 0x04:  mover_ser(0x08,0x5a);                   
                                        command.printf("moviendo pata izquierda trasera atras..\n");
                                        break;      
                        }
                        break;
                        
            default:    debug_m("error de comando.\n");
                        break ;      
        }
    //debug_m("fin del comado guardar..\n");       
}

void sensor()
{
    command.printf ("se inicia el 3er comando sensor de color..\n");    
    char leer=command.getc();
    char endc=command.getc();

    unsigned int ROJO=0;
    unsigned int VERDE=0;       
    unsigned int AZUL=0;
    
    if(leer==0x01)
    {
        s0=1;
        s1=0;
   
        s2=0;
        s3=0;
        ROJO= lectura();
        
        s2=0;
        s3=1;
        AZUL= lectura();
        
        s2=1;
        s3=1;
        VERDE= lectura();
        
        if (ROJO<VERDE && VERDE>AZUL && AZUL>ROJO)
        {
            command.printf("Color detectado: ROJO \n");
            command.printf("ADELANTE....\n");
            
                        for(int x=0;x<5;x++)
                        {
                        mover_ser(0x01,0x00);
                        wait(0.1);
                        mover_ser(0x02,0x32);
                        wait(0.1);
                        mover_ser(0x01,0x3c);
                        wait(0.1);
                        mover_ser(0x03,0x00);
                        wait(0.1);
                        mover_ser(0x04,0x5a);
                        wait(0.1);
                        mover_ser(0x03,0x3c);
                        wait(0.1);
                        mover_ser(0x02,0x5a);
                        wait(0.1);
                        mover_ser(0x04,0x82);
                        wait(0.1);  
                        mover_ser(0x05,0x00);
                        wait(0.1);
                        mover_ser(0x06,0x82);
                        wait(0.1);
                        mover_ser(0x05,0x3c);
                        wait(0.1);
                        mover_ser(0x07,0x00);
                        wait(0.1);
                        mover_ser(0x08,0x5a);
                        wait(0.1);
                        mover_ser(0x07,0x3c);
                        wait(0.1);
                        mover_ser(0x06,0x5a);
                        wait(0.1);
                        mover_ser(0x08,0x32);
                        wait(0.1);  
                        
                        }     
        } 
        else
        {
            if (VERDE<AZUL && AZUL>ROJO && ROJO>VERDE)
            {
                command.printf("Color detectado: VERDE \n");
                command.printf("ROTAR DERECHA\n");
                        
                        for(int x=0;x<5;x++)
                        {
                        mover_ser(0x01,0x00);
                        wait(0.1);
                        mover_ser(0x02,0x32);
                        wait(0.1);
                        mover_ser(0x01,0x3C);
                        wait(0.1);
                        mover_ser(0x03,0x00);
                        wait(0.1);
                        mover_ser(0x04,0x32);
                        wait(0.1);
                        mover_ser(0x03,0x3C);
                        wait(0.1);
                        mover_ser(0x07,0x00);
                        wait(0.1);
                        mover_ser(0x08,0x32);
                        wait(0.1);
                        mover_ser(0x07,0x3C);
                        wait(0.1);
                        mover_ser(0x05,0x00);
                        wait(0.1);
                        mover_ser(0x06,0x32);
                        wait(0.1);
                        mover_ser(0x05,0x3C);
                        wait(0.1);
                        mover_ser(0x02,0x5a);
                        wait(0.1);
                        mover_ser(0x04,0x5a);
                        wait(0.1);
                        mover_ser(0x06,0x5a);
                        wait(0.1);
                        mover_ser(0x08,0x5a);
                        wait(0.1);
                        }
            } 
            else
            {
                if (AZUL<ROJO && ROJO>VERDE && VERDE>AZUL)
                {
                    command.printf("Color detectado: AZUL \n");
                    command.printf("ATRAS....\n");
                    
                    
                    for(int x=0;x<5;x++)
                        {
                        mover_ser(0x01,0x00);
                        wait(0.2);
                        mover_ser(0x02,0x5a);
                        wait(0.2);
                        mover_ser(0x01,0x3c);
                        wait(0.2);
                        mover_ser(0x03,0x00);
                        wait(0.2);
                        mover_ser(0x04,0x82);
                        wait(0.2);
                        mover_ser(0x03,0x3c);
                        wait(0.2);
                        mover_ser(0x02,0x32);
                        wait(0.2);
                        mover_ser(0x04,0x5a);
                        wait(0.2);  
                        mover_ser(0x05,0x00);
                        wait(0.2);
                        mover_ser(0x06,0x5a);
                        wait(0.2);
                        mover_ser(0x05,0x3c);
                        wait(0.2);
                        mover_ser(0x07,0x00);
                        wait(0.2);
                        mover_ser(0x08,0x32);
                        wait(0.2);
                        mover_ser(0x07,0x3c);
                        wait(0.2);
                        mover_ser(0x06,0x82);
                        wait(0.2);
                        mover_ser(0x08,0x5a);
                        wait(0.2);  
                        
                        }    
                } 
                else
                {
                    command.printf("otro color \n");
                }
            }
        }
        
        wait(0.5);
  } 
}


unsigned int lectura (void)
{

unsigned int inicio=0, final=0, resultado=0;
    tiempo.start ();        
    while (out) {}
    while (!out) {}
    while (out) {}
    
    inicio= tiempo.read_us();
    while (!out) {}
    final=tiempo.read_us();
    resultado=(final-inicio);
    tiempo.reset ();
    return (resultado);
    
         
}


void saludo (void)
{
    char lado=command.getc();
    switch (lado)
    {
        case  0x01:     command.printf("HOLA!!!!....\n");
                        mover_ser(0x03,0x00);
                        mover_ser(0x07,0x00);
                        mover_ser(0x05,0x5a);
                        wait(0.1);
                        mover_ser(0x01,0x00);
                        wait(0.4);
                        mover_ser(0x02,0x5a);
                        wait(0.4);  
                        mover_ser(0x02,0x0a);
                        wait(0.4); 
                        mover_ser(0x02,0x5a);
                        wait(0.4); 
                        mover_ser(0x02,0x0a);     
                        wait(0.4); 
                        mover_ser(0x02,0x5a);
                        wait(0.4);
                        mover_ser(0x01,0x3c); 
                        mover_ser(0x03,0x3c);
                        mover_ser(0x07,0x3c);
                        mover_ser(0x05,0x3c);
                        wait(0.1);
                        break;
                        
        case  0x02:     command.printf("EMOCIONADO!!!!....\n");
                        mover_ser(0x01,0x00);
                        mover_ser(0x03,0x00);
                        mover_ser(0x05,0x00);
                        mover_ser(0x07,0x00);
                        wait(0.3);
                        mover_ser(0x01,0x3c);
                        mover_ser(0x03,0x3c);
                        mover_ser(0x05,0x3c);
                        mover_ser(0x07,0x3c);
                        wait(0.3);
                        mover_ser(0x01,0x00);
                        mover_ser(0x03,0x00);
                        mover_ser(0x05,0x00);
                        mover_ser(0x07,0x00);
                        wait(0.3);
                        mover_ser(0x01,0x3c);
                        mover_ser(0x03,0x3c);
                        mover_ser(0x05,0x3c);
                        mover_ser(0x07,0x3c);
                        wait(0.3);
                        break;
                        
            case  0x03: command.printf("''GIRANDO''!!!!....\n");
                        mover_ser(0x01,0x00);
                        wait(0.3);
                        mover_ser(0x03,0x00);
                        wait(0.3);
                        mover_ser(0x07,0x00);
                        wait(0.3);
                        mover_ser(0x01,0x46);
                        wait(0.3);
                        mover_ser(0x05,0x00);
                        wait(0.3);
                        mover_ser(0x03,0x46);
                        wait(0.3);
                        mover_ser(0x01,0x00);
                        wait(0.3);
                        mover_ser(0x07,0x46);
                        wait(0.3);
                        mover_ser(0x03,0x00);
                        wait(0.3);
                        mover_ser(0x05,0x46);
                        mover_ser(0x01,0x46);
                        mover_ser(0x03,0x46);
                        mover_ser(0x07,0x46);
                        wait(0.3);
                        break;
                        
                        
        default:    command.printf("error de comando.\n");
                    break ;      
        }
    
    
}


void debug_m(char *s , ... ){
    #if DEBUG
    command.printf(s);
    #endif  
}