File content as of revision 0:4b10df072322:

#include "mbed.h"
#include "scolor_TCS3200.h"

/*****************************************************************************
generar un programa que controle por el puerto serial el grado de 4 servo motores.
por medio de la comunicacion serial el comando es

|            |            |             |
|   INITCMD  |   N_motor  |   N_grados  |
|     0xff   | 0x01- 0x04 | 0x00 - 0xb4 |


*****************************************************************************/
/*CONSTANTES*/
#define INITCMD 0xFF
#define DEGREES_MAX 180

/*TELECOMANDO*/
#define AUTOMATIC 0x01
#define PINZA 0x02
#define M1_MANUAL 0x03
#define M2_MANUAL 0x04
#define M3_MANUAL 0x05
#define M4_MANUAL 0x06
#define VELOCIDAD 0x07
#define JOYSTICK 0x08
#define COLOR 0x09

/*PARAMETRO*/
#define HOME 0x01
#define MATERIAL 0x02
#define CELDA1 0x03
#define CELDA2 0x04
#define CELDA3 0x05
#define CELDA4 0x06
#define ABRIR_PINZA 0x01
#define CERRAR_PINZA 0x02
#define VELOCIDAD_LENTA 0x01
#define VELOCIDAD_MEDIA 0x02
#define VELOCIDAD_ALTA 0x03
#define INICIA_JOYSTICK 0x01
#define PARA_JOYSTICK 0x02
#define LEE_COLOR 0x00


/*VARIABLES GLOBALES*/
uint8_t N_motor;    // variable almacena el número de motor en leer_datos()
uint8_t N_grados;    // varable almacena los grados que se mueve el motor en leer_datos()

/*VARIABLE RECIBE COMANDOS*/
/*Mueve Base y Eslabon Izquierdo*/
Serial pc(USBTX, USBRX);
unsigned char telecomando;
unsigned char parametro;
unsigned char teleJoys;
unsigned char paramJoys;

/*CONFIGURACIÓN SERVOS*/
PwmOut myservo1(PB_4);//d5 -- BASE
PwmOut myservo2(PB_10);//d6 -- IZQUIERDO
PwmOut myservo3(PA_8);//d7 -- DERECHO
PwmOut myservo4(PA_9);//d8 -- PINZA

/*CONFIGURACIÓN JOYSTICK 1*/
/*Mueve Base y Eslabon Izquierdo*/
AnalogIn analog1_value_x(A0);//A0 -- POSICION X -- BASE
AnalogIn analog1_value_y(A1);//A1 -- POSICION Y -- ESLABON INZQUIERDO
/*Mueve Eslabon Derecho y Pinza*/
AnalogIn analog2_value_x(A2);//A2 -- POSICION X -- ESLABON DERECHO
AnalogIn analog2_value_y(A3);
DigitalIn sw2(PB_6);//D10 -- BOTON JOYSTICK -- 0-ABRE PINZA , 1-CIERRA PINZA

/*CONFIGURACION SENSOR DE COLOR*/
//                     S0,    S1,   S2,   S3,    OUT
//scolor_TCS3200 scolor(PA_8, PB_10, PB_9, PB_5, PB_8);
//                      D2    D3    D14   D4    D15
scolor_TCS3200 scolor(PA_10, PB_3, PB_9, PB_5, PB_8);

/*DEFINICIÓN FUNCIONES*/
void setup_uart();
void setup_servo();
void mover_servo(uint8_t motor, uint8_t gradss);
uint32_t degrees2usec(uint8_t grados);
void motor_velocidad(uint8_t motor, float valor);

//MOVIMIENTO ESLABONES POR POSICION
void mover_base(int posicion);
void mover_izquierdo(int posicion);
void mover_derecho(int posicion);
void mover_pinza(int posicion);

void mover_home();
void mover_material();
void mover_celda1();
void mover_celda2();
void mover_celda3();
void mover_celda4();
void abrir_pinza();
void cerrar_pinza();

void mover_servo(uint8_t motor, uint8_t pos);
void mover_con_joystick();
void leer_color();

/*INICIO DE PROGRAMA*/
int main()
{
    pc.printf("INICIO PROGRAMA...\n\r");
    setup_uart();//Configuración Velocidad de Transmisión
    setup_servo();//Configuración de Motores -> periodo = 20 ms
    
   while(1){
       
       pc.printf("\nEsperando Telecomando...\n");
       while(pc.getc() != INITCMD);//0xFF
       telecomando = pc.getc();
       parametro= pc.getc();
       
       pc.printf("Validando Telecomando: ");
       pc.printf("%02x",telecomando);pc.printf("\n");
       if(telecomando == AUTOMATIC) {
            pc.printf("MOVIMIENTO AUTOMATICO\n");
            
            pc.printf("Validando Parametro: ");
            pc.printf("%02x",parametro);
            switch (parametro) {
                    case  HOME:
                        pc.printf("\nHOME..\n");
                        mover_home();
                        NVIC_SystemReset();
                        break;
                    case  MATERIAL:
                        pc.printf("\nMATERIAL.\n");
                        mover_material();
                        NVIC_SystemReset();
                        break;
                    case  CELDA1:
                        pc.printf("\nCELDA1.\n");
                        mover_celda1();
                        NVIC_SystemReset();
                        break;
                    case  CELDA2:
                        pc.printf("\nCELDA2.\n");
                        mover_celda2();
                        NVIC_SystemReset();
                        break;
                    case  CELDA3:
                        pc.printf("\nCELDA3.\n");
                        mover_celda3();
                        NVIC_SystemReset();
                        break;
                    case  CELDA4:
                        pc.printf("\nCELDA4.\n");
                        mover_celda4();
                        NVIC_SystemReset();
                        break;
                    default:
                        pc.printf("No se encontro parametro valido.\n");
                        break ;
                }//Fin Switch
           
            
       }else if(telecomando == PINZA){
            pc.printf("PINZA\n");
            
            pc.printf("Validando Parametro: ");
            pc.printf("%02x",parametro);
            switch (parametro) {
                    case  ABRIR_PINZA:
                        pc.printf("\nABRE PINZA..\n");
                        abrir_pinza();
                        break;
                    case  CERRAR_PINZA:
                        pc.printf("\nCIERRA PINZA..\n");
                        cerrar_pinza();
                        break;
                    default:
                        pc.printf("No se encontro comando valido.\n");
                        break ;
                }//Fin Switch
            
       }else if(telecomando == M1_MANUAL){
            pc.printf("M1_MANUAL\n");
            
            pc.printf("Validando Parametro: ");
            pc.printf("%02x",parametro);
            
            mover_servo(telecomando, parametro);  
            
       }else if(telecomando == M2_MANUAL){
            pc.printf("M2_MANUAL\n");
            
            pc.printf("Validando Parametro: ");
            pc.printf("%02x",parametro);
            
            mover_servo(telecomando, parametro);  
       }else if(telecomando == M3_MANUAL){
            pc.printf("M3_MANUAL\n");
            
            pc.printf("Validando Parametro: ");
            pc.printf("%02x",parametro);
            
            mover_servo(telecomando, parametro);  
       
       }else if(telecomando == M4_MANUAL){
            pc.printf("M4_MANUAL\n");
            
            pc.printf("Validando Parametro: ");
            pc.printf("%02x",parametro);
            
            mover_servo(telecomando, parametro);  
            
       }else if(telecomando == VELOCIDAD){
            pc.printf("VELOCIDAD\n");
          
            pc.printf("Validando Parametro: ");
            pc.printf("%02x",parametro);
            switch (parametro) {
                    case  VELOCIDAD_LENTA:
                        pc.printf("\nVELOCIDAD_LENTA..\n");
                        break;
                    case  VELOCIDAD_MEDIA:
                        pc.printf("\nVELOCIDAD_MEDIA..\n");
                        break;
                    case  VELOCIDAD_ALTA:
                        pc.printf("\nVELOCIDAD_ALTA..\n");
                        break;
                    default:
                        pc.printf("No se encontro comando valido.\n");
                        break ;
                }//Fin Switch
            
       }else if(telecomando == JOYSTICK){
            pc.printf("JOYSTICK\n");
            
            pc.printf("Validando Parametro: ");
            pc.printf("%02x",parametro);
            switch (parametro) {
                    case  INICIA_JOYSTICK:
                        pc.printf("\nINICIA_JOYSTICK ..\n");
                        mover_con_joystick();
                        break;
                    case  PARA_JOYSTICK:
                        pc.printf("\nPARA_JOYSTICK..\n");
                        break;
                    default:
                        pc.printf("No se encontro comando valido.\n");
                        break ;
                }//Fin Switch
         
            
       }else if(telecomando == COLOR){
            pc.printf("COLOR\n");
            
            leer_color();
      
       }else {
            pc.printf("Telecomando No Encontrado...\n");
       }//Fin Validación Telecomando 
    }//Fin While
}// Fin main

/*CONFIGURA VELOCIDAD DE TRAMSMISIÓN*/
void setup_uart()
{
    pc.baud(9600);
}

/*CONFIGURACIÓN DE SERVOS*/
void setup_servo()
{
    myservo1.period_ms(20);
    myservo2.period_ms(20);
    myservo3.period_ms(20);
    myservo4.period_ms(20);
}


//MOVER BASE
void mover_base(int posicion)
{
    myservo1.pulsewidth_us(posicion);
}
//
//MOVER IZQUIERDO
void mover_izquierdo(int posicion)
{
    myservo2.pulsewidth_us(posicion);
}

//MOVER DERECHO
void mover_derecho(int posicion)
{
    myservo3.pulsewidth_us(posicion);
}

//MOVER PINZA
void mover_pinza(int posicion)
{
    myservo4.pulsewidth_us(posicion);
}


//MUEVE A POSICIÓN HOME
void mover_home()
{
    myservo1.pulsewidth_us(600);
    wait(1.0);
    myservo2.pulsewidth_us(600);
    wait(1.0);
    myservo3.pulsewidth_us(600);
    wait(1.0);
}

//MUEVE A POSICIÓN MATERIAL
void mover_material()
{
    myservo1.pulsewidth_us(700);
    wait(1.0);
    myservo2.pulsewidth_us(650);
    wait(1.0);
    myservo3.pulsewidth_us(630);
    wait(1.0);
}

//MUEVE A POSICIÓN CELDA1
void mover_celda1()
{
    myservo1.pulsewidth_us(800);
    wait(1.0);
    myservo2.pulsewidth_us(700);
    wait(1.0);
    myservo3.pulsewidth_us(660);
    wait(1.0);
}

//MUEVE A POSICIÓN CELDA2
void mover_celda2()
{
    myservo1.pulsewidth_us(900);
    wait(1.0);
    myservo2.pulsewidth_us(750);
    wait(1.0);
    myservo3.pulsewidth_us(690);
    wait(1.0);
}

//MUEVE A POSICIÓN CELDA3
void mover_celda3()
{
    myservo1.pulsewidth_us(1000);
    wait(1.0);
    myservo2.pulsewidth_us(770);
    wait(1.0);
    myservo3.pulsewidth_us(720);
    wait(1.0);
}

//MUEVE A POSICIÓN CELDA4
void mover_celda4()
{
    myservo1.pulsewidth_us(1100);
    wait(1.0);
    myservo2.pulsewidth_us(780);
    wait(1.0);
    myservo3.pulsewidth_us(750);
    wait(1.0);
}

//ABRE PINZA
void abrir_pinza()
{
    myservo4.pulsewidth_us(600);
}

//CIERRA PINZA
void cerrar_pinza()
{
    myservo4.pulsewidth_us(900);
}

// Mover Servo con posicion
void mover_servo(uint8_t motor, uint8_t pos){

    int position = degrees2usec(pos);

     switch (motor) {
      
      case 0x01: 
         myservo1.pulsewidth_us(position);
        break;
      case 0x02: 
         myservo2.pulsewidth_us(position);
        break;    
      case 0x03: 
         myservo3.pulsewidth_us(position);
        break;
      case 0x04: 
         myservo4.pulsewidth_us(position);
        break;   
      default: break;
    }

}

/*CONVERTIR GRADOS A SEGUNDOS*/
uint32_t degrees2usec(uint8_t grados)
{
// Retorna el valor en microsegundos, donde
//      y − y1 = m(x − x1 )
//      y=  b + mx

    if(grados <= DEGREES_MAX)
        return int(750+grados*1900/180);// u6
    return 750;

}

/*MOVIMIENTOS CON JOYSTICK*/
void mover_con_joystick(){
    
    double base = 0.0;
    double izquierdo = 0.0;
    double derecho = 0.0;
    
    int posBase=600;
    int posIzquierdo=600;
    int posDerecho=600;
 
    while(1){
           base = analog1_value_x.read() * 3.3;
           izquierdo = analog1_value_y.read() * 3.3;
           derecho= analog2_value_x.read() * 3.3;
           
           //Valida Joystick 1 en X - base
           if (base < 2.0){
                posBase= posBase+3;
                mover_base(posBase);
           } else if(base > 3.0){
                posBase= posBase-3;
                mover_base(posBase);
           }
           
           //Valida Joystick 1 en Y - Izquierdo
           if (izquierdo < 2){
                posIzquierdo= posIzquierdo+3;
                mover_izquierdo(posIzquierdo);
           } else if(izquierdo > 3){
                posIzquierdo= posIzquierdo-13;
                mover_izquierdo(posIzquierdo);
           }
           
           //Valida Joystic 2 en X - Derecho
           if (derecho < 2){
                posDerecho= posDerecho+3;
                mover_derecho(posDerecho);
           } else if(derecho > 3){
                posDerecho= posDerecho-3;
                mover_derecho(posDerecho);
           }
           
           if(sw2.read() == 0){
               cerrar_pinza();
           } else if(sw2.read() == 1){
               abrir_pinza();
           }
 
    }
}

//LECTURA DE COLOR
void leer_color(){

    long     red;
    long     green;
    long     blue;
    long     clear;

    red = scolor.ReadRed();
    green = scolor.ReadGreen();
    blue = scolor.ReadBlue();
    clear = scolor.ReadClear();
    
    if (red < 100 && green<100 && blue<100){
        pc.printf("COLOR LEIDO: CLARO\n");
        pc.printf("%#04x","04");
        pc.printf("\n");
    }else if (blue<100){
        pc.printf("COLOR LEIDO: AZUL\n");
        pc.printf("%#04x","02");
         pc.printf("\n");
    }else if (red < 100){
        pc.printf("COLOR LEIDO: ROJO\n");
        pc.printf("%#04x","01");
         pc.printf("\n");
    }else if (red > 100 && green>100 && blue>100 && red < 140 && green<140 && blue<140){
        pc.printf("COLOR LEIDO: VERDE\n");
        pc.printf("%#04x","03");
         pc.printf("\n");
    }
    
    pc.printf("RED: %5d     GREEN: %5d     BLUE: %5d     CLEAR: %5d    \n ", red, green, blue, clear);   
}