Revision 1:59be7d7e1b54, committed 2018-11-20

Comitter:

sebasmartinez

Date:

Tue Nov 20 01:48:09 2018 +0000

Parent:

0:73e805ecf765

Commit message:

Programa_final;

Changed in this revision

--- 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

--- a/main.h	Wed Oct 10 22:49:18 2018 +0000
+++ b/main.h	Tue Nov 20 01:48:09 2018 +0000
@@ -7,7 +7,7 @@
 void init_serial();
 void moving();
 void pares();
-void caminar();
+void caminar(int w);
 unsigned int lectura(void);
 void borrar(void);
 unsigned int leer_color(void);

--- a/mover.cpp	Wed Oct 10 22:49:18 2018 +0000
+++ b/mover.cpp	Tue Nov 20 01:48:09 2018 +0000
@@ -3,22 +3,17 @@
 #include "math.h"
 /*
 PwmOut Servo1(PB_14); //ROJO
-
 PwmOut Servo2(PB_0); //BLANCO
-
 PwmOut Servo3(PA_11); //VERDE
 PwmOut Servo4(PA_8);  //NEGRO
 PwmOut Servo5(PB_6);  //NARANJA
-
 PwmOut Servo6(PB_1);  //CAFE
-
 PwmOut Servo7(PA_10); //AZUL
-
 PwmOut Servo8(PA_6);  //AMARILLO
 */
 PwmOut Servo1(PA_5); //ROJO
 PwmOut Servo2(PA_6); //BLANCO
-PwmOut Servo3(PA_7); //VERDE
+PwmOut Servo3(PB_2); //VERDE
 PwmOut Servo4(PB_6);  //NEGRO
 PwmOut Servo5(PC_7);  //NARANJA
 PwmOut Servo6(PA_9);  //CAFE
@@ -96,89 +91,49 @@
      switch ( pares ) {
   
   case 0x01: 
-        if(dir==0x01){
-            Servo1.pulsewidth_us(ARRIBA);
-            wait(0.5);
-            Servo2.pulsewidth_us(ADELANTE);
-            wait(0.5);
-            Servo1.pulsewidth_us(ABAJO);
-            wait(0.5);
-            Servo2.pulsewidth_us(ATRAS);
-            wait(0.5);
-         }else{
-            Servo1.pulsewidth_us(ARRIBA);
-            wait(0.5);
-            Servo2.pulsewidth_us(ATRAS);
-            wait(0.5);
-            Servo1.pulsewidth_us(ABAJO);
-            wait(0.5);
-            Servo2.pulsewidth_us(ADELANTE);
-            wait(0.5);
-          }
-    break;
-    case 0x02: 
-        if(dir==0x01){
-            Servo3.pulsewidth_us(ARRIBA);
-            wait(0.5);
-            Servo4.pulsewidth_us(ADELANTE);
-            wait(0.5);
-            Servo3.pulsewidth_us(ABAJO);
-            wait(0.5);
-            Servo4.pulsewidth_us(ATRAS);
-            wait(0.5);
-         }else{
-            Servo3.pulsewidth_us(ARRIBA);
-            wait(0.5);
-            Servo4.pulsewidth_us(ATRAS);
-            wait(0.5);
-            Servo3.pulsewidth_us(ABAJO);
-            wait(0.5);
-            Servo4.pulsewidth_us(ADELANTE);
-            wait(0.5);
-          }
-    break;
-    case 0x03: 
-        if(dir==0x01){
-            Servo5.pulsewidth_us(ARRIBA);
-            wait(0.5);
-            Servo6.pulsewidth_us(ADELANTE);
-            wait(0.5);
-            Servo5.pulsewidth_us(ABAJO);
-            wait(0.5);
-            Servo6.pulsewidth_us(ATRAS);
-            wait(0.5);
-         }else{
-            Servo5.pulsewidth_us(ARRIBA);
-            wait(0.5);
-            Servo6.pulsewidth_us(ATRAS);
-            wait(0.5);
-            Servo5.pulsewidth_us(ABAJO);
-            wait(0.5);
-            Servo6.pulsewidth_us(ADELANTE);
-            wait(0.5);
-          }
-    break;
-    case 0x04: 
-        if(dir==0x01){
-            Servo7.pulsewidth_us(ARRIBA);
-            wait(0.5);
-            Servo8.pulsewidth_us(ADELANTE);
-            wait(0.5);
-            Servo7.pulsewidth_us(ABAJO);
-            wait(0.5);
-            Servo8.pulsewidth_us(ATRAS);
-            wait(0.5);
-         }else{
-            Servo7.pulsewidth_us(ARRIBA);
-            wait(0.5);
-            Servo8.pulsewidth_us(ATRAS);
-            wait(0.5);
-            Servo7.pulsewidth_us(ABAJO);
-            wait(0.5);
-            Servo8.pulsewidth_us(ADELANTE);
-            wait(0.5);
-          }
-    break;
+  if(dir == 0x01){
+      Servo1.pulsewidth_us(ARRIBA);
+      }else if(dir == 0x02){
+          Servo1.pulsewidth_us(ABAJO);
+          }else if(dir == 0x03){
+              Servo2.pulsewidth_us(ADELANTE);
+              }else if(dir == 0x04){
+                  Servo2.pulsewidth_us(ATRAS);
+                  } else{break;} 
+  break;
+  case 0x02:
+    if(dir == 0x01){
+      Servo3.pulsewidth_us(ARRIBA);
+      }else if(dir == 0x02){
+          Servo3.pulsewidth_us(ABAJO);
+          }else if(dir == 0x03){
+              Servo4.pulsewidth_us(ADELANTE);
+              }else if(dir == 0x04){
+                  Servo4.pulsewidth_us(ATRAS);
+                  } else{break;} 
+   break;
+  case 0x03: 
+    if(dir == 0x01){
+      Servo5.pulsewidth_us(ARRIBA);
+      }else if(dir == 0x02){
+          Servo5.pulsewidth_us(ABAJO);
+          }else if(dir == 0x03){
+              Servo6.pulsewidth_us(ADELANTE);
+              }else if(dir == 0x04){
+                  Servo6.pulsewidth_us(ATRAS);
+                  } else{break;} 
+  break;
+  case 0x04: 
+    if(dir == 0x01){
+      Servo7.pulsewidth_us(ARRIBA);
+      }else if(dir == 0x02){
+          Servo7.pulsewidth_us(ABAJO);
+          }else if(dir == 0x03){
+              Servo8.pulsewidth_us(ADELANTE);
+              }else if(dir == 0x04){
+                  Servo8.pulsewidth_us(ATRAS);
+                  } else{break;} 
+  break;
 
      default: break;
 }
@@ -213,6 +168,280 @@
       default: break;
 }
     }    
+void direccion(uint8_t lado){
+    switch ( lado ) {
+  
+  case 0x01: //Derecha
+            
+            Servo2.pulsewidth_us(ADELANTE);
+            Servo1.pulsewidth_us(ARRIBA);
+            wait_ms(300);
+            Servo1.pulsewidth_us(ABAJO);
+            wait_ms(500);
+            
+            Servo6.pulsewidth_us(ADELANTE);
+            Servo5.pulsewidth_us(ARRIBA);
+            wait_ms(300);
+            Servo5.pulsewidth_us(ABAJO);
+            wait_ms(500);
+            
+            Servo4.pulsewidth_us(ADELANTE);
+            Servo3.pulsewidth_us(ARRIBA);
+            wait_ms(300);
+            Servo3.pulsewidth_us(ABAJO);
+            wait_ms(500);
+            
+            Servo8.pulsewidth_us(ADELANTE);
+            Servo7.pulsewidth_us(ARRIBA);
+            wait_ms(300);
+            Servo7.pulsewidth_us(ABAJO);
+            wait_ms(500);
+
+            Servo2.pulsewidth_us(CENTRO);
+            Servo4.pulsewidth_us(CENTRO);
+            Servo6.pulsewidth_us(CENTRO);
+            Servo8.pulsewidth_us(CENTRO);
+            wait(1);
+    break;
+  case 0x02: //Izquierda
+            Servo2.pulsewidth_us(ATRAS);
+            Servo1.pulsewidth_us(ARRIBA);
+            wait_ms(300);
+            Servo1.pulsewidth_us(ABAJO);
+            wait_ms(500);
+            
+            Servo6.pulsewidth_us(ATRAS);
+            Servo5.pulsewidth_us(ARRIBA);
+            wait_ms(300);
+            Servo5.pulsewidth_us(ABAJO);
+            wait_ms(500);
+            
+            Servo4.pulsewidth_us(ATRAS);
+            Servo3.pulsewidth_us(ARRIBA);
+            wait_ms(300);
+            Servo3.pulsewidth_us(ABAJO);
+            wait_ms(500);
+            
+            Servo8.pulsewidth_us(ATRAS);
+            Servo7.pulsewidth_us(ARRIBA);
+            wait_ms(300);
+            Servo7.pulsewidth_us(ABAJO);
+            wait_ms(500);
+
+            Servo2.pulsewidth_us(CENTRO);
+            Servo4.pulsewidth_us(CENTRO);
+            Servo6.pulsewidth_us(CENTRO);
+            Servo8.pulsewidth_us(CENTRO);
+            wait(1);
+    break; 
+    case 0x03: // Adelante
+        Servo1.pulsewidth_us(1500);//arriba
+        Servo5.pulsewidth_us(1500);//arriba
+        wait_ms(80);
+        Servo2.pulsewidth_us(1100);//adelante
+        Servo6.pulsewidth_us(900);//adelante
+        wait_ms(80);
+        Servo1.pulsewidth_us(2000);//abajo
+        Servo5.pulsewidth_us(2000);//abajo
+        wait_ms(80);
+        Servo2.pulsewidth_us(900);//atras
+        Servo6.pulsewidth_us(1200);//atras
+        wait_ms(80);        
+        Servo3.pulsewidth_us(1500);//arriba
+        Servo7.pulsewidth_us(1500);//arriba
+        wait_ms(80);
+        Servo4.pulsewidth_us(2000);//adelante
+        Servo8.pulsewidth_us(1900);//adelante
+        wait_ms(80);
+        Servo3.pulsewidth_us(2000);//abajo
+        Servo7.pulsewidth_us(2000);//abajo
+        wait_ms(80);
+        Servo4.pulsewidth_us(1700);//atras
+        Servo8.pulsewidth_us(2100);//atras
+        wait_ms(80);
+
+            
+    break; 
+    case 0x04: //Atras
+    
+        Servo1.pulsewidth_us(2000);//abajo
+        Servo5.pulsewidth_us(2000);//abajo
+        wait_ms(80);
+        Servo2.pulsewidth_us(1100);//adelante
+        Servo6.pulsewidth_us(900);//adelante
+        wait_ms(80);
+        Servo1.pulsewidth_us(1500);//arriba
+        Servo5.pulsewidth_us(1500);//arriba
+        wait_ms(80);
+        Servo2.pulsewidth_us(900);//atras
+        Servo6.pulsewidth_us(1200);//atras
+        wait_ms(80);       
+
+        Servo3.pulsewidth_us(2000);//abajo
+        Servo7.pulsewidth_us(2000);//abajo
+        wait_ms(80);
+        Servo4.pulsewidth_us(2000);//adelante
+        Servo8.pulsewidth_us(1900);//adelante
+        wait_ms(80);      
+        Servo3.pulsewidth_us(1500);//arriba
+        Servo7.pulsewidth_us(1500);//arriba
+        wait_ms(80);
+        Servo4.pulsewidth_us(1700);//atras
+        Servo8.pulsewidth_us(2100);//atras
+        wait_ms(80);
+ 
+
+
+            
+    break;  
+    case 0x05: //Saludar1
+         Servo2.pulsewidth_us(CENTRO);
+         Servo8.pulsewidth_us(FRONT);
+         Servo4.pulsewidth_us(BACK);
+         Servo6.pulsewidth_us(CENTRO);
+            
+         Servo3.pulsewidth_us(ABAJO);
+         Servo5.pulsewidth_us(ABAJO);
+         Servo7.pulsewidth_us(ABAJO);
+         wait_ms(500);
+         Servo1.pulsewidth_us(ARRIBA);
+         Servo2.pulsewidth_us(ADELANTE);
+         wait_ms(500);
+         Servo2.pulsewidth_us(ATRAS);
+         wait_ms(500);
+         Servo2.pulsewidth_us(ADELANTE);
+         wait_ms(500);
+         Servo2.pulsewidth_us(ATRAS);
+         wait_ms(500);
+         Servo2.pulsewidth_us(ADELANTE);
+         wait_ms(500);
+         Servo2.pulsewidth_us(ATRAS);
+         wait_ms(500);
+         Servo2.pulsewidth_us(CENTRO);
+         Servo1.pulsewidth_us(ABAJO);
+         Servo4.pulsewidth_us(CENTRO);
+         Servo8.pulsewidth_us(CENTRO);
+         wait_ms(2000);
+         
+         
+    break;  
+        case 0x06: //Saludar2
+            Servo1.pulsewidth_us(ARRIBA);
+            Servo3.pulsewidth_us(ARRIBA);
+            Servo7.pulsewidth_us(ABAJO);
+            Servo5.pulsewidth_us(ABAJO);
+            
+            Servo2.pulsewidth_us(FRONT);
+            Servo4.pulsewidth_us(FRONT);
+            Servo8.pulsewidth_us(FRONT);
+            Servo6.pulsewidth_us(FRONT);
+            wait_ms(500);
+            Servo1.pulsewidth_us(ABAJO);
+            Servo3.pulsewidth_us(ABAJO);
+            Servo3.pulsewidth_us(ABAJO);
+            Servo7.pulsewidth_us(ARRIBA);
+            Servo5.pulsewidth_us(ARRIBA);
+            
+            Servo8.pulsewidth_us(BACK);
+            Servo6.pulsewidth_us(BACK);
+            Servo2.pulsewidth_us(BACK);
+            Servo4.pulsewidth_us(BACK);
+            wait_ms(500);
+    break;
+    
+    case 0x07: //Pruebas
+    
+        Servo1.pulsewidth_us(2000);//abajo
+        Servo5.pulsewidth_us(2000);//abajo
+        wait_ms(80);
+        Servo2.pulsewidth_us(1100);//adelante
+        Servo6.pulsewidth_us(900);//adelante
+        wait_ms(80);
+        Servo1.pulsewidth_us(1500);//arriba
+        Servo5.pulsewidth_us(1500);//arriba
+        wait_ms(80);
+        Servo2.pulsewidth_us(900);//atras
+        Servo6.pulsewidth_us(1200);//atras
+        wait_ms(80);       
+
+        Servo3.pulsewidth_us(2000);//abajo
+        Servo7.pulsewidth_us(2000);//abajo
+        wait_ms(80);
+        Servo4.pulsewidth_us(2000);//adelante
+        Servo8.pulsewidth_us(1900);//adelante
+        wait_ms(80);      
+        Servo3.pulsewidth_us(1500);//arriba
+        Servo7.pulsewidth_us(1500);//arriba
+        wait_ms(80);
+        Servo4.pulsewidth_us(1700);//atras
+        Servo8.pulsewidth_us(2100);//atras
+        wait_ms(80);
+
+
+               
+    break;
+      default: break;
+}
+    }
+void arrancar()
+{
+    Servo1.pulsewidth_us(ABAJO);
+    Servo3.pulsewidth_us(ABAJO);
+    Servo5.pulsewidth_us(ABAJO);
+    Servo7.pulsewidth_us(ABAJO);
+wait_ms(1000);
+    Servo1.pulsewidth_us(ARRIBA);
+wait_ms(500);
+Servo1.pulsewidth_us(ABAJO);
+wait_ms(500);
+Servo1.pulsewidth_us(CENTRO);
+wait_ms(500);
+Servo2.pulsewidth_us(ADELANTE);
+wait_ms(500);
+Servo2.pulsewidth_us(ATRAS);
+wait_ms(500);
+Servo2.pulsewidth_us(CENTRO);
+wait_ms(500);
+
+Servo3.pulsewidth_us(ARRIBA);
+wait_ms(500);
+Servo3.pulsewidth_us(ABAJO);
+wait_ms(500);
+Servo3.pulsewidth_us(CENTRO);
+wait_ms(500);
+Servo4.pulsewidth_us(ADELANTE);
+wait_ms(500);
+Servo4.pulsewidth_us(ATRAS);
+wait_ms(500);
+Servo4.pulsewidth_us(CENTRO);
+wait_ms(500);
+
+Servo5.pulsewidth_us(ARRIBA);
+wait_ms(500);
+Servo5.pulsewidth_us(ABAJO);
+wait_ms(500);
+Servo5.pulsewidth_us(CENTRO);
+wait_ms(500);
+Servo6.pulsewidth_us(ADELANTE);
+wait_ms(500);
+Servo6.pulsewidth_us(ATRAS);
+wait_ms(500);
+Servo6.pulsewidth_us(CENTRO);
+wait_ms(500);
+
+Servo7.pulsewidth_us(ARRIBA);
+wait_ms(500);
+Servo7.pulsewidth_us(ABAJO);
+wait_ms(500);
+Servo7.pulsewidth_us(CENTRO);
+wait_ms(500);
+Servo8.pulsewidth_us(ADELANTE);
+wait_ms(500);
+Servo8.pulsewidth_us(ATRAS);
+wait_ms(500);
+Servo8.pulsewidth_us(CENTRO);
+wait_ms(500);
+    }   
 void init_servo()
 {
    Servo1.period_ms(20);

--- a/mover.h	Wed Oct 10 22:49:18 2018 +0000
+++ b/mover.h	Tue Nov 20 01:48:09 2018 +0000
@@ -4,11 +4,15 @@
 #define MAXPOS 180       // en milimetros
 //#define POSDRAW 50  
 //#define POSNODRAW 10  
-#define ARRIBA 1700
+#define ARRIBA 600
 #define CENTRO 1500
-#define ABAJO 600
-#define ADELANTE 720
+#define ABAJO 1700
+#define ADELANTE 800
 #define ATRAS 2200
+#define UP 700
+#define DOWN 1500
+#define FRONT 1000
+#define BACK 1750
 
 void init_servo();
 void draw();
@@ -16,5 +20,7 @@
 void mover_ser(uint8_t motor, uint8_t pos);
 void mover_par(uint8_t pares, uint8_t dir);
 void centro(uint8_t pos_p);
+void direccion(uint8_t lado);
+void arrancar();
 
 #endif //  DRAW_H 
\ No newline at end of file