Programar los movimientos del 4-Bot

Objetivos

Realizar las conexiones.

Entender cómo funciona el movimiento del 4-Bot.

Aprender a manejar el shield de servos.

Programar los movimientos del 4-Bot.

Material requerido

Añadir al carrito

Robot 4-bot

85.00€ + IVA

Conexión del 4-Bot

Una vez tenemos el 4-Bot montado podemos empezar a conectar todos los componentes, principalmente los servos al shield. Es importante que hayáis seguido las instrucciones de la sesión anterior para que tengamos montado el robot de la misma forma, de la misma manera que tenemos que conectar los servos al shield en el mismo orden para que el programa que vamos a hacer funcione correctamente.

Si lo hacéis de otra forma tendréis que modificar el programa, aunque si entendéis cómo funcionan los movimientos del robot seguro que podéis modificarlo para que cuadre con vuestro montaje.

Y dicho esto, vamos a empezar. La primera recomendación, para que luego sea más sencillo saber en todo momento el orden de las patas del robot, es que marquéis cada pata, por ejemplo, con un rotulador permanente. Para ello vamos a ponernos el robot delante de manera que veamos el puerto serie del Arduino. Teniéndolo en esa posición, la pata que nos quede a la izquierda va a ser la pata 1, y a partir de ahí las vamos a enumerar en el sentido de las agujas del reloj.

Cuando tengamos eso claro podemos conectar los servos al shield. El shield tiene conexiones hasta para 16 servos, divididos en grupos 4 de 4, que van desde el 0 hasta el 15. Pues vamos a usar uno de esos grupos para cada pata, de forma que en cualquier momento podamos tener claro donde está conectado cada servo.

Conectaremos siempre primero el servo del hombro, y después el de la pata. Es decir, si por ejemplo vamos a conectar la primera extremidad, conectaremos el servo del hombro a la posición 0 y el de la pata al 1.

A la hora de conectarlos al terminal, tenéis marcado qué pin es cada cual en el espacio que hay entre el 2º y el 3º grupo de conexiones para servos del shield. En los servos el cable naranja es PWM, el rojo 5V y el marrón GND.

Además de los servos tenemos que conectar la batería. Ésta tiene dos conexiones, una para cargar, la que tiene 3 cables, y otra por la que sacar la alimentación para el Arduino con 2 cables.

En el esquema podréis ver cómo conectarla, pero si no habéis trabajado antes con baterías LiPo os recomendamos que antes leáis estas advertencias si no queréis estropear nada:

Para cargarla no hay problema, los conectores sólo enganchan en una posición, así que no tiene perdida. De todas formas el color de los cables de la batería y el cargador tiene que coincidir.

El cable rojo de la batería son 5V y el negro GND. Si conectáis estos cables al revés al servo o al Arduino lo más probable es que los queméis.

Cuando desconectéis la batería hacerlo siempre desde el conector de la batería, para evitar que podáis cortocircuitarla. Es decir, nunca dejéis los cables saliendo de la batería y al aire, porque si se tocasen las puntas harían cortocircuito.

Fijaros que además de alimentar el shield tenemos que conectar un cable también desde la toma positiva de la batería al pin de 5V del shield para alimentar también el Arduino desde la batería.

Programación

Ahora sí, ya podemos empezar a programar el robot. Lo primero que vamos a hacer es descargare incluir en el IDE la librería Adafruit-PWM-Servo-Driver-Library-master para poder usar el shield. Una vez hecho vamos a declarar las librerías y a crear la instancia para el shield. Además vamos a definir los límites de movimiento para los servos.

Con la librería no usamos la posición en grados sino un número entre 0 y 4096 para posicionar los servos.

En este caso vamos a poner los límites en 150 y 550 para asegurarnos de no dañar los servos.

La posición de 90º equivale al valor 350, pero ya lo veremos más adelante.

También vamos a crear las variables necesarias para controlar los servos, dándoles el valor del puerto del shield al que está conectado.

#include 
#include  // Librería shield servos

// De esta manera le llamamos a la direccion por defecto 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
// Se puede llamar a la direccion que se quiera de esta forma
// Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41);
	
// Dependiendo del tipo de servo el pulso máximo y mínimo es diferente
// Lo preferible es que sea el máximo posible sin llegar a hacer tope

#define SERVOMIN  150 // Minimo (sobre 4096)
#define SERVOMAX  550 // Maximo (sobre 4096)

const unsigned short h1 = 0; //Hombro extremidad 1
const unsigned short p1 = 1; //Pata extremidad 1
const unsigned short h2 = 4; //Hombro extremidad 2
const unsigned short p2 = 5; //Pata extremidad 2
const unsigned short h3 = 8; //Hombro extremidad 3
const unsigned short p3 = 9; //Pata extremidad 3
const unsigned short h4 = 12; //Hombro extremidad 4
const unsigned short p4 = 13; //Pata extremidad 4

En el setup vamos a inicializar la instancia y a establecer la frecuencia a la que trabajan los servos, que en este caso son 60Hz.

void setup()
{
  pwm.begin();
  pwm.setPWMFreq(60);  // Los servos funcionan a ~60 Hz
}

Antes de empezar a programar los movimientos, hay dos cosas que tenemos que tener claras a la hora de hacerlo, cómo queremos que se mueva el robot y cómo se mueven los servos en función de su posición. Lo mejor es tener un pequeño esquema que podamos consultar para poder consultar cuando tengamos dudas de cómo se mueve cada servo.

Teniendo en cuenta que el valor 350 coincide con 90º, y que hemos colocado los servos de forma que a 90º la araña quedaría tumbada, hemos hecho este esquema del movimiento de cada extremidad. En él podemos ver si al aumentar o disminuir el valor del servo, este se mueve en una dirección u otra.

Ahora sí que estamos en posición de empezar a programar los movimientos de 4-Bot. El primero, por ser el más sencillo, va a ser el de tumbarse. Para ello no tenemos más que poner todos los servos a 90º, es decir con valor 350.

Si queréis ir probándolos uno a uno (os lo recomendamos), cada vez que programemos uno meted la función en el setup y probad si funciona.

// Funcion para tumbar el robot
void tumbar()
{
  pwm.setPWM(h1, 0, 350);
  pwm.setPWM(h2, 0, 350);
  pwm.setPWM(h3, 0, 350);
  pwm.setPWM(h4, 0, 350);

  pwm.setPWM(p1, 0, 350);
  pwm.setPWM(p2, 0, 350);
  pwm.setPWM(p3, 0, 350);
  pwm.setPWM(p4, 0, 350);
}

El siguiente movimiento que vamos a programar es el de levantarse. Para ello los servos de los hombros los dejaremos igual, pero bajaremos los servos de las patas.

// Funcion para poner de pie el robot
void depie()
{
  pwm.setPWM(h1, 0, 350);
  pwm.setPWM(h2, 0, 350);
  pwm.setPWM(h3, 0, 350);
  pwm.setPWM(h4, 0, 350);

  pwm.setPWM(p1, 0, 500);
  pwm.setPWM(p2, 0, 200);
  pwm.setPWM(p3, 0, 500);
  pwm.setPWM(p4, 0, 200);
}

A continuación vamos a crear los movimientos para inclinar a 4-Bot en las 4 direcciones. Vamos a empezar por hacer que se siente. Para conseguirlo vamos a dejar las extremidades delanteras como si estuviera en pie y vamos a juntar y estirar las de atrás.

// Funcion para sentar el robot
void sentar()
{
  pwm.setPWM(h1, 0, 300);
  pwm.setPWM(h2, 0, 400);
  pwm.setPWM(h3, 0, 350);
  pwm.setPWM(h4, 0, 350);

  pwm.setPWM(p1, 0, 350);
  pwm.setPWM(p2, 0, 350);
  pwm.setPWM(p3, 0, 500);
  pwm.setPWM(p4, 0, 200);
}

// Funcion para inclinar hacia delante el robot
void inclinar()
{
  pwm.setPWM(h1, 0, 350);
  pwm.setPWM(h2, 0, 350);
  pwm.setPWM(h3, 0, 300);
  pwm.setPWM(h4, 0, 400);

  pwm.setPWM(p1, 0, 500);
  pwm.setPWM(p2, 0, 200);
  pwm.setPWM(p3, 0, 350);
  pwm.setPWM(p4, 0, 350);
}

// Funcion para inclinar hacia la derecha el robot
void inclinarder()
{
  pwm.setPWM(h1, 0, 400);
  pwm.setPWM(h2, 0, 350);
  pwm.setPWM(h3, 0, 350);
  pwm.setPWM(h4, 0, 300);

  pwm.setPWM(p1, 0, 350);
  pwm.setPWM(p2, 0, 200);
  pwm.setPWM(p3, 0, 500);
  pwm.setPWM(p4, 0, 350);
}

// Funcion para inclinar hacia la izquierda el robot
void inclinarizq()
{
  pwm.setPWM(h1, 0, 350);
  pwm.setPWM(h2, 0, 300);
  pwm.setPWM(h3, 0, 400);
  pwm.setPWM(h4, 0, 350);

  pwm.setPWM(p1, 0, 500);
  pwm.setPWM(p2, 0, 350);
  pwm.setPWM(p3, 0, 350);
  pwm.setPWM(p4, 0, 200);
}

El siguiente movimiento va a ser un poco más curioso, y es que vamos a hacer que el robot nos salude. Para ello vamos a hacer que levante la extremidad 3 y la mueva de lado a lado. Pero antes vamos modificar la posición del robot para asegurarnos de que no pierda el equilibrio cuando la suba. Tomando como referencia la posición en pie, la idea es esta:

Subimos la pata de la extremidad 1 y bajamos la de la 3, de forma que se incline hacia atrás.
Movemos los hombros de las extremidades dos y 4 hacia delante para asegurarnos que se apoye bien.
Levantamos la pata de la extremidad 3.
Creamos un bucle en el que movemos el hombro de la extremidad 3 de lado a lado.
Ponemos a 4-Bot en pie.

// Funcion para que salude el robot
void hola()
{
  //posicionamos el robot
  pwm.setPWM(h1, 0, 350);
  pwm.setPWM(h2, 0, 250);
  pwm.setPWM(h3, 0, 350);
  pwm.setPWM(h4, 0, 450);

  pwm.setPWM(p1, 0, 400);
  pwm.setPWM(p2, 0, 200);
  pwm.setPWM(p3, 0, 550);
  pwm.setPWM(p4, 0, 200);
  delay (500);

  pwm.setPWM(p3, 0, 250);
  delay (500);

  for (int i = 0; i < 3; i++)
  {
    pwm.setPWM(h3, 0, 250);
    delay (500);
    pwm.setPWM(h3, 0, 450);
    delay (500);
  }
  depie();
}

Ya tenemos los movimientos que va a hacer 4-bot sin desplazarse, pero por supuesto que queremos que ande. Así que vamos a hacer que ande hacia delante, y cuanto lo tengamos sólo tendremos que modificar un poco la función para que ande en las demás direcciones.

De la misma forma que el anterior lo que queremos que haga es:

Levantar las patas de las extremidades 1 y 3.
Repetir en bucle en función de los pasos que queremos que dé:
1. Mover los hombros de las extremidades 1 y 3 hacia delante.
2. Bajar las patas de las extremidades 1 y 3 a la posición de en pie.
3. Subir las patas de las extremidades 2 y 4.
4. Mover los hombros de las extremidades 1 y 3 hacia atrás, a la posición de en pie.
5. Mover los hombros de las extremidades 2 y 4 hacia delante.
6. Bajar las patas de las extremidades 2 y 4.
7. Subir las patas de las extremidades 1 y 3.
8. Mover los hombros de las extremidades 2 y 4 hacia atrás, a la posición de en pie.
Terminamos poniendo el robot en pie.

// Funcion para que ande el robot
void andar()
{

  pwm.setPWM(p1, 0, 450);
  pwm.setPWM(p3, 0, 450);
  delay (100);

  for (int i = 0; i < 10; i++)
  {
    pwm.setPWM(h1, 0, 400);
    pwm.setPWM(h3, 0, 300);
    delay (100);

    pwm.setPWM(p1, 0, 500);
    pwm.setPWM(p3, 0, 500);
    delay (100);

    pwm.setPWM(p2, 0, 250);
    pwm.setPWM(p4, 0, 250);
    delay(100);

    pwm.setPWM(h1, 0, 350);
    pwm.setPWM(h3, 0, 350);
    delay(100);

    pwm.setPWM(h2, 0, 300);
    pwm.setPWM(h4, 0, 400);
    delay (100);

    pwm.setPWM(p2, 0, 200);
    pwm.setPWM(p4, 0, 200);
    delay(100);

    pwm.setPWM(p1, 0, 450);
    pwm.setPWM(p3, 0, 450);
    delay (100);

    pwm.setPWM(h2, 0, 350);
    pwm.setPWM(h4, 0, 350);
    delay(100);
  }
  depie();
}

Para que ande hacia atrás el algoritmo es muy parecido, pero movemos los hombros en la otra dirección:

// Funcion para que ande el robot hacia atras
void atras()
{

  pwm.setPWM(p1, 0, 450);
  pwm.setPWM(p3, 0, 450);
  delay (100);

  for (int i = 0; i < 10; i++)
  {
    pwm.setPWM(h1, 0, 300);
    pwm.setPWM(h3, 0, 400);
    delay (100);

    pwm.setPWM(p1, 0, 500);
    pwm.setPWM(p3, 0, 500);
    delay (100);

    pwm.setPWM(p2, 0, 250);
    pwm.setPWM(p4, 0, 250);
    delay(100);

    pwm.setPWM(h1, 0, 350);
    pwm.setPWM(h3, 0, 350);
    delay(100);

    pwm.setPWM(h2, 0, 400);
    pwm.setPWM(h4, 0, 300);
    delay (100);

    pwm.setPWM(p2, 0, 200);
    pwm.setPWM(p4, 0, 200);
    delay(100);

    pwm.setPWM(p1, 0, 450);
    pwm.setPWM(p3, 0, 450);
    delay (100);

    pwm.setPWM(h2, 0, 350);
    pwm.setPWM(h4, 0, 350);
    delay(100);
  }
  depie();
}

Nos queda hacer que gire hacia ambos lados. Para que gire hacia la derecha tenemos que mover los hombros de las extremidades 2 y 3 hacia delante, como en la función andar(), y las de las 1 y 4 hacia atrás, como en atras(). Para que gire hacia la izquierda haremos lo contrario.

// Funcion para que gire el robot a la derecha
void girarderecha()
{

  pwm.setPWM(p1, 0, 450);
  pwm.setPWM(p3, 0, 450);
  delay (100);

  for (int i = 0; i < 10; i++)
  {
    pwm.setPWM(h1, 0, 300);
    pwm.setPWM(h3, 0, 300);
    delay (100);

    pwm.setPWM(p1, 0, 500);
    pwm.setPWM(p3, 0, 500);
    delay (100);

    pwm.setPWM(p2, 0, 250);
    pwm.setPWM(p4, 0, 250);
    delay(100);

    pwm.setPWM(h1, 0, 350);
    pwm.setPWM(h3, 0, 350);
    delay(100);

    pwm.setPWM(h2, 0, 300);
    pwm.setPWM(h4, 0, 300);
    delay (100);

    pwm.setPWM(p2, 0, 200);
    pwm.setPWM(p4, 0, 200);
    delay(100);

    pwm.setPWM(p1, 0, 450);
    pwm.setPWM(p3, 0, 450);
    delay (100);

    pwm.setPWM(h2, 0, 350);
    pwm.setPWM(h4, 0, 350);
    delay(100);
  }
  depie();
}

// Funcion para que gire el robot hacia la izquierda
void girarizquierda()
{

  pwm.setPWM(p1, 0, 450);
  pwm.setPWM(p3, 0, 450);
  delay (100);

  for (int i = 0; i < 10; i++)
  {
    pwm.setPWM(h1, 0, 400);
    pwm.setPWM(h3, 0, 400);
    delay (100);

    pwm.setPWM(p1, 0, 500);
    pwm.setPWM(p3, 0, 500);
    delay (100);

    pwm.setPWM(p2, 0, 250);
    pwm.setPWM(p4, 0, 250);
    delay(100);

    pwm.setPWM(h1, 0, 350);
    pwm.setPWM(h3, 0, 350);
    delay(100);

    pwm.setPWM(h2, 0, 400);
    pwm.setPWM(h4, 0, 400);
    delay (100);

    pwm.setPWM(p2, 0, 200);
    pwm.setPWM(p4, 0, 200);
    delay(100);

    pwm.setPWM(p1, 0, 450);
    pwm.setPWM(p3, 0, 450);
    delay (100);

    pwm.setPWM(h2, 0, 350);
    pwm.setPWM(h4, 0, 350);
    delay(100);
  }
  depie();
}

Y con esto ya tenemos todas las funciones que vamos a necesitar para los movimientos. Aunque, por supuesto, podéis crear nuevos movimientos o mejorar los que hemos hecho. Ahora vamos a ponerlos todos en el loop para que los repita de forma indefinida y comprobaremos una vez más que todo funciona correctamente.

void loop()
{
  depie();
  delay(3000);
  tumbar();
  delay(3000);
  depie();
  delay(3000);
  sentar();
  delay(3000);
  inclinar();
  delay(3000);
  inclinarder();
  delay(3000);
  inclinarizq();
  delay(3000);
  hola();
  delay(3000);
  andar();
  delay(3000);
  atras();
  delay(3000);
  girarderecha();
  delay(3000);
  girarizquierda();
  delay(3000);
}

Podéis descargar el programa completo aquí: _4-Bot_Movimientos. Aquí tenéis un vídeo con el resultado:

Con los movimientos programados y probados, se nos abren muchas posibilidades para seguir trabajando con el 4-Bot. Podemos controlarlo con un Nunchuk de la Wii, con un teléfono móvil mediante bluetooth, crear un mando con otro Arduino y el shield con Joystick y botones… o podemos dotarlo de cierta autonomía utilizando sensores de distancia o convirtiéndolo en un sigue líneas.

Resumen de la sesión

En esta sesión hemos aprendido varias cosas importantes:

Cómo se conecta y se programa el shield para servos.

Hemos pensado como debe mover 4-Bot las extremidades para que haga los diferentes movimientos.

Hemos transformado esas ideas en programación.