Revision 2:e5c7ae640dfd, committed 2018-11-20

Comitter:

andrescas

Date:

Tue Nov 20 02:33:47 2018 +0000

Parent:

1:9430a3a15f7a

Commit message:

ROBOTCUADRUPEDO2018

Changed in this revision

--- a/main.cpp	Wed Nov 14 21:13:59 2018 +0000
+++ b/main.cpp	Tue Nov 20 02:33:47 2018 +0000
@@ -1,37 +1,37 @@
 #include "mbed.h"
 Serial pc(USBTX, USBRX);    // leer comnunicacion serial
-Serial pcBT(D8, D2);        // tx, rx
+//Serial pcBT(D8, D2);        // tx, rx
 AnalogIn analog_value(A0);  // definimos puerto para leer analogamente el joystick
-PwmOut myServo1(PC_7);      // definir puertos de los servos
-PwmOut myServo2(PB_4);
-PwmOut myServo3(PB_5);
-PwmOut myServo4(PB_8);
-PwmOut myServo5(PA_6);
-PwmOut myServo6(PB_9);
-PwmOut myServo7(PA_8);
-PwmOut myServo8(PA_7);
-DigitalOut led(LED1,0);     //led indicador de interrupcion
-DigitalOut S0(PA_13);       //definimos los puertos del sensor
+PwmOut myServo1(PB_5);      // definir puertos de los servos
+PwmOut myServo2(PB_3);
+PwmOut myServo3(PA_5);
+PwmOut myServo4(PA_6);
+PwmOut myServo5(PA_7);
+PwmOut myServo6(PA_8);
+PwmOut myServo7(PA_9);
+PwmOut myServo8(PA_10);
+DigitalOut led(LED1,0);     // led indicador de interrupcion
+DigitalOut S0(PA_13);       // definimos los puertos del sensor
 DigitalOut S1(PA_14);
-DigitalOut S2(PA_15);
+DigitalOut S2(PC_14);
 DigitalOut S3(PC_8);
 DigitalIn  leer(PC_9);      // puerto donde leemos la frecuencia que envia el sensor
 Timer tiempo;               // timer para uso del sensor
-Timeout timeout;            //creamos el objeto timeout
-unsigned int lectura();     //respuesta de lectura de sensor
+Timeout timeout;            // creamos el objeto timeout
+unsigned int lectura();     // respuesta de lectura de sensor
 unsigned int inicio=0, final=0, resultado=0, rojo=0,verde=0,azul=0;// variables que se usan en los procesos
-float meas_r;               //variables del joystick
+float meas_r;               // variables del joystick
 float meas_v;
-void moving();
-void grupomover();
-void mover_ser(uint8_t nmotor, uint8_t pos);
-void timer_interrupt();
-void read_command();        // definimos los 4 numeros hexadecimales enviados
-void read_commandbt();      // definimos los 4 numeros hexadecimales enviados
-void adelante();
-void atras();
-void init_servo();
-char init_c;
+void moving();              // rutina para mover cada servo serialmente
+void grupomover();          // rutina para mover conjunto de servos serialmente
+void mover_ser(uint8_t nmotor, uint8_t pos);    //mover el servo dependiendo los grados leidos
+void timer_interrupt();     // rutina de interrupcion por el Timeout
+void read_command();        // variable donde definimos los 4 numeros hexadecimales enviados
+void read_commandbt();      // variable donde definimos los 4 numeros hexadecimales enviados
+void adelante();            // rutina de movimiento de servos hacia adelante
+void atras();               // rutina de movimiento de servos hacia atras
+void init_servo();          // inicializar los servomotores
+char init_c;                // variables para leer serialmente
 int read_cc;
 uint32_t grados;
 uint32_t nmotor;
@@ -40,41 +40,42 @@
 {
     pc.baud(9600);                              // leer en el monitor serial
     pc.printf("inicio\n");
-    timeout.attach(&timer_interrupt,0.5);       // llamamos la interrupcion del timeout cada 0.5 segundos
+    timeout.attach(&timer_interrupt,0.3);       // llamamos la interrupcion del timeout cada 0.5 segundos para lectura del sensor
     pc.attach(&read_command, Serial::RxIrq);    //llamamos la interrupcion serialmente
-    pcBT.attach(&read_commandbt, Serial::RxIrq);//llamamos la interrupcion serialmente
+    //  pcBT.attach(&read_commandbt, Serial::RxIrq);//llamamos la interrupcion serialmente
     S0=1,S1=0;                                  //calibracion de lectura del sensor
     S2=0,S3=0;
-    init_servo();
+    init_servo();                               // inicializar servos
     while(1) {
-        wait(1);
+        wait(0.2);
         pc.printf("modo joystick\n");
-        meas_r = analog_value.read();            // se lee la entrada analoga del joystick
-        meas_v = meas_r * 3300;                  // se convierte la variable a 3.3v
-        pc.printf("medida = %f = %.0f mV\n", meas_r, meas_v); //muestra en el monitor lectura del joystick
+        meas_r = analog_value.read();                         // se lee la entrada analoga del joystick
+        meas_v = meas_r * 3300;                               // se convierte la variable a 3.3v
+        //pc.printf("medida = %f = %.0f mV\n", meas_r, meas_v); //muestra en el monitor lectura del joystick
+
         if (meas_v < 1000) {
             pc.printf("adelante\n");
             adelante();
-
         }
         if (meas_v > 3000) {
             pc.printf("atras\n");
             atras();
         }
+
     }
 }
 unsigned int lectura()      // lectura del sensor
 {
-    tiempo.start();
+    tiempo.start();         // iniciamos timer
     while(leer) {}
     while(!leer) {}         // inicia desde el inicio de cambio de flanco
     //pc.printf("sale");
     while(leer) {}
-    tiempo.reset();
-    inicio=tiempo.read_us();
+    tiempo.reset();         // reset timer
+    inicio=tiempo.read_us();// leemos el valor de timer cuando inicia el flanco
     while(!leer) {}
-    final=tiempo.read_us();
-    resultado=(final-inicio);
+    final=tiempo.read_us();// leemos el valor del timer al finalizar el flanco
+    resultado=(final-inicio);// se hace una resta de valores obtenidos y asi sabemos la frecuencia
     return resultado;
 }
 void init_servo()                   //inicializamos servomotores
@@ -97,15 +98,15 @@
 }
 void read_command()
 {
-    init_c=pc.getc();
-    if( init_c==0xFF) {
-        read_cc=pc.getc();
-        nmotor=pc.getc();
-        grados=pc.getc();
-        endc=pc.getc();
+    init_c=pc.getc();                   // se leen los datos enviados serialmente y se asignan a una variable
+    if( init_c==0xFF) {                 // se ejecutan las demas lecturas si inicia la trama con 0xff
+        read_cc=pc.getc();              // se lee el segundo dato hexadecimal
+        nmotor=pc.getc();               // se lee el tercer dato hexadecimal
+        grados=pc.getc();               // se lee el cuarto dato hexadecimal
+        endc=pc.getc();                 // se lee el quinto dato hexadecimal
 
-        if(endc==0xFD) {
-            switch (read_cc) {
+        if(endc==0xFD) {                // si el quinto dato no termina en 0xfd no se realiza nada
+            switch (read_cc) {          // ejecutamos rutinas segun el segundo dato hexadecimal
                 case  0x01:
                     moving();
                     break;
@@ -117,10 +118,10 @@
                     break ;
             }
         } else
-            pc.printf("Error de codigo de cierre \n");
+            pc.printf("Error en el codigo de cierre \n");
 
     } else
-        pc.printf("Error de codigo de inicio. \n");
+        pc.printf("Error en el codigo de inicio. \n");
 }
 void read_commandbt()
 {
@@ -144,10 +145,10 @@
                     break ;
             }
         } else
-            pc.printf("Error de codigo de cierre \n");
+            pc.printf("Error en el codigo de cierre \n");
 
     } else
-        pc.printf("Error de codigo de inicio. \n");
+        pc.printf("Error en el codigo de inicio. \n");
 
 }
 void moving()
@@ -160,7 +161,7 @@
 void mover_ser(uint8_t nmotor, uint8_t grados)  // definimos que leen los 8 servos y le asiganamos a cada variable un servo
 {
     int pulsea = coord2us(grados);
-    switch (nmotor) {
+    switch (nmotor) {                           // segun el tercer dato hexadecimal se mueve cada servo
         case  0x01:
             myServo1.pulsewidth_us(pulsea);
             break;                              // caso para cada servo
@@ -191,62 +192,41 @@
     }
 }
 
-void timer_interrupt()   //la respuesta de cada interrupcion externamente por timer
+void timer_interrupt()      //la respuesta de cada interrupcion externamente por timer
 {
     led = !led;
-    S2=0,S3=0;
+    S2=0,S3=0;              // se envia 00 al sensor para que lea el color rojo
     rojo=lectura();         //pc.printf("rojo --%d\n",rojo);
-    S2=1,S3=1;
+    S2=1,S3=1;              // se envia 11 al sensor para que lea el color verde
     verde=lectura();        //pc.printf("verde --%d\n",verde);
-    S2=0,S3=1;
+    S2=0,S3=1;              // se envia 01 al sensor para que lea el color azul
     azul=lectura();         //pc.printf("azul --%d\n",azul);
-   
+
     if (rojo>=100 and rojo <=400) {          // lee el rojo------------
         if (verde>=450 and verde <=700) {
             if (azul>=300 and azul <=600) {
 
-                
-               //POSICION ORIGINAL
-               pc.printf("POSICION ORIGINAL --%d\n",rojo);
-               
-               myServo1.pulsewidth_us(1025);
-wait (0.5); 
-myServo2.pulsewidth_us(1711); 
-wait (0.5); 
-myServo1.pulsewidth_us(1412); 
-wait (0.5); 
-myServo7.pulsewidth_us(1025); 
-wait (0.5); 
-myServo8.pulsewidth_us(1319); 
-wait (0.5); 
-myServo7.pulsewidth_us(1417); 
-wait (0.5); 
-myServo2.pulsewidth_us(1515); 
-myServo8.pulsewidth_us(1515); 
-wait (0.5); 
-myServo3.pulsewidth_us(1221); 
-wait (0.5);
-               /*  myServo2.pulsewidth_us(1800);
-                 wait(0.5);
-                 myServo8.pulsewidth_us(1600);
-                 wait(0.5);
-                 myServo4.pulsewidth_us(1400);
-                 wait(0.5);
-                 myServo6.pulsewidth_us(1600);
-                 wait(0.5);
+                pc.printf("POSICION ORIGINAL --\n");
+                pc.printf("rojo --%d\n",rojo);
+                //POSICION ORIGINAL
+                myServo2.pulsewidth_us(1600);
+                wait(0.1);
+                myServo8.pulsewidth_us(1800);
+                wait(0.1);
+                myServo4.pulsewidth_us(1400);
+                wait(0.1);
+                myServo6.pulsewidth_us(1600);
+                wait(0.1);
 
-               myServo1.pulsewidth_us(1200);
-                 wait(0.5);
-                 myServo3.pulsewidth_us(1200);
-                 wait(0.5);
-                 myServo5.pulsewidth_us(1200);
-                 wait(0.5);
-                 myServo7.pulsewidth_us(1200);
-                 wait(1);*/
-                 
-                 pc.printf("rojo --%d\n",rojo);
-
-             }
+                myServo1.pulsewidth_us(1200);
+                wait(0.1);
+                myServo3.pulsewidth_us(1200);
+                wait(0.1);
+                myServo5.pulsewidth_us(1200);
+                wait(0.1);
+                myServo7.pulsewidth_us(1200);
+                wait(0.1);
+            }
         }
     }
 
@@ -254,52 +234,45 @@
         if (verde>=200 and verde <=450) {
             if (azul>=200 and azul <=450) {
 
-                
-            // GIRO A LA DERECHA
-                pc.printf("GIRO A LA DERECHA --%d\n",verde);             
-                myServo1.pulsewidth_us(1200);
-                myServo3.pulsewidth_us(1200);
-                myServo5.pulsewidth_us(1200);
-                myServo7.pulsewidth_us(1200);
-                wait(0.5); 
-     
-                myServo1.pulsewidth_us(2000);
-                wait(0.5);
+                pc.printf("GIRO A LA DERECHA --\n");
+                pc.printf("verde --%d\n",verde);
+                // GIRO A LA DERECHA
+                myServo7.pulsewidth_us(1600);
+                wait(0.1);
+                myServo3.pulsewidth_us(1600);
+                wait(0.1);
+                myServo8.pulsewidth_us(1200);
+                wait(0.1);
+                myServo4.pulsewidth_us(800);
+                wait(0.1);
+                myServo7.pulsewidth_us(1400);
+                wait(0.1);
+                myServo3.pulsewidth_us(1400);
+                wait(0.1);
+                myServo1.pulsewidth_us(1600);
+                wait(0.1);
+                myServo5.pulsewidth_us(1600);
+                wait(0.1);
                 myServo2.pulsewidth_us(1000);
-                wait(0.5); 
-                myServo1.pulsewidth_us(1200);
-                wait(0.5); 
-    
-                myServo7.pulsewidth_us(2000);
-                wait(0.5);
-                myServo8.pulsewidth_us(1000);
-                wait(0.5); 
-                myServo7.pulsewidth_us(1200);
-                wait(0.5); 
-    
-                myServo5.pulsewidth_us(2000);
-                wait(0.5);
+                wait(0.1);
                 myServo6.pulsewidth_us(1000);
-                wait(0.5); 
-                myServo5.pulsewidth_us(1200);
-                wait(0.5); 
-    
-                myServo3.pulsewidth_us(2000);
-                wait(0.5);
-                myServo4.pulsewidth_us(1000);
-                wait(0.5); 
-                myServo3.pulsewidth_us(1200);
-                wait(0.5);
-    
-   
-                myServo2.pulsewidth_us(2200);
-                myServo6.pulsewidth_us(2200);
-                wait(0.5); 
-                myServo4.pulsewidth_us(2200);
-                myServo8.pulsewidth_us(2200);
-                wait(1); 
+                wait(0.1);
+                myServo1.pulsewidth_us(1400);
+                wait(0.1);
+                myServo5.pulsewidth_us(1400);
+                wait(0.1);
+
+                // POSICION ORIGINAL
+                myServo2.pulsewidth_us(1500);
+                wait(0.1);
+                myServo6.pulsewidth_us(1500);
+                wait(0.1);
+                myServo8.pulsewidth_us(1500);
+                wait(0.1);
+                myServo4.pulsewidth_us(1500);
+                wait(0.1);
+
                 
-                pc.printf("verde --%d\n",verde);
 
             }
         }
@@ -309,64 +282,55 @@
         if (verde>=200 and verde <=500) {
             if (azul>=1 and azul <=400) {
 
-                 
-               
-             // GIRO A LA IZQUIERDA
-             pc.printf("GIRO A LA IZQUIERDA --%d\n",azul);
-             
-             myServo1.pulsewidth_us(1200);
-             myServo3.pulsewidth_us(1200);
-             myServo5.pulsewidth_us(1200);
-             myServo7.pulsewidth_us(1200);
-             wait(1); 
- 
-             myServo1.pulsewidth_us(2000);
-             wait(0.5);
-             myServo2.pulsewidth_us(2200);
-             wait(0.5); 
-             myServo1.pulsewidth_us(1200);
-             wait(0.5); 
-    
-             myServo7.pulsewidth_us(2000);
-             wait(0.5);
-             myServo8.pulsewidth_us(2200);
-             wait(0.5); 
-             myServo7.pulsewidth_us(1200);
-             wait(0.5); 
-    
-             myServo5.pulsewidth_us(2000);
-             wait(0.5);
-             myServo6.pulsewidth_us(2200);
-             wait(0.5); 
-             myServo5.pulsewidth_us(1200);
-             wait(0.5); 
-    
-             myServo3.pulsewidth_us(2000);
-             wait(0.5);
-             myServo4.pulsewidth_us(2200);
-             wait(0.5); 
-             myServo3.pulsewidth_us(1200);
-             wait(0.5);
-    
-             myServo2.pulsewidth_us(1200);
-             myServo6.pulsewidth_us(1200);
-             wait(0.5); 
-             myServo4.pulsewidth_us(1200);
-             myServo8.pulsewidth_us(1200);
-             wait(1); 
-  
-             pc.printf("azul --%d\n",azul);
+                pc.printf("GIRO A LA IZQUIERDA --\n");
+                pc.printf("azul --%d\n",azul);
+                // GIRO A LA IZQUIERDA
+                myServo1.pulsewidth_us(1600);
+                wait(0.1);
+                myServo5.pulsewidth_us(1600);
+                wait(0.1);
+                myServo2.pulsewidth_us(2200);
+                wait(0.1);
+                myServo6.pulsewidth_us(2200);
+                wait(0.1);
+                myServo1.pulsewidth_us(1400);
+                wait(0.1);
+                myServo5.pulsewidth_us(1400);
+                wait(0.1);
+                myServo7.pulsewidth_us(1600);
+                wait(0.1);
+                myServo3.pulsewidth_us(1600);
+                wait(0.1);
+                myServo8.pulsewidth_us(1900);
+                wait(0.1);
+                myServo4.pulsewidth_us(2000);
+                wait(0.1);
+                myServo7.pulsewidth_us(1400);
+                wait(0.1);
+                myServo3.pulsewidth_us(1400);
+                wait(0.1);
+
+                // POSICION ORIGINAL
+                myServo2.pulsewidth_us(1500);
+                wait(0.1);
+                myServo6.pulsewidth_us(1500);
+                wait(0.1);
+                myServo8.pulsewidth_us(1500);
+                wait(0.1);
+                myServo4.pulsewidth_us(1500);
+                wait(0.1);
             }
         }
     }
-    timeout.attach(&timer_interrupt, 0.5); // reiniciamos el timeout para la siguiente lectura
+    timeout.attach(&timer_interrupt, 0.3); // reiniciamos el timeout para la siguiente lectura
 }
 void grupomover()
 {
     pc.printf("mueve grupo \n");
-    switch (nmotor) {
-        case 0x01: {
+    switch (nmotor) {                       // se mueven los servos en conjunto
+        case 0x01: {                        // caso para cada comando
             myServo1.pulsewidth_us(550);
+            wait(0.5);
             myServo2.pulsewidth_us(1800);
             pc.printf("primer caso\n");
             break;
@@ -374,6 +338,7 @@
 
         case 0x02: {
             myServo3.pulsewidth_us(550);
+            wait(0.5);
             myServo4.pulsewidth_us(1800);
             pc.printf("segundo caso \n");
             break ;
@@ -381,6 +346,7 @@
 
         case 0x03: {
             myServo5.pulsewidth_us(550);
+            wait(0.5);
             myServo6.pulsewidth_us(1800);
             pc.printf("tercer caso \n");
             break;
@@ -388,6 +354,7 @@
 
         case 0x04: {
             myServo7.pulsewidth_us(500);
+            wait(0.5);
             myServo8.pulsewidth_us(1800);
             pc.printf("cuarto caso \n");
             break;
@@ -399,123 +366,104 @@
     pc.printf("fin mueve grupo\n");
 
 }
-void adelante()
+void adelante()                     // rutina de movimiento hacia adelante
 {
-//  PASO DERECHA-DELANTERA 
-   
-    myServo8.pulsewidth_us(1000);
-    wait(0.5);
-    myServo7.pulsewidth_us(2000);
-    wait(0.5);
-    myServo8.pulsewidth_us(2000);
-    wait(0.5);
-    myServo7.pulsewidth_us(1200);
-    wait(0.5); 
-    myServo8.pulsewidth_us(1000);
-    wait(0.5);   
+//  PASO DERECHA-DELANTERA Y IZQUIERDA-TRASERA
 
-//  PASO IZQUIERDA-TRASERA
-    
-    myServo4.pulsewidth_us(1600);
-    wait(0.5); 
-    myServo3.pulsewidth_us(2000);
-    wait(0.5);
-    myServo4.pulsewidth_us(800);
-    wait(0.5);
-    myServo3.pulsewidth_us(1200);
-    wait(0.5); 
-    myServo4.pulsewidth_us(1600);
-    wait(0.5); 
+    myServo8.pulsewidth_us(1500);
+    wait(0.1);
+    myServo4.pulsewidth_us(1300);
+    wait(0.1);
+    myServo7.pulsewidth_us(1600);
+    wait(0.1);
+    myServo3.pulsewidth_us(1600);
+    wait(0.1);
+    myServo8.pulsewidth_us(1900);
+    wait(0.1);
+    myServo4.pulsewidth_us(900);
+    wait(0.1);
+    myServo7.pulsewidth_us(1400);
+    wait(0.1);
+    myServo3.pulsewidth_us(1400);
+    wait(0.1);
+    myServo8.pulsewidth_us(1500);
+    wait(0.1);
+    myServo4.pulsewidth_us(1300);
+    wait(0.1);
 
-//  PASO IZQUIERDA-DELANTERA  
-    
-    myServo2.pulsewidth_us(2400);
-    wait(0.5); 
-    myServo1.pulsewidth_us(2000);
-    wait(0.5);
-    myServo2.pulsewidth_us(1200);
-    wait(0.5);
-    myServo1.pulsewidth_us(1200);
-    wait(0.5); 
-    myServo2.pulsewidth_us(2400);
-    wait(0.5);   
+//  PASO IZQUIERDA-DELANTERA Y DERECHA-TRASERA
 
-//  PASO DERECHA-TRASERA
-    
+    myServo2.pulsewidth_us(1700);
+    wait(0.1);
     myServo6.pulsewidth_us(1500);
-    wait(0.5);  
-    myServo5.pulsewidth_us(2000);
-    wait(0.5);
-    myServo6.pulsewidth_us(2300);
-    wait(0.5);
-    myServo5.pulsewidth_us(1200);
-    wait(0.5); 
+    wait(0.1);
+    myServo1.pulsewidth_us(1600);
+    wait(0.1);
+    myServo5.pulsewidth_us(1600);
+    wait(0.1);
+    myServo2.pulsewidth_us(1300);
+    wait(0.1);
+    myServo6.pulsewidth_us(1900);
+    wait(0.1);
+    myServo1.pulsewidth_us(1400);
+    wait(0.1);
+    myServo5.pulsewidth_us(1400);
+    wait(0.1);
+    myServo2.pulsewidth_us(1700);
+    wait(0.1);
     myServo6.pulsewidth_us(1500);
-    wait(0.5);    
+    wait(0.1);
 
 }
 
-void atras()
+
+void atras()                        // rutina de movimiento hacia atras
 {
-//  PASO DERECHA-DELANTERA 
-   
-    myServo8.pulsewidth_us(1000);
-    wait(0.5);
-    myServo7.pulsewidth_us(1200);
-    wait(0.5);
-    myServo8.pulsewidth_us(2000);
-    wait(0.5);
-    myServo7.pulsewidth_us(2000);
-    wait(0.5); 
-    myServo8.pulsewidth_us(1000);
-    wait(0.5);  
-    myServo7.pulsewidth_us(1200);
-    wait(0.5);
+//  PASO DERECHA-DELANTERA Y IZQUIERDA-TRASERA
 
-//  PASO IZQUIERDA-TRASERA
-    
-    myServo4.pulsewidth_us(1600);
-    wait(0.5); 
-    myServo3.pulsewidth_us(1200);
-    wait(0.5);
-    myServo4.pulsewidth_us(800);
-    wait(0.5);
-    myServo3.pulsewidth_us(2000);
-    wait(0.5); 
-    myServo4.pulsewidth_us(1600);
-    wait(0.5); 
-    myServo3.pulsewidth_us(1200);
-    wait(0.5);
+    myServo8.pulsewidth_us(1500);
+    wait(0.1);
+    myServo4.pulsewidth_us(1300);
+    wait(0.1);
+    myServo7.pulsewidth_us(1600);
+    wait(0.1);
+    myServo3.pulsewidth_us(1600);
+    wait(0.1);
+    myServo8.pulsewidth_us(1100);
+    wait(0.1);
+    myServo4.pulsewidth_us(1700);
+    wait(0.1);
+    myServo7.pulsewidth_us(1400);
+    wait(0.1);
+    myServo3.pulsewidth_us(1400);
+    wait(0.1);
+    myServo8.pulsewidth_us(1500);
+    wait(0.1);
+    myServo4.pulsewidth_us(1300);
+    wait(0.1);
 
-//  PASO IZQUIERDA-DELANTERA  
-    
-    myServo2.pulsewidth_us(2400);
-    wait(0.5); 
-    myServo1.pulsewidth_us(1200);
-    wait(0.5);
-    myServo2.pulsewidth_us(1200);
-    wait(0.5);
-    myServo1.pulsewidth_us(2000);
-    wait(0.5); 
-    myServo2.pulsewidth_us(2400);
-    wait(0.5);   
-    myServo1.pulsewidth_us(1200);
-    wait(0.5); 
+
+//  PASO IZQUIERDA-DELANTERA Y DERECHA-TRASERA
 
-//  PASO DERECHA-TRASERA
-    
+    myServo2.pulsewidth_us(1700);
+    wait(0.1);
     myServo6.pulsewidth_us(1500);
-    wait(0.5);  
-    myServo5.pulsewidth_us(1200);
-    wait(0.5);
-    myServo6.pulsewidth_us(2300);
-    wait(0.5);
-    myServo5.pulsewidth_us(2000);
-    wait(0.5); 
+    wait(0.1);
+    myServo1.pulsewidth_us(1600);
+    wait(0.1);
+    myServo5.pulsewidth_us(1600);
+    wait(0.1);
+    myServo2.pulsewidth_us(2100);
+    wait(0.1);
+    myServo6.pulsewidth_us(1100);
+    wait(0.1);
+    myServo1.pulsewidth_us(1400);
+    wait(0.1);
+    myServo5.pulsewidth_us(1400);
+    wait(0.1);
+    myServo2.pulsewidth_us(1700);
+    wait(0.1);
     myServo6.pulsewidth_us(1500);
-    wait(0.5);   
-    myServo5.pulsewidth_us(1200);
-    wait(0.5);
+    wait(0.1);
 
-   
 }