El robot Beetle es un diseño interesante para aplicar varios tipos de control. Así que vamos a proceder paso por paso a cómo desarrollar cada modo de juego por separado para juntarlo todo en un programa completo.
Vamos a desarrollar un guión de programas y después finalizaremos para integrarlo todo para controlarlo a través de un módulo Bluetooth HC-05 que atienda a cada modo.
Para iniciarnos, es recomendable leer el siguiente post, para poder establecer las primeras comunicaciones con nuestro módulo Bluetooth; ya que muchas veces, el problema es que no hemos configurado correctamente nuestro módulo.
Para este tutorial, vamos a utilizar la aplicación de móvil Robopad++ que dispone de diversos modelos de la rama DIWO disponibles para comunicarnos por Bluetooth e intercambiar información. Si ya hemos hecho las primeras comunicaciones con nuestro robot, podemos proceder para crear nuestros programas de control.
Antes de comenzar a programar vamos a definir los conceptos de control manual, o control autónomo para un robot. Un control de robot autónomo es aquel en el que el robot no necesita de ayuda humana para cumplir con un objetivo. Mientras que el modo manual, se refiere a aquel en el que el robot se mueve completamente a nuestra merced hasta cumplir un objetivo guiado.
Desde la imagen, podemos ver que los botones de movimiento y de pinza son de control manual, mientras que los botones de la esquina inferior izquierda son de control autónomo para un modo siguelineas y para un modo Huyeluz.
Esto es importante para que el robot sepa en qué estado se encuentra y realizar una acción u otra.
- Modo manual
- Control de movimiento
- Control de pinza
- Modo autónomo
- Control Siguelineas
- Control Huyeluz
Vamos a ir paso a paso para ir elaborando cada una de estas estructuras de programación con ArduBlockly que podemos acceder a través del siguiente enlace.
Conexionado
- Sensor LDR Izquierdo –> A0
- Sensor LDR Derecho –> A1
- Bluetooth RX –> 2
- Bluetooth TX –> 3
- Sensor IR Izquierdo –> 4
- Sensor IR Derecho –> 5
- MiniServo Pinza –> 6
- Servomotor Izquierdo –> 9
- Servomotor Derecho –> 10
Comunicación SoftwareSerial
Para la comunicación por Bluetooth, utilizaremos la librería SoftwareSerial en los pines 2 y 3, al que conectaremos un módulo HC-05 para desarrollar esta comunicación.
Habrá que configurar convenientemente la tasa de baudios por segundo entre dispositivos. Por defecto, realizaremos la comunicación a 38400 bps , pero en caso de hacer la comunicación con otros valores, deberemos especificarlo.
Para establecer la velocidad a 38400, es recomendable dirigirse al siguiente enlace.
Movimientos básicos
Para programar los movimientos básicos, vamos a crear unas funciones de dirección. Como ya hablamos en un post anterior, podemos definir dos modos de comunicar nuestra aplicación con nuestro robot.
Nosotros usaremos el modo a tiempo real.
#include <Servo.h>
#include <SoftwareSerial.h>
char BTData;
Servo LeftServo;
Servo RightServo;
SoftwareSerial comm(2,3);
void forward() {
LeftServo.write(0);
RightServo.write(180);
}
void turn_left() {
LeftServo.write(180);
RightServo.write(180);
}
void stop() {
LeftServo.write(90);
RightServo.write(90);
}
void backwards() {
LeftServo.write(180);
RightServo.write(0);
}
void turn_right() {
LeftServo.write(0);
RightServo.write(0);
}
void setup() {
Serial.begin(38400);
comm.begin(38400);
LeftServo.attach(9);
RightServo.attach(10);
}
void loop() {
if (comm.available()) {
BTData = (char)((comm.read()));
if (BTData == ('U')) {
forward();
} else if (BTData == ('D')) {
backwards();
} else if (BTData == ('L')) {
turn_left();
} else if (BTData == ('R')) {
turn_right();
} else if (BTData == ('S')) {
stop();
}
}
}
Para mejorar los modos de movimiento, yo recomiendo hacer un detach o un attach de sus pines en el momento que se paran o se ponen en movimiento. A veces puede ocurrir que los motores tengan algún error de montaje y aún expresando el valor 90 como en parado, se muevan muy poco, pero perceptible.
Pinza y lectura de números
Antes de controlar la pinza, vamos a revisar un poco el montaje, ya que la calibración de este paso en la pinza, va a permitirnos que este robot funcione adecuadamente.
Antes de nada , hay que saber que la aplicación soporta una apertura de la pinza de 10 a 55 grados. Por lo que deberemos asegurarnos y nivelar que el valor de 10 grados será para la pinza cerrada y 55 para la pinza abierta. En este tutorial podremos seguir los pasos de montaje de la pinza, pero más adelante untilizaremos un pequeño truco.
El programa a realizar para leer números para la pinza será el siguiente. Primero tenemos que leer la primera letra. Si esta letra es la C, entoncer es que queremos mover la pinza y el número que hay a continuación serán los grados que queremos mover.
Es por ello, que los bloques utilizados, se diferencian a la hora de chequear si queremos leer la linea completa con readString o solo un caracter con read.
Ahora que podemos leer el valor de la pinza, solamente lo tendremos que aplicar sobre el miniservo que debe cerrar o abrir hasta ese valor.
Pero es bastante posible que el número que comunique nuestra aplicación móvil con el montaje de nuestro robot no sea tan preciso en el cierre y apertura de la pinza. Para no tener que estar montando y desmontando hasta lograr el punto preciso para hacer coincidir el cierre o apertura de la pinza con los valores 10º o 55º vamos a mapear.
Para ello, vamos a hacer pruebas con el giro del miniservo hasta establecer el número para el cual la pinza estará abierta. Este valor lo guardaremos en una variable que llamaremos ClawMin. Ahora haremos lo mismo para la pinza cerrada y lo guardaremos en una variable llamada ClawMax.
A mí me ha salido 40º y 115º respectivamente, pero en otros montajes, estos valores pueden salir diferentes.
Con estos valores, vamos a utilizar la función mapear; que lo que hace es convertir las escalas de 10º a 55º; que son los valores que obtenemos de la aplicación RoboPad++; a los que nosotros hemos obtenido para hacer una equivalencia. De esta manera nos ahorramos tiempo con una solución inteligente.
#include <SoftwareSerial.h>
#include <Servo.h>
int ClawMin;
int ClawMax;
char BTData;
int ClawValue;
SoftwareSerial comm(2,3);
Servo ClawServo;
void setup() {
Serial.begin(38400);
comm.begin(38400);
ClawMin = (int)(40);
ClawMax = (int)(110);
ClawServo.attach(6);
}
void loop() {
if (comm.available()) {
BTData = (char)((comm.read()));
if (BTData == ('C')) {
ClawValue = (int)(((comm.readString()).toInt()));
ClawServo.write((map(ClawValue,10, 55, ClawMin , ClawMax)));
}
}
}
Lectura de sensores infrarrojos
Antes de definir el modelo de estados para diferencia el control automático o el control manual; vamos a crear el ejemplo sencillo para hacer que el movimiento de nuestro robot solamente funcione con el modo siguelineas sin comunicaciones.
De la misma manera que antes, para evitar movimientos de error podemos utilizar las funciones de attach y detach tal y como se ve en la siguiente imagen.
#include <Servo.h>
Servo LeftServo;
Servo RightServo;
void setup() {
pinMode(4, INPUT);
pinMode(5, INPUT);
LeftServo.attach(9);
RightServo.attach(10);
}
void loop() {
if (digitalRead(4)) {
RightServo.write(90);
RightServo.detach();
} else {
RightServo.attach(10);
RightServo.write(180);
}
if (digitalRead(5)) {
LeftServo.write(90);
LeftServo.detach();
} else {
LeftServo.attach(9);
LeftServo.write(0);
}
}
Lectura de sensores de luz
Para este modo de funcionamiento, vamos a establecer algunas pautas adicionales. Y es que ahora vamos a controlar el movimiento en función del valor leido por los sensores de luz que disponemos.
Lo malo de estos sensores es que miden valores entre 0 y 1023, no todos son iguales y la intensidad de luz puede no ser interpretada de la misma forma en una zona abierta que en una cerrada.
En una primera prueba, deberíamos leer el valor que obtenemos de cada uno y calibrarlos. El problema reside en que dependeríamos de una referencia absoluta que es la luz del entorno. Pero tendríamos que volver a calibrar para los valores obtenidos en una habitación con menos luz.
Este modelo de programación funcionaría dando vueltas en función de dónde hay más luz, pero de esta manera podremos leer desde el Serial Plotter de Arduino cómo evolucionan las señales en el tiempo.
Para no depender tanto de factores externos, vamos a desarrollar una referencia relativa. Puesto que tenemos dos sensores, vamos a establecer la diferencia entre los dos valores leidos y si son mayor que cero, girarán a un lado y si no al lado contrario.
Esta diferencia hará que el valor sea positivo cuando el valor del sensor de la iquierda sea mayor que el de la derecha. Y si es un valor negativo ocurrirá lo contrario. A medida que gire se encontrará con un nuevo estado en el que cambiarán los valores.
El problema que tendremos aquí es que en ningún momento se dirigirá hacia delante, y además inevitablemente siempre existirá una diferencia mayor o menor que cero; por lo que nunca dejará de girar como un loco.
Por ello, hemos de definir un valor umbral en el que el robot se quedará parado, girará o ira hacia delante.
Para ello, definimos tres variables. En una variable almacenaremos el valor de la diferencia entre los dos valores análogicos leidos. Si la diferencia entre ambas es menor de 100, entonces realizaremos el giro hacia un lado u otro en función de cuál sea predominante.
Establecemos esta función como la prioritaria y definimos qué es lo que ocurrirá si la diferencia entre los dos sensores es mayor.
La siguiente prioridad es reconocer si hay que ir hacia delante o hacia atrás. Por ello, vamos a hacer que se cumplan dos hechos. Si los dos sensores miden un valor mayor a la variable umbral, iremos hacia delante, en caso contrario, pararemos nuestro robot.
#include <Servo.h>
#include <SoftwareSerial.h>
char BTData;
int LDR_diff;
int threshold;
int diff_threshold;
Servo LeftServo;
Servo RightServo;
SoftwareSerial comm(2,3);
void Servo_attach() {
LeftServo.attach(9);
RightServo.attach(10);
}
void Servo_detach() {
LeftServo.detach();
RightServo.detach();
}
void forward() {
Servo_attach();
LeftServo.write(0);
RightServo.write(180);
}
void turn_left() {
Servo_attach();
LeftServo.write(180);
RightServo.write(180);
}
void stop() {
LeftServo.write(90);
RightServo.write(90);
Servo_detach();
}
void backwards() {
Servo_attach();
LeftServo.write(180);
RightServo.write(0);
}
void turn_right() {
Servo_attach();
LeftServo.write(0);
RightServo.write(0);
}
void setup() {
Serial.begin(38400);
comm.begin(38400);
pinMode(A0, INPUT);
pinMode(A1, INPUT);
LeftServo.attach(9);
RightServo.attach(10);
LDR_diff = (int)(0);
threshold = (int)(300);
diff_threshold = (int)(100);
}
void loop() {
LDR_diff = analogRead(A0) - analogRead(A1);
Serial.print(analogRead(A0));
Serial.print(",");
Serial.println(analogRead(A1));
if (abs(LDR_diff) > diff_threshold) {
if (LDR_diff > diff_threshold) {
Serial.println("LEFT");
turn_left();
} else if (LDR_diff < diff_threshold * -1) { Serial.println("RIGHT"); turn_right(); } } else if (analogRead(A0) > threshold && analogRead(A1) > threshold) {
forward();
} else if (analogRead(A0) < threshold && analogRead(A1) < threshold) {
stop();
}
}
Aunque el programa es bastante funcional, en otro tutorial aprenderemos a como desarrollar funciones con parámetros e introducir velocidades dependientes de las medidas para los servomotores, de manera que cuando obtengamos más luz, el robot, huirá más rápido y cuando se acerque a zonas más oscuras, su velocidad disminuya hasta pararse.
Ahora finalmente vamos a integrar todo en uno.
Vamos a comunicar nuestro robot por Bluetooth con todas las opciones de funcionamiento.
Modo automático, modo manual, con siguelineas, con pinza y con detector de luz. Todo en uno.
Para ello, hemos de comprender, qué datos o caracteres lee nuestro robot de la aplicación.
- U (UP) –> Mover adelante
- D (DOWN) –> Mover atrás
- L (LEFT) –> Girar a la izquierda
- R (RIGHT) –> Girar a la derecha
- S (STOP) –> Parar
- I –> Modo Automático Siguelineas
- G –> Modo Automático Huyeluz
- M –> Parar modo automático y pasar a modo manual
- C (CLAW) –>
- Claw Number
Con estas declaraciones, lo primero que vamos a hacer es declarar una serie de estados. El estado que guardaremos en la variable status nos servirá para que nuestro robot sepa que se encuentra en modo automático siguelineas o huyeluz, o en modo manual.
*IMPORTANTE: La pinza y los demás botones de movimiento aplican el modo manual desde el momento que son pulsados. Esto provoca que no podamos desarrollar un híbrido para la pinza con control manual y otro control de movimiento autómatico. Pero en el siguiente tutorial aprenderemos a hacerlo.
Una vez entendida esta parte, ahora solo nos hace falta completar e interpretar el condicional con las letras restantes, asociarles movimiento e introducir los modo de operación del siguelineas y del huyeluz.
Los modos de operación del siguelineas y del huyeluz los introduciremos en funciones a parte para mayor limpieza.
Las mismas funciones de movimiento que ya hemos desarrollado antes.
Y la ejecución principal del programa queda de la siguiente manera.
Finalmente el código de nuestro programa quedará de la siguiente forma. Tampoco es para tanto, 160 lineas de código para un robot en condiciones.
#include <Servo.h>
#include <SoftwareSerial.h>
int LDR_Left;
int LDR_Right;
int LDR_diff;
String status;
int threshold;
int diff_threshold;
int ClawMin;
int ClawMax;
char BTData;
int ClawValue;
Servo RightServo;
Servo LeftServo;
SoftwareSerial comm(2,3);
Servo ClawServo;
void FollowLineMode() {
Serial.println("FOLLOWLINE MODE");
if (digitalRead(4)) {
RightServo.write(90);
RightServo.detach();
} else {
RightServo.attach(10);
RightServo.write(180);
}
if (digitalRead(5)) {
LeftServo.write(90);
LeftServo.detach();
} else {
LeftServo.attach(9);
LeftServo.write(0);
}
}
void Lightmode() {
Serial.println("BEETLE MODE");
LDR_Left = (int)(analogRead(A0));
LDR_Right = (int)(analogRead(A1));
LDR_diff = LDR_Left - LDR_Right;
Serial.print(LDR_Left);
Serial.print(",");
Serial.println(LDR_Right);
if (abs(LDR_diff) > diff_threshold) {
if (LDR_diff > diff_threshold) {
turn_left();
} else if (LDR_diff < diff_threshold * -1) { turn_right(); } } else if (analogRead(A0) > threshold && analogRead(A1) > threshold) {
forward();
} else if (analogRead(A0) < threshold && analogRead(A1) < threshold) {
stop();
}
}
void ManualMode() {
Serial.println("Manual Mode");
}
void forward() {
Servo_attach();
LeftServo.write(0);
RightServo.write(180);
}
void backwards() {
Servo_attach();
LeftServo.write(180);
RightServo.write(0);
}
void turn_left() {
Servo_attach();
LeftServo.write(180);
RightServo.write(180);
}
void turn_right() {
Servo_attach();
LeftServo.write(0);
RightServo.write(0);
}
void stop() {
LeftServo.write(90);
RightServo.write(90);
Servo_detach();
}
void Servo_attach() {
LeftServo.attach(9);
RightServo.attach(10);
}
void Servo_detach() {
LeftServo.detach();
RightServo.detach();
}
void setup() {
pinMode(4, INPUT);
pinMode(5, INPUT);
pinMode(A0, INPUT);
pinMode(A1, INPUT);
Serial.begin(38400);
comm.begin(38400);
status = (String)("MANUAL");
LeftServo.attach(9);
RightServo.attach(10);
ClawServo.attach(6);
LDR_diff = (int)(0);
threshold = (int)(300);
diff_threshold = (int)(100);
ClawMin = (int)(40);
ClawMax = (int)(115);
}
void loop() {
if (comm.available()) {
BTData = (char)((comm.read()));
if (BTData == ('G')) {
Serial.println("BEETLE MODE AUTOMATIC");
status = "BEETLE";
} else if (BTData == ('I')) {
Serial.println("FOLLOWLINES MODE AUTOMATIC");
status = "FOLLOW";
} else if (BTData == ('M')) {
Serial.println("MANUAL CONTROL MODE ON");
status = "MANUAL";
} else if (BTData == ('U')) {
forward();
} else if (BTData == ('D')) {
backwards();
} else if (BTData == ('L')) {
turn_left();
} else if (BTData == ('R')) {
turn_right();
} else if (BTData == ('S')) {
stop();
} else if (BTData == ('C')) {
ClawValue = (int)(((comm.readString()).toInt()));
ClawServo.write((map(ClawValue,10, 55, ClawMin , ClawMax)));
}
}
if (status == "FOLLOW") {
FollowLineMode();
} else if (status == "BEETLE") {
Lightmode();
}
}
Ha costado, pero con este programa tendremos un ejercicio completo de robot. Pero aún se puede mejorar. Así que en el siguiente tutorial, explicaremos cómo.