Archivos para agosto, 2016

Los robots seguidores de línea son robots muy sencillos, que cumplen una única misión: seguir una línea marcada en el suelo normalmente de color negro sobre un tablero blanco (normalmente una línea negra sobre un fondo blanco). Son considerados los “Hola mundo” de la robótica.

Estos robots pueden variar desde los más básicos (van tras una línea única) hasta los robots que recorren laberintos, como el que construiremos en esta publicación. Todos ellos, sin embargo, poseen (por lo general) ciertas partes básicas comunes entre todos:

Sensores: Un rastreador detecta la línea a seguir por medio de sensores. Hay muchos tipos de sensores que se pueden usar para este fin; sin embargo, por razones de costos y practicidad, los más comunes son los sensores infrarrojos (IR), que normalmente constan de un LED infrarrojo y un fototransistor, la línea a seguir, puede ser de color negro con fondo blanco o línea blanca con fondo negro. Nuestro robot ofrecerá ambas opciones modificando simplemente un par de líneas en su código fuente. Utilizaremos 4 módulos detectores cuyo componente principal será el sensor óptico reflectivo TCRT5000 fabricado por Vishay Semiconductors, el cual incluye un filtro que bloquea la luz visible (luz ambiente) lo cual nos evitará varios dolores de cabeza y riesgos de ACV. En las siguientes imágenes observamos el sensor y su correspondiente módulo ya ensamblado. La tensión de alimentación es de 5V que obtendremos de nuestra tarjeta de control Arduino UNO R3 y dicho módulo entrega un valor lógico alto [1] cuando no se refleja la luz infrarroja en la superficie y un valor lógico bajo [0] cuando existe reflejo.

Motores: El robot se mueve utilizando motores. Dependiendo del tamaño, el peso, la precisión del motor, entre otros factores, éstos pueden ser de varias clases: motores de corriente continua (nuestro robot), motores paso a paso o servomotores liberados para giro continuo.

Ruedas: Las ruedas del robot son movidas por los motores. Normalmente se usan ruedas de materiales anti-deslizantes para evitar fallas de tracción. Su tamaño es otro factor a tener en cuenta a la hora de armar el robot.

Fuente de energía: El robot obtiene la energía que necesita para su funcionamiento de baterías o de una fuente de corriente alterna, siendo esta última menos utilizada debido a que le resta independencia al robot.

Tarjeta de control: La toma de decisiones y el control de los motores están generalmente a cargo de un microcontrolador. La tarjeta de control contiene dicho elemento, junto a otros componentes electrónicos básicos que requiere el microcontrolador para funcionar. Utilizaremos una tarjeta de control Arduino Uno R3 en nuestro robot.

Todos los rastreadores basan su funcionamiento en los sensores. Sin embargo, dependiendo de la complejidad del recorrido, el robot debe ser más o menos complejo (y, por ende, utilizar más o menos sensores).

Los rastreadores más simples utilizan 2 sensores, ubicados en la parte inferior de la estructura, uno junto al otro. Cuando uno de los dos sensores detecta el color blanco, significa que el robot está saliendo de la línea negra por ese lado. En ese momento, el robot gira hacia el lado contrario hasta que vuelve a estar sobre la línea. Esto en el caso de los seguidores de línea negra, ya que también hay seguidores de línea blanca. Como se mencionó anteriormente, nuestro robot ofrecerá ambas opciones modificando simplemente un par de líneas en su código fuente.

Lista de Componentes
1 Plataforma Turtle2WD con placa Arduino UNO R3 y Shield de Control de Motores ensamblados anteriormente.
4 Módulos Detectores con Sensor Óptico Reflectivo TCRT5000.
4 Separadores Plásticos 5mm
1 Papel Afiche 100cm x 70cm
1 Rollo de Cinta de Papel Color Blanco 10mm

En primer lugar debemos instalar los cuatro módulos detectores en la parrilla de montaje de sensores de la plataforma Turtle2WD como se observa en las siguientes imágenes:

(click para ampliar)

(click para ampliar)

(click para ampliar)

(click para ampliar)

IMPORTANTE: lo que haremos a continuación es crucial para el correcto funcionamiento del robot. Conectaremos a la placa Arduino los cuatro módulos detectores respetando la numeración y nomenclatura utilizada a continuación que se corresponde con el código fuente provisto, de lo contrario el funcionamiento del robot será errático.

Vista trasera de la plataforma Turtle2WD, de izquierda a derecha (parte frontal de la plataforma: portapilas).

Sensor Izquierdo Externo (Salida “D0”)  –>  A2
Sensor Izquierdo Interno (Salida “D0”)  –>  A3
Sensor Derecho Interno (Salida “D0”)    –>  A4
Sensor Derecho Externo (Salida “D0”)    –>  A5

Los pines “A0” de todos los módulos quedan sin conexión ya que son las salidas analógicas de los sensores y no las utilizaremos en este caso.

Los pines “GND” y “VCC” (5V), los conectaremos a sus correspondientes pines “GND” y “5V” en la placa Arduino para suministrarles una tensión de 5V a cada módulo.

Habiendo conectado correctamente los cuatro sensores reflectivos, sólo resta compilar y cargar en nuestro Arduino UNO R3 el sketch correspondiente, cuyo código fuente se encuentra a continuación:

/*
Caracteristicas:
- Seguidor de Linea (LINEA BLANCA/FONDO NEGRO) o (LINEA NEGRA/FONDO BLANCO).
*/

// Control de Motores: Direccion y Velocidad
// Vista desde atras del movil, de izquierda a derecha
// Parte frontal del movil: Portapilas
#define     PWM_RIGHT   3     // (Control Velocidad Motor Derecho) Timer2 8 bits D3
#define     PWM_LEFT    11    // (Control Velocidad Motor Izquierdo) Timer2 8bits D11
#define     DIR_A       12    // (Control de Direccion de Motores) D12
#define     DIR_B       13    // (Control de Direccion de Motores) D13
// -----

// Sensores Reflectivos
// Vista desde atras del movil, de izquierda a derecha
// Parte frontal del movil: Portapilas
#define     SENSOR_LO   A2    // (Sensor Izquierdo Externo)
#define     SENSOR_LI   A3    // ((Sensor Izquierdo Interno))
#define     SENSOR_RI   A4    // (Sensor Derecho Interno)
#define     SENSOR_RO   A5    // (Sensor Derecho Externo)
// -----

// -----

#define    WEIGHT_LO    8
#define    WEIGHT_LI    4
#define    WEIGHT_RI    2
#define    WEIGHT_RO    1

#define    VMAX         90 //PWM
#define    REDUCTF      0.1f

#define    BLACK        1
#define    WHITE        0

#define    BACKGROUND   BLACK
#define    LINE         WHITE

byte state, leftwheel, rightwheel;

void scan() {
    state = ((digitalRead(SENSOR_LO) == LINE) * WEIGHT_LO) + ((digitalRead(SENSOR_LI) == LINE) * WEIGHT_LI) + ((digitalRead(SENSOR_RI) == LINE) * WEIGHT_RI) + ((digitalRead(SENSOR_RO) == LINE) * WEIGHT_RO);
}

void forward() {
    rightwheel = VMAX;
    leftwheel = VMAX;
    analogWrite(PWM_RIGHT, rightwheel);
    analogWrite(PWM_LEFT, leftwheel);
}

void turnleft() {
    leftwheel = 0;
    rightwheel = VMAX;
    analogWrite(PWM_RIGHT, rightwheel);
    analogWrite(PWM_LEFT, leftwheel);
    
	do {
        scan();
    } while(state != 2 && state != 6);
    //} while(state < 8);
    //} while(state != 2;
}

void turnright() {
    leftwheel = VMAX;
    rightwheel = 0;
    analogWrite(PWM_RIGHT, rightwheel);
    analogWrite(PWM_LEFT, leftwheel);

    do {
        scan();
    } while(state != 4 && state != 6);
    //} while(state != 4 && state != 6);
    //} while(state != 4 && state != 6);
}

void leftcorrection() {
    leftwheel = leftwheel - leftwheel * REDUCTF;
    rightwheel = VMAX;
    analogWrite(PWM_RIGHT, rightwheel);
    analogWrite(PWM_LEFT, leftwheel);
}

void rightcorrection() {
    rightwheel = rightwheel - rightwheel * REDUCTF;
    leftwheel = VMAX;
    analogWrite(PWM_RIGHT, rightwheel);
    analogWrite(PWM_LEFT, leftwheel);
}

void setup() {
    pinMode(PWM_RIGHT, OUTPUT);
    pinMode(PWM_LEFT, OUTPUT);
    pinMode(DIR_A, OUTPUT);
    pinMode(DIR_B, OUTPUT);

    pinMode(SENSOR_LO, INPUT);
    pinMode(SENSOR_LI, INPUT);
    pinMode(SENSOR_RI, INPUT);
    pinMode(SENSOR_RO, INPUT);

    digitalWrite(DIR_A, LOW);
    digitalWrite(DIR_B, LOW);
}

void loop() {
    scan();

    switch (state) {
        case 0:
            // 0000 -> FORWARD
            forward();
            break;

        case 1:
            // 0001 RIGHT CURVE -> TURN RIGHT
            turnright();
            break;

        case 2:
            // 0010 GOING LEFT -> RIGHT CORRECTION
            rightcorrection();
            break;

        case 3:
            // 0011 RIGHT CURVE -> TURN RIGHT
            turnright();
            break;

        case 4:
            // 0100 GOING RIGHT -> LEFT CORRECTION
            leftcorrection();
            break;

        // case 5:
            // // 0101 ???
            // break;

        // case 6:
            // 0110 GOING TO AN UNDEFINED SIDE -> NOTHING TO DO HERE
            // forward();
            // break;

        case 7:
            // 0111 RIGHT CURVE -> TURN RIGHT
            turnright();
            break;

        case 8:
            // 1000 LEFT COURVE -> TURN LEFT
            turnleft();
            break;

        case 9:
            // 1001
            turnright();
            break;

        // case 10:
            // 1010 ??? SHOULD NEVER HAPPEN SAME AS 5
            // break;

        // case 11:
            // // 1011 ??? SAME AS 13
            // break;

        case 12:
            // 1100 LEFT COURVE -> TURN LEFT
            turnleft();
            break;

        // case 13:
            // 1101 ??? SAME AS 11
            // break;

        case 14:
            // 1110 LEFT CURVE -> TURN LEFT SAME AS 7
            turnleft();
            break;

        case 15:
            // 1111 T -> TURN TO RIGHT
            turnright();
            break;

        default:
            // KEEP STATUS
            break;
    }
}

Finalmente, pegando la cinta de papel sobre el afiche diseñaremos el recorrido a realizar por nuestro robot, que puede ser desde una simple línea recta, un bucle en forma de “O”, o un laberinto como el que observamos en el vídeo que se encuentra la final de la publicación.
Antes de accionar el interruptor de encendido para que nuestro robot entre en acción debemos ubicarlo sobre el punto de partida de la siguiente manera:

(click para ampliar)

Anuncios

Dejando de lado, solo momentáneamente, los juegos construidos con Arduino publicados en las últimas cuatro entradas de este blog, retomaremos los proyectos de robótica ya que desde hace tiempo no hice publicaciones al respecto.

En esta ocasión quiero compartir la construcción de mi último robot, se trata de un robot araña cuadrúpedo con 8 DOF (Degrees Of Freedom – Grados De Libertad) construido con un total de nueve servomotores “TowerPro SG90”, de los cuales ocho están destinados a los movimientos y articulaciones de cada una de las patas y un servomotor adicional para controlar los movimientos de la cabeza del robot, en la cual se encuentra instalado un Sensor UltrasónicoHC-SR04” para detectar y evadir obstáculos. Para comandar nuestra araña emplearemos un Joystick Inalámbrico de Playstation 2 que seguramente tenemos alguno juntando tierra en algún cajón o bien podemos conseguir una versión genérica del mismo a un bajo costo. Este joystick es sencillo de conectar a nuestra placa Arduino ya que utiliza el protocolo SPI (Serial Peripheral Interface) y posee todas las características necesarias para comandar nuestro robot.

En el sitio web de Kaiwa Technology encontré un kit que se ajusta perfectamente a nuestros requerimientos. Si bien he realizado modificaciones personales que permiten obtener un mejor rendimiento, debo decir que el resultado final de este proyecto ha satisfecho ampliamente mis expectativas desde que decidí comenzar con la construcción de este robot. El kit es muy completo e incluye todos los componentes necesarios para el armado del mismo sin mayores dificultades. Los componentes que encontraremos en este kit son los siguientes:

Componentes de Electrónica

  • 1 Arduino Nano V3.0 (clon)
  • 1 Arduino Nano Shield
  • 1 Sensor Ultrasónico “HC-SR04
  • 9 Servomotores “TowerPro SG90
  • 1 Portabaterías AA x 4
  • 1 Cable USB 2.0 A Macho – Mini USB B Macho

Piezas Plásticas Impresas en 3D

  • 1 Cuerpo de Araña
  • 8 Piezas en “U
  • 4 Patas
  • 1 Cabeza

Además de los componentes incluidos en el kit, precisaremos los siguientes componentes adicionales NO INCLUIDOS en el kit:

  • 1 Joystick Inalámbrico de Playstation 2
  • 2 Capacitores Electrolíticos 1000uF 10V
  • 1 Capacitor Electrolítico 220uF 10V
  • 1 Capacitor Cerámico 100nF 50v
  • 1 Controlador de Posición de Servomotores
  • 4 Baterías Recargables AA Ni-Mh 1.2V 2700mAh
  • 1 Controlador de Posición de Servomotores
  • 1 Interruptor Tipo Palanca SPST 3A 250VAC
  • 1 Pomo de Pegamento “UHU” 35ml
  • 1 Lata de Adhesivo de Contacto EspecialFortex” 125cm3
  • Librería NewPing desarrollada por Tim Ecker
  • Librería Arduino-PS2X desarrollada por Bill Porter

En la siguiente imagen podemos observar los componentes incluidos en el kit:

(click para ampliar)

IMPORTANTE: los servomotores que forman parte del cuerpo llevan un solo tornillo al chasis, en caso de detectar que no sujetan lo suficiente, los mismos pueden ser pegados con pegamento “UHU“.

En la siguiente imagen observamos los primeros cuatro servomotores que se montarán en el cuerpo:

(click para ampliar)

Cada servo tiene su posición y numeración. Colocamos los 4 servos en sus lugares correspondientes, cada uno con su tornillo y pegamento “UHU” del lado opuesto que no posee tornillo para asegurarlo lo suficiente.

(click para ampliar)

A continuación ensamblamos las piezas en “U“, de manera tal que coincidan las tres perforaciones y las fijamos con sus tres tornillos y tuercas correspondientes como se muestra en la siguiente imagen:

(click para ampliar)

IMPORTANTE: antes de colocar cualquiera de las partes de los servomotores, debemos buscar la posición central del servomotor. Para lograr nuestro objetivo utilizaremos el Controlador de Posición de Servomotores. Alimentaremos el Controlador con una tensión de 5V, conectaremos el servomotor al Controlador y pulsaremos el botón “Select” del controlador hasta que el LED azul nos indique que se encuentra en la posición “Neutral” como se observa en la siguiente imagen:

(click para ampliar)

(click para ampliar)

Repetiremos la misma operación para los 3 servomotores restantes, de esta manera quedarán ubicados en su posición central.
Luego, centraremos las piezas en “U” de manera tal que las mismas queden en paralelo a los servomotores y luego colocaremos los brazos plásticos para fijarlas a cada servomotor.

(click para ampliar)

De la parte inferior colocamos los tornillos de soporte, y los ajustamos hasta que ejerzan cierta presión contra el servomotor. No presionar demasiado ya que el plástico no posee rosca.

(click para ampliar)

Ahora comenzaremos a ensamblar las patas, colocando un servomotor en cada una de las mismas. El eje del servomotor debe quedar en el extremo opuesto a la pata. Fijamos cada servomotor a cada una de las patas con su tornillo y pegamento “UHU” del lado opuesto que no posee tornillo para asegurarlo lo suficiente.

(click para ampliar)

A continuación instalaremos las patas teniendo en cuenta la numeración correspondiente. Cada pata se coloca de la misma manera como la pieza en “U” al servomotor del cuerpo central.

(click para ampliar)

Ahora instalaremos el último servomotor donde posteriormente fijaremos el Sensor Ultrasónico que se encuentra instalado en la pieza plástica que conforma el rostro de nuestro robot araña.

(click para ampliar)

Luego, fijaremos el brazo plástico más largo del servomotor al soporte (rostro de la araña) del Sensor Ultrasónico, como observamos en las siguientes imágenes:

(click para ampliar)

(click para ampliar)

Buscamos la posición neutral de este servomotor como ya lo hicimos anteriormente con los ocho servomotores anteriores y procedemos a atornillar el Sensor Ultrasónico.

(click para ampliar)

Imagen completa del robot y todas sus partes ensambladas con la nomenclatura correspondiente:

(click para ampliar)

A continuación pegaremos el portabaterías a la parte inferior del cuerpo de nuestro robot utilizando el Adhesivo de Contacto EspecialFortex” ya que se requiere una fijación resistente para soportar el peso de las baterías y la manipulación de las mismas. En la siguiente imagen se observa el portabaterías ya instalado:

(click para ampliar)

Habiendo ensamblado exitosamente la estructura siguiendo los pasos anteriores, en este punto comenzaremos con el montaje de la parte electrónica.
Un servomotor consta de un pequeño motor que hace girar una serie de engranajes dispuestos en árbol para aumentar el torque final del ultimo engranaje, esto es simplemente una “caja reductora”, en consecuencia, cuando se modifica el ángulo del servomotor lo que realmente se hace es enviar tensión al motor en forma de pulsos, “esto sucede internamente dentro del servo”. Si el brazo o leva del servomotor está sometido a una gran fuerza también consumirá más energía/corriente, ya que para mantener la posición demandada el motor interno estará todo el tiempo contrarrestando la fuerza que se ejerce sobre su brazo a base de ligeros impulsos de corriente hacia el motor interno.
Por lo tanto, necesitamos asegurarnos que no les faltará energía a los nueve servos que componen nuestro robot. Para lograr este objetivo instalaremos en el lateral izquierdo del portabaterías dos capacitores electrolíticos de 1000uF 10V conectados en paralelo, obteniendo en total 2000uF 10V. Luego soldaremos los cables de alimentación provenientes del portabaterías en paralelo a los capacitores electrolíticos respetando la polaridad de los mismos. Finalmente conectaremos el terminal positivo a un interruptor tipo palanca SPST instalado en la parte trasera del cuerpo de nuestro robot como se observa en las siguientes imágenes:

(click para ampliar)

(click para ampliar)

Luego insertaremos el “Arduino Nano V3.0” en su correspondiente “Arduino Nano Shield” y lo instalaremos en el cuerpo central del robot:

(click para ampliar)

IMPORTANTE: lo que haremos a continuación es crucial para el correcto funcionamiento del robot. Conectaremos al “Arduino Nano Shield” los nueve servomotores respetando la numeración y nomenclatura original provista en el kit y el receptor inalámbrico de Playstation 2 de acuerdo a la siguiente imagen:

En los pines de alimentación de 3.3V que utilizaremos para suministrar energía al receptor inalámbrico de Playstation 2, también debemos insertar en paralelo aGNDy3.3Vdos capacitores: un capacitor electrolítico de 220uF 10V y un capacitor cerámico de 100nF 50V como se muestra en la siguiente imagen:

(click para ampliar)

Habiendo finalizado exitosamente el armado de la estrucutra y conectado correctamente los servomotores, el Sensor Ultrasónico y el Receptor Inalamábrico de Playstation 2, sólo resta compilar y cargar en nuestro Arduino Nano el sketch correspondiente, cuyo código fuente se encuentra a continuación:
(No olvidar descargar y agregar las librerías NewPing y Arduino-PS2X)

#include <Servo.h>
#include <NewPing.h>
#include <PS2X_lib.h>

#define TRIG_PIN        A0
#define ECHO_PIN        A1
#define MAX_DISTANCE    400

//-------------------------------------------------------------------
//  Definicion de las constantes posicion inicial de cada servo
//-------------------------------------------------------------------
//subir = giro antihor en cuerpo y patas

#define FIC_centro  1100     //Frente_Izquierda_cuerpo_centro
#define FDC_centro  1700     //Frente_derecha_cuerpo_centro
#define AIC_centro  1800     //Atras_Izquierda_cuerpo_centro
#define ADC_centro  1250     //Atras_derecha_cuerpo_centro

#define FIP_centro  800      //Frente_Izquierda_pata_centro
#define FDP_centro  800      //Frente_derecha_pata_centro
#define AIP_centro  700      //Atras_Izquierda_pata_centro
#define ADP_centro  600      //Atras_derecha_pata_centro

//-------------------------------------------------------------------
//      Definicion Alias de cada servo y los Pines del Arduino
//-------------------------------------------------------------------
#define FIC_pin             4     // Pin Frente Izquierda Cuerpo
#define FDC_pin             7     // Pin Frente Derecha Cuerpo
#define AIC_pin             5     // Pin Atras Izquierda Cuerpo
#define ADC_pin             6     // Pin Atras Derecha Cuerpo
#define FIP_pin             8     // Pin Frente Izquierda Pata
#define FDP_pin             11    // Pin Frente Derecha Pata
#define AIP_pin             9     // Pin Atras Izquierda Pata
#define ADP_pin             10    // Pin Atras Derecha Pata
#define Cabeza_pin          3
#define pulsador_arranque   12    //defino donde va conectado el pulsador de arranque
//-------------------------------------------------------------------

PS2X ps2x; // create PS2 Controller Class

NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);

//-------------------------------------------------------------------
//Con este timer se setea la velocidad de movimiento de todo el robot, mas chico, mas rapido
//-------------------------------------------------------------------
int Time = 100;

int movimiento_izq;
int movimiento_der;
int levanta=-400;

int contador_sentarse;
int distancia_actual = 0;
int pos = 90;

int max_der = 0;
int max_izq = 0;
int aux = 0;    //auxiliar que utilizo para las funciones especiales
int variable_sienta = 800;

const int tiempo_entre_servo = 5;
const int tiempo_sienta = 50;

const int margen_joystick = 10;      //margen que tiene el joystick para saber si esta corrido del centro

int variable_control = 0;   //variable que indica que comando fue pulsado en el PS2

//-------------------------------------------------------------------
//       Defino como le va a llamar cada servo
//-------------------------------------------------------------------
Servo FIC_servo;                                               // Front Left  Hip
Servo FDC_servo;                                               // Front Right Hip
Servo AIC_servo;                                               // Rear  Left  Hip
Servo ADC_servo;                                               // Rear  Right Hip
Servo FIP_servo;                                               // Front Left  Knee
Servo FDP_servo;                                               // Front Right Knee
Servo AIP_servo;                                               // Rear  left  Knee
Servo ADP_servo;                                               // Rear  Right Knee
Servo Cabeza_servo;

//-------------------------------------------------------------------------------------
//       funcion para volver a la posicion inicial despues de cada movimiento
//-------------------------------------------------------------------------------------
void posicion_reposo() {

    int tiempo = 20;
    FIC_servo.writeMicroseconds(FIC_centro);       // envia servo a su posicion inicial
    delay(tiempo);
    FDC_servo.writeMicroseconds(FDC_centro);
    delay(tiempo);
    AIC_servo.writeMicroseconds(AIC_centro);
    delay(tiempo);
    ADC_servo.writeMicroseconds(ADC_centro);
    delay(tiempo);
    FIP_servo.writeMicroseconds(FIP_centro);
    delay(tiempo);
    FDP_servo.writeMicroseconds(FDP_centro);
    delay(tiempo);
    AIP_servo.writeMicroseconds(AIP_centro);
    delay(tiempo);
    ADP_servo.writeMicroseconds(ADP_centro);
    delay(tiempo);
    Cabeza_servo.write(90);
}

//-------------------------------------------------------------------
//             Funcion Avanzar hacia el frente
//-------------------------------------------------------------------
void Camina_frente() {

    if ((movimiento_izq < 0) && (movimiento_der < 0)) {
        Cabeza_servo.write(110);
        decide_costados();
    }

    FDP_servo.writeMicroseconds(FDP_centro-levanta);                // raise front right leg
    delay(tiempo_entre_servo);
    AIP_servo.writeMicroseconds(AIP_centro-levanta);                // raise rear  left  leg
    delay(tiempo_entre_servo);
    FIC_servo.writeMicroseconds(FIC_centro-movimiento_izq);         // move  front left  leg backward
    delay(tiempo_entre_servo);
    ADC_servo.writeMicroseconds(ADC_centro+movimiento_der);         // move  rear  right leg backward
    delay(Time/2);
    FDC_servo.writeMicroseconds(FDC_centro-movimiento_der);         // move  front right leg forward
    delay(tiempo_entre_servo);
    AIC_servo.writeMicroseconds(AIC_centro+movimiento_izq);         // move  rear  left  leg forward
    delay(Time);
    FDP_servo.writeMicroseconds(FDP_centro);                        // lower front right leg
    delay(tiempo_entre_servo);
    AIP_servo.writeMicroseconds(AIP_centro);                        // lower rear  left  leg
    delay(Time);

    if ((movimiento_izq < 0) && (movimiento_der < 0)) {
        Cabeza_servo.write(70);
        decide_costados();
    }

    FIP_servo.writeMicroseconds(FIP_centro-levanta);                // raise front left  leg
    delay(tiempo_entre_servo);
    ADP_servo.writeMicroseconds(ADP_centro-levanta);                // raise rear  right leg
    delay(tiempo_entre_servo);
    FDC_servo.writeMicroseconds(FDC_centro+movimiento_der);         // move  front right leg backward
    delay(tiempo_entre_servo);
    AIC_servo.writeMicroseconds(AIC_centro-movimiento_izq);         // move  rear  left  leg backward
    delay(Time/2);
    FIC_servo.writeMicroseconds(FIC_centro+movimiento_izq);         // move  front left  leg forward
    delay(tiempo_entre_servo);
    ADC_servo.writeMicroseconds(ADC_centro-movimiento_der);         // move  rear  right leg forward
    delay(Time);
    FIP_servo.writeMicroseconds(FIP_centro);                        // lower front left  leg
    delay(tiempo_entre_servo);
    ADP_servo.writeMicroseconds(ADP_centro);                        // lower rear  right leg
    delay(Time);
}

//-------------------------------------------------------------------
//            Funcion que lee el sensor frontal
//-------------------------------------------------------------------
unsigned int readPing() {                           //funcion para leer US y pasarlo a cm
    unsigned int uS = sonar.ping();
    delay(100);

    return (uS/US_ROUNDTRIP_CM);
}

//-------------------------------------------------------------------
//                    Elije para donde avanzar
//-------------------------------------------------------------------
void decide_costados() {

    delay(50);
    distancia_actual = readPing();

    if ((distancia_actual < 30) && (distancia_actual > 10)) {      //chequeo si obstaculo esta entre 10 y 30 cm
        mira_costados();                             //encontrar la salida
    }

    if ((distancia_actual < 10) && (distancia_actual != 0)) {      //chequea obstaculo menor a 10cm
        camina_atras();                               //hace secuancia para atras
    }

}

//-------------------------------------------------------------------
//  Mide para los costados para despues decidir hacia donde avanzar
//-------------------------------------------------------------------
void mira_costados() {                        //hace un barrido y decide para donde ir

    delay(90);
    max_der = 0;
    max_izq = 0;

    for(pos = 144; pos >= 36; pos-= 36) {    // goes from 180 degrees to 0 degrees

        Cabeza_servo.write(pos);                 // tell servo to go to position in variable 'pos'
        delay(90);
        distancia_actual = readPing();                 //lee ultrasonido

        if ((pos > 90) && (distancia_actual > max_izq)) {   // si es mayor al maximo izquierda lo actualiza
            max_izq = distancia_actual;
        }

        if ((pos < 90) && (distancia_actual > max_der)) {  // si es mayor al maximo derecha lo actualiza
            max_der = distancia_actual;
        }
    }

    Cabeza_servo.write(90);                //posiciona servo en el medio

    if (max_der > max_izq) {
        Gira_der();                       //gira derecha
    }

    if (max_izq > max_der) {
        Gira_izq();                      //gira izquierda
    }
}

//-------------------------------------------------------------------
//                 Gira Derecha
//-------------------------------------------------------------------
void Gira_der() {
    movimiento_izq=300;
    movimiento_der=-300;
    Camina_frente();
}

//-------------------------------------------------------------------
//                 Gira Izquierda
//-------------------------------------------------------------------
void Gira_izq() {
    movimiento_izq=-300;
    movimiento_der=300;
    Camina_frente();
}

//-------------------------------------------------------------------
//               Camina hacia atras
//-------------------------------------------------------------------
void camina_atras() {
    movimiento_izq=300;
    movimiento_der=300;
    Camina_frente();
}

//-------------------------------------------------------------------
//                  Funcion Sentarse
//-------------------------------------------------------------------
void sienta() {

    FDC_servo.writeMicroseconds(FDC_centro);
    delay(Time/2);
    FIC_servo.writeMicroseconds(FIC_centro);
    delay(Time/2);

    ADC_servo.writeMicroseconds(ADC_centro+variable_sienta/3);  //se tira de a poco
    delay(Time/2);
    AIC_servo.writeMicroseconds(AIC_centro-variable_sienta/3);
    delay(Time/2);
    ADP_servo.writeMicroseconds(ADP_centro+variable_sienta/3);
    delay(Time/2);
    AIP_servo.writeMicroseconds(AIP_centro+variable_sienta/3);
    delay(Time/2);

    ADC_servo.writeMicroseconds(ADC_centro+variable_sienta/2);  //se tira de a poco
    delay(Time/2);
    AIC_servo.writeMicroseconds(AIC_centro-variable_sienta/2);
    delay(Time/2);
    ADP_servo.writeMicroseconds(ADP_centro+variable_sienta/2);
    delay(Time/2);
    AIP_servo.writeMicroseconds(AIP_centro+variable_sienta/2);
    delay(Time/2);

    ADC_servo.writeMicroseconds(ADC_centro+variable_sienta);   //se termina de tirar
    delay(Time/2);
    AIC_servo.writeMicroseconds(AIC_centro-variable_sienta);
    delay(Time/2);
    ADP_servo.writeMicroseconds(ADP_centro+variable_sienta);
    delay(Time/2);
    AIP_servo.writeMicroseconds(AIP_centro+variable_sienta);
    delay(Time/2);

    delay(10);
}

void mueve_cabeza() {

    Cabeza_servo.write(45);
    delay(3*Time);
    Cabeza_servo.write(135);
    delay(3*Time);
    Cabeza_servo.write(90);
    delay(3*Time);
}

void sentado_levanta() {

    mueve_cabeza();

    FIP_servo.writeMicroseconds(FIP_centro);
    delay(Time/2);
    FDP_servo.writeMicroseconds(FDP_centro);
    delay(Time/2);
    FIC_servo.writeMicroseconds(FIC_centro);
    delay(Time/2);
    FDC_servo.writeMicroseconds(FDC_centro);
    delay(Time/2);
    AIP_servo.writeMicroseconds(AIP_centro);
    delay(Time/2);
    ADP_servo.writeMicroseconds(ADP_centro);
    delay(Time/2);
    AIC_servo.writeMicroseconds(AIC_centro);                      // center servos
    delay(Time/2);
    ADC_servo.writeMicroseconds(ADC_centro);

    delay(10);
}

//-------------------------------------------------------------------
//                  Funcion Saluda
//-------------------------------------------------------------------
void saluda() {

    FDC_servo.writeMicroseconds(FDC_centro);
    delay(Time/2);
    FIC_servo.writeMicroseconds(FIC_centro);
    delay(Time/2);

    ADC_servo.writeMicroseconds(ADC_centro+variable_sienta);  //se tira al piso de costado
    delay(Time/2);
    AIC_servo.writeMicroseconds(AIC_centro-variable_sienta);
    delay(Time/2);

    FDP_servo.writeMicroseconds(2550);
    delay(Time/2);
    ADP_servo.writeMicroseconds(ADP_centro+variable_sienta/3);
    delay(Time/2);
    AIP_servo.writeMicroseconds(AIP_centro+variable_sienta/3);
    delay(Time/2);
    AIP_servo.writeMicroseconds(AIP_centro+variable_sienta/2);
    delay(Time/2);
    AIP_servo.writeMicroseconds(AIP_centro+variable_sienta);
    delay(Time/2);

    delay(10);
}

void saluda_levanta() {

    mueve_cabeza();

    FIP_servo.writeMicroseconds(FIP_centro);
    delay(Time/2);
    FDP_servo.writeMicroseconds(FDP_centro);
    delay(Time/2);
    FIC_servo.writeMicroseconds(FIC_centro);
    delay(Time/2);
    FDC_servo.writeMicroseconds(FDC_centro);
    delay(Time/2);
    AIP_servo.writeMicroseconds(AIP_centro);
    delay(Time/2);
    ADP_servo.writeMicroseconds(ADP_centro);
    delay(Time/2);
    AIC_servo.writeMicroseconds(AIC_centro);                      // center servos
    delay(Time/2);
    ADC_servo.writeMicroseconds(ADC_centro);

    delay(10);
}

//-------------------------------------------------------------------
//                  Funcion Mueve Pata en Frente
//-------------------------------------------------------------------
void mueve_pata_frente() {

    int maximo = 500;

    for(int j = 0; j<maximo; j = j + 100) {
        FDC_servo.writeMicroseconds(FDC_centro+j);  //se tira al piso de costado
        delay(Time/2);
    }

    for(int j = maximo; j>0; j = j - 100) {
        FDC_servo.writeMicroseconds(FDC_centro+j);  //se tira al piso de costado
        delay(Time/2);
    }

    for(int j = 0; j<maximo; j= j + 100) {
        FDC_servo.writeMicroseconds(FDC_centro-j);  //se tira al piso de costado
        delay(Time/2);
    }

    for(int j = maximo; j > 0; j = j - 100) {
        FDC_servo.writeMicroseconds(FDC_centro-j);  //se tira al piso de costado
        delay(Time/2);
    }
}

//-------------------------------------------------------------------
//                  Funcion Alaba y se queda abajo
//-------------------------------------------------------------------
void alaba() {

    ADC_servo.writeMicroseconds(ADC_centro);
    delay(Time/2);
    AIC_servo.writeMicroseconds(AIC_centro);
    delay(Time/2);

    FDC_servo.writeMicroseconds(FDC_centro+variable_sienta);   //se termina de tirar
    delay(Time/2);
    FIC_servo.writeMicroseconds(FIC_centro-variable_sienta);
    delay(Time/2);
    FDP_servo.writeMicroseconds(FDP_centro+variable_sienta);
    delay(Time/2);
    FIP_servo.writeMicroseconds(FIP_centro+variable_sienta);
    delay(Time/2);

    delay(10);
}

void alaba_levanta() {

    FIP_servo.writeMicroseconds(FIP_centro);
    delay(Time/2);
    FDP_servo.writeMicroseconds(FDP_centro);
    delay(Time/2);
    FIC_servo.writeMicroseconds(FIC_centro);
    delay(Time/2);
    FDC_servo.writeMicroseconds(FDC_centro);
    delay(Time/2);
    AIP_servo.writeMicroseconds(AIP_centro);
    delay(Time/2);
    ADP_servo.writeMicroseconds(ADP_centro);
    delay(Time/2);
    AIC_servo.writeMicroseconds(AIC_centro);                      // center servos
    delay(Time/2);
    ADC_servo.writeMicroseconds(ADC_centro);

    delay(10);
}

//-------------------------------------------------------------------
//                  Funcion angelito
//-------------------------------------------------------------------
void angelito_cae() {

    FDC_servo.writeMicroseconds(FDC_centro+variable_sienta);   //se termina de tirar
    delay(Time/2);
    FIC_servo.writeMicroseconds(FIC_centro-variable_sienta);
    delay(Time/2);
    FDP_servo.writeMicroseconds(FDP_centro+variable_sienta);
    delay(Time/2);
    FIP_servo.writeMicroseconds(FIP_centro+variable_sienta);
    delay(Time/2);

    ADC_servo.writeMicroseconds(ADC_centro-variable_sienta);   //se termina de tirar
    delay(Time/2);
    AIC_servo.writeMicroseconds(AIC_centro+variable_sienta);
    delay(Time/2);
    ADP_servo.writeMicroseconds(ADP_centro+variable_sienta);
    delay(Time/2);
    AIP_servo.writeMicroseconds(AIP_centro+variable_sienta);
    delay(Time/2);
}

void angelito_saluda() {

    int maximo = 600;

    for(int j = 0; j < maximo; j = j + 100) {
        FDC_servo.writeMicroseconds(FDC_centro+j);  //se tira al piso de costado
        FIC_servo.writeMicroseconds(FIC_centro-j);  //se tira al piso de costado
        ADC_servo.writeMicroseconds(ADC_centro-j);  //se tira al piso de costado
        AIC_servo.writeMicroseconds(AIC_centro+j);  //se tira al piso de costado
        delay(Time/2);
    }

    for(int j = maximo; j > 0; j = j - 100) {
        FDC_servo.writeMicroseconds(FDC_centro+j);  //se tira al piso de costado
        FIC_servo.writeMicroseconds(FIC_centro-j);  //se tira al piso de costado
        ADC_servo.writeMicroseconds(ADC_centro-j);  //se tira al piso de costado
        AIC_servo.writeMicroseconds(AIC_centro+j);  //se tira al piso de costado
        delay(Time/2);
    }

    for(int j = 0; j < maximo; j = j + 100) {
        FDC_servo.writeMicroseconds(FDC_centro-j);  //se tira al piso de costado
        FIC_servo.writeMicroseconds(FIC_centro+j);  //se tira al piso de costado
        ADC_servo.writeMicroseconds(ADC_centro+j);  //se tira al piso de costado
        AIC_servo.writeMicroseconds(AIC_centro-j);  //se tira al piso de costado
        delay(Time/2);
    }

    for(int j = maximo; j > 0; j = j - 100) {
    FDC_servo.writeMicroseconds(FDC_centro-j);  //se tira al piso de costado
    FIC_servo.writeMicroseconds(FIC_centro+j);  //se tira al piso de costado
    ADC_servo.writeMicroseconds(ADC_centro+j);  //se tira al piso de costado
    AIC_servo.writeMicroseconds(AIC_centro-j);  //se tira al piso de costado
    delay(Time/2);
    }
}
//-------------------------------------------------------------------------------------
void lectura_control() {

    ps2x.read_gamepad(false, 0);      //read controller and set large motor to spin at 'vibrate' speed

    //-------------------------------------lectura de los Botones------------------------------------------------
    variable_control = 0;

    if(ps2x.ButtonPressed(PSB_START)) {
        variable_control = 1;

        return;
    }

    if(ps2x.ButtonPressed(PSB_SELECT)) {
        variable_control = 2;

        return;
    }

    if(ps2x.ButtonPressed(PSB_L3)) {
        variable_control = 3;

        return;
    }

    if(ps2x.ButtonPressed(PSB_R3)) {
        variable_control = 4;

        return;
    }

    if(ps2x.ButtonPressed(PSB_L2)) {
        variable_control = 5;

        return;
    }

    if(ps2x.Button(PSB_R2)) {
        variable_control = 6;

        return;
    }

    if(ps2x.Button(PSB_GREEN)) { //triangulo
        variable_control = 7;

        return;
    }

    if(ps2x.Button(PSB_RED)) {  //circulo
        variable_control = 8;

        return;
    }

    if(ps2x.Button(PSB_BLUE)) {  //x
        variable_control = 9;

        return;
    }

    if(ps2x.Button(PSB_PINK)) {  //cuadrado
        variable_control = 10;

        return;
    }

    if(ps2x.Button(PSB_PAD_UP)) {  //boton arriba
        variable_control = 11;

        return;
    }

    if(ps2x.Button(PSB_PAD_DOWN)) {   //boton abajo
        variable_control = 12;

        return;
    }

    if(ps2x.Button(PSB_PAD_LEFT)) {  //boton izquierda
        variable_control = 13;

        return;
    }

    if(ps2x.Button(PSB_PAD_RIGHT)) {  //boton derecha
        variable_control = 14;

        return;
    }

    if(ps2x.ButtonPressed(PSB_L1)) {
        variable_control = 23;

        return;
    }

    if(ps2x.ButtonPressed(PSB_R1)) {
        variable_control = 24;
        return;
    }
}

//-------------------------------------------------------------------
//                 Seteo de Entradas, salidas y funciones
//-------------------------------------------------------------------
void setup() {
    //-------------------------------------------------------------------
    // Inicializo Los servos con los nombres y cual es su pin asociado
    //-------------------------------------------------------------------
    FIC_servo.attach(FIC_pin);           //define los servos con su pin
    FDC_servo.attach(FDC_pin);
    AIC_servo.attach(AIC_pin);
    ADC_servo.attach(ADC_pin);
    FIP_servo.attach(FIP_pin);
    FDP_servo.attach(FDP_pin);
    AIP_servo.attach(AIP_pin);
    ADP_servo.attach(ADP_pin);
    Cabeza_servo.attach(Cabeza_pin);

    ps2x.config_gamepad(A2, A4, A3, A5, true, true);   //clock, command, attention, data

    //-------------------------------------------------------------------
    //         Arranco Todos los Servos en sus posiciones iniciales
    //-------------------------------------------------------------------
    FIC_servo.writeMicroseconds(FIC_centro);       // envia servo a su posicion inicial
    delay(tiempo_entre_servo);
    FDC_servo.writeMicroseconds(FDC_centro);
    delay(tiempo_entre_servo);
    AIC_servo.writeMicroseconds(AIC_centro);
    delay(tiempo_entre_servo);
    ADC_servo.writeMicroseconds(ADC_centro);
    delay(tiempo_entre_servo);
    FIP_servo.writeMicroseconds(FIP_centro);
    delay(tiempo_entre_servo);
    FDP_servo.writeMicroseconds(FDP_centro);
    delay(tiempo_entre_servo);
    AIP_servo.writeMicroseconds(AIP_centro);
    delay(tiempo_entre_servo);
    ADP_servo.writeMicroseconds(ADP_centro);
    delay(tiempo_entre_servo);
    Cabeza_servo.write(90);

    delay(500);
}

//-------------------------------------------------------------------
//         Sector donde comienza la funcion Principal,
//        Donde se va a quedar el resto de la ejecucion
//-------------------------------------------------------------------
void loop() {

    lectura_control();  //lectura del control PS2

    if(variable_control == 0) { //si no llegan comandos de la PS2 hago nada

        posicion_reposo();
    }

//-------------comandos de habilidades especiales----------------------------

//--------------------------se pulsa triangulo  ---------------------------------

    while(variable_control == 7) {  //fue pulsado triangulo

        if(aux == 0)
            sienta();

        aux = 1;
        delay(10);
        lectura_control();

        while(variable_control == 7) {  //fue pulsado triangulo
            delay(10);
            lectura_control();
       }
    }

     if(aux == 1) {

        sentado_levanta();
        aux = 0;
        delay(300);
    }

//--------------------------se pulsa circulo  ---------------------------------

    while(variable_control == 8) { //fue pulsado circulo

        if(aux == 0)
            saluda();

        aux = 1;
        delay(10);
        lectura_control();

        while(variable_control == 8) { //fue pulsado circulo
            delay(10);
            mueve_pata_frente();
            lectura_control();
       }
    }

    if(aux == 1) {
        saluda_levanta();
        aux=0;
        delay(300);
    }

//--------------------------se pulsa X   ---------------------------------

    while(variable_control == 9) { //fue pulsado X

        if(aux == 0)
            alaba();

        aux=1;
        delay(10);
        lectura_control();

        while(variable_control == 9) { //fue pulsado x
            delay(10);
            lectura_control();
        }
    }

    if(aux == 1) {
        alaba_levanta();
        aux = 0;
        delay(300);
    }

//--------------------------se pulsa Cuadrado   ---------------------------------

    while(variable_control==10) { //fue pulsado cuadrado

        if(aux == 0)
        angelito_cae();
        aux = 1;
        delay(10);
        lectura_control();

        while(variable_control==10) { //fue pulsado cuadrado
            delay(10);
            angelito_saluda();
            lectura_control();
        }
    }

    if(aux == 1) {
        aux = 0;
        delay(300);
    }

//-------------------comandos de movimientos normales--------------------
    if(variable_control == 11) { //fue pulsado arriba
        movimiento_izq=-300;
        movimiento_der=-300;
        Camina_frente();
    }
    if(variable_control == 12) { //fue pulsado abajo
        camina_atras();
    }
    if(variable_control == 13) { //fue pulsado izquierda
        Gira_der();
    }
    if(variable_control == 14) { //fue pulsado derecha
        Gira_izq();
    }

//----------------comando aumento y disminucion de velocidad--------------------------

    if(variable_control == 23) { //si no llegan comandos de la PS2 hago nada
        Time = Time + 20;
        if(Time > 240)
            Time = 240;
    }

    if(variable_control == 24) { //si no llegan comandos de la PS2 hago nada
        Time = Time - 20;
        if(Time < 10)
            Time = 10;
    }
}

(click para ampliar)

(click para ampliar)

(click para ampliar)