Diff: main.cpp

Revision:

1:59be7d7e1b54

Parent:

0:73e805ecf765

--- a/main.cpp	Wed Oct 10 22:49:18 2018 +0000
+++ b/main.cpp	Tue Nov 20 01:48:09 2018 +0000
@@ -3,13 +3,16 @@
 #include "main.h"
 #define MEM_SIZE 5
 #define MEM_TYPE char
-#define COLOR_SIZE 3
-#define TIEMPO_SIZE 0.2
+#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 command(PC_10,PC_11); //Tx, RX. Usar Bluetooth HC-06.
+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();
 
-DigitalIn button(USER_BUTTON);
 /* //Puertos Sensor color
 //433
 DigitalIn OUT(PC_0);//A5 BC1
@@ -35,47 +38,50 @@
      int Blanco = 0;
  int color = 0;
  // color
+
 int main() {
-    init_servo();
+    //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");
-    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 ;      
-        }
+       //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()
 {
@@ -83,8 +89,9 @@
    // 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 }
@@ -135,11 +142,72 @@
     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");    
 }
 
@@ -167,72 +235,45 @@
     printf("Dir: %x \n",  dire ); 
     //char endcc = buffer[4];
     //printf("Cierre: %x \n",  endcc ); 
-    while(button == 1) { // 433 0 // 446 1
+    //while(button == 1) { // 433 0 // 446 1
     mover_par(par,dire); 
-    }
-    centro(par);
+    //}
+    //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] ) {
+void caminar(int w){
+    debug_m("Caminando..\n"); 
+        switch ( w ) {
   
-                        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");
-        }
+        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){
@@ -329,6 +370,7 @@
                             }else{
                                 //INDEFINIDO
                                 command.printf("Nada ");
+                               
                          
                                 }                             
     if( Rojo > COLOR_SIZE ){ // 
@@ -364,6 +406,7 @@
                             }else{
                                 //INDEFINIDO
                                 command.printf("Contando \n");
+                                
                                 }   
                                                         
     }
@@ -398,5 +441,131 @@
 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;
+    
 }
\ No newline at end of file