File content as of revision 2:ff10ffd246e2:

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

Timer tiempo;
Serial command(USBTX, USBRX);

DigitalIn entrada (PC_9);
DigitalOut S0 (PA_9);
DigitalOut S1 (PA_8);
DigitalOut S3 (PC_7);
DigitalOut S2 (PA_10);

AnalogIn analog_valueX(A0);
AnalogIn analog_valueY(A1);

int valor;
int color=0;
int rojo=0;
int azul=0;
int verde=0;
int Detectar();
int tme=0;

int main() {
    init_servo();
    init_serial();    
    debug_m("inicio \n");
    uint32_t read_cc;   
    while(1)
    {
        read_cc=read_command();        
        switch (read_cc) {
            case  0x01: moving(); break ;             
            case  0x02: moving2();break ;              
            case  0x03: sensor();break ;  
            case  0x04: joystick();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(){
    debug_m("se inicia el comado mover..\n");   
    char nmotor=command.getc();
    char pos=command.getc();
    char endc=command.getc();
    mover_ser(nmotor,pos); 
    debug_m("fin del comado guardar..\n");    
} 
void moving2(){
    debug_m("se inicia el comado mover arana..\n");       
    char nmotor=command.getc();
    char pos=command.getc();
    char endc=command.getc();
    mover_ser2(nmotor,pos); 
    debug_m("fin del comado guardar..\n");    
}

void moving3(){
    debug_m("se inicia el comado caminar adelante..\n");    
    derecha();
    debug_m("fin del comado guardar..\n");    
}

void joystick(){
    
    double posX;
    double posY;
    
    printf("\nJoystick\n");
    
    while(1) {
       
        posX = analog_valueX.read(); 
        posY = analog_valueY.read(); 
    
        posX = posX * 3300; 
        posY = posY * 3300; 
        if (posX > 3000) { 
        debug_m("se inicia el comado mover a la derecha por joystick..\n");  
        derecha();
        }
        
        else if (posX < 1000) { 
        debug_m("se inicia el comado mover a la izquierda por joystick..\n");  
        izquierda();
        }
        
        else if (posY > 3000) { 
        debug_m("se inicia el comado mover a la adelante por joystick..\n");  
        adelante();
        }
        
        else if (posY < 1000) { 
        debug_m("se inicia el comado mover hacia atras por joystick..\n");  
        atras();
        }
        
        else {
        }
        wait(1); // 
    }

   } 


void sensor()
{
    while(1){
        color=command.getc();        
                S0=1;
                S1=1; 
                S2=0;
                S3=0;                
                rojo=Detectar();
                command.printf("ROJO ");
                command.printf("%d  ",rojo);
                S0=1;
                S1=1; 
                S2=0;
                S3=1;
                azul=Detectar();
                command.printf("AZUL ");
                command.printf("%d   ",azul);
                S0=1;
                S1=1;             
                S2=1;
                S3=1;
                verde=Detectar();
                command.printf("VERDE");
                command.printf("%d\n   ",verde);
                
                
                if ((rojo>500)&(rojo<700) & (azul>100)&(azul<200) & (verde>100)&(verde<200)){
                    
                   abajo();
                    
                    
                    command.printf("AZUL \n");}
                    
                    else{
                        if ((rojo>0)&(rojo<200) & (azul>200)&(azul<400) & (verde>200)&(verde<400)){
                            
                           arriba();
                            
                            command.printf("ROJO\n");}
                            
                            else{
                                
                        if ((rojo>400)&(rojo<600) & (azul>200)&(azul<300) & (verde>200)&(verde<300)){
                            
                            adelante();
                            
                            command.printf("VERDE\n");}
                            
                            else{
                                command.printf("COLOR NO VALIDO\n");
                                }
                                }
                            }
   }
   }
    
    int Detectar(){
    tme=0;
    while (!entrada){}
    while (entrada) {}
    while (!entrada){}
    tiempo.start();
    while (entrada) {}
    while (!entrada){}
    while (entrada) {}
    while (!entrada){}
    while (entrada) {}
    tiempo.stop();
    tme=tiempo.read_us();
    tiempo.reset();
    return(tme);
    
    
    }



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