Diff: main.cpp

Revision:

0:73e805ecf765

Child:

1:59be7d7e1b54

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 10 22:49:18 2018 +0000
@@ -0,0 +1,402 @@
+#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  
+}
\ No newline at end of file