Proyecto Del Robot Escorpion

Proyecto Fin de Carrera
Grado en Ingeniería de Tecnologías Industriales
Control de un robot escorpión con hardware Arduino
Autor: Fernando Carlos García García

Tutor: José Ángel Acosta Rodríguez
Equation Chapter 1 Section 1
Dep. Sistemas y Automática

Escuela Técnica Superior de Ingeniería
Universidad de Sevilla
Sevilla, 2017
Proyecto Fin de Carrera
Grado en Ingeniería de Tecnologías Industriales
Control de un robot escorpión con hardware

Arduino
Autor:
Fernando Carlos García García
Tutor:
José Ángel Acosta Rodríguez
Profesor titular
Dep. Sistemas y Automática

Escuela Técnica Superior de Ingeniería
Universidad de Sevilla
Sevilla, 2017
iii
Proyecto Fin de Carrera: Control de un robot escorpión con hardware Arduino
Autor: Fernando Carlos García García
Tutor: José Ángel Acosta Rodríguez
El tribunal nombrado para juzgar el Proyecto arriba indicado, compuesto por los siguientes miembros:
Presidente:
Vocales:
Secretario:
Acuerdan otorgarle la calificación de:
Sevilla, 2017
v
El Secretario del Tribunal
A mis padres
A mi hermana
vii
Agradecimientos
En primer lugar, quiero agradecer a mis padres por su paciencia infinita y por todo el esfuerzo que han hecho
por mí, y a mis compañeros de carrera, sobre todo a María y a Bea por aguantar estoicamente mis quejas y
lamentos, no sólo durante este trabajo sino durante toda la carrera.
A mi hermana María, por ser un referente en todos los aspectos de mi vida, incluso desde la distancia, y ser
prácticamente perfecta en todo.
Por último, agradecer a los profesores que me han enseñado durante estos cuatro años, y especialmente a José
Ángel Acosta, mi tutor en este proyecto.
ix
Resumen
En este proyecto se desarrolla el control del robot escorpión Bioloid Premium de la compañía ROBOTIS,
sustituyendo el controlador original del fabricante por otro basado en Arduino. Hemos buscado diferentes
controladores y lenguajes que pudiesen simular a los proporcionados por la compañía ya que éstos no explotan
todo el potencial del robot ni las características de los actuadores.
La posibilidad de modificar sistemas genéricos para conseguir características únicas puede llevar a obtener
sistemas que satisfagan necesidades específicas y adaptadas a los requerimientos de cada usuario. De esta
forma, se evita la dependencia de las restricciones impuestas por los fabricantes, que limitan el potencial de sus
dispositivos para su beneficio económico, además de la imposibilidad de obtener dispositivos genéricos que
satisfagan completamente a todos los usuarios.
Este trabajo es un ejemplo de esta personalización y puede ser el comienzo de un desarrollo futuro en el que se
amplíen aún más las capacidades del robot así como para aplicarlo a robots más complejos.
xi
Abstract
In this Project we have developed the control of the Bioloid Premium scorpion robot by ROBOTIS Company,
replacing the manufacturer’s original controller for another Arduino-based. Since the original controller
provided by the constructor do not use the whole potential of the robot nor the features of the actuators, we
have searched for different controllers and programming languages that could enhance the performance of the
original.
The possibility of modifying systems in order to achieve unique features can lead to new systems that can
satisfy specific needs and adapt to the requirements of each user, thus avoiding the dependency of the
manufacturer’s restrictions that limit the full potencial of the devices, besides the impossibility of making
generic devices that satisfy all the needs of the consumers.
This work is an example of this customization and can be seen as the start of a future development in which
we improve the performance of the robot as well as applying these concepts to more complex robots.
xiii
Índice
Agradecimientos ix
Resumen xi
Abstract xiii
Índice de Figuras xv
1 Introducción 1
1.1 Objetivos 1
1.2 Estructura de la memoria 1
2 Conceptos previos 3
2.1 Comunicación SPI 3
2.2 UART 5
3 Hardware 6
3.1 Motores 6
3.2 Sensores 8
3.2.1 Sensor DMS 8
3.3 Controlador 9
3.4 Comunicación entre los dispositivos 10
4 Software 12
4.1 Arduino IDE y C++ 12
4.2 RoboPlus 12
5 Librerías 14
5.1 ServoCds55 14
5.2 Slave_servo 18
6 Programación 21
6.1 Comunicación con los motores 21
6.2 Reprogramación del shield 22
6.3 Ampliación de la librería ServoCds55 23
6.4 Programación de la placa 27
7 Conclusiones 31
Referencias 32
Anexo I 33
Anexo II 39
ÍNDICE DE FIGURAS
Figura 2-1. Comunicación en paralelo. 3

Figura 2-2. Comunicación en serie. 3
Figura 2-3. Cableado para la comunicación entre maestro y esclavo. 4
Figura 2-4. Cableado para la comunicación SPI incluyendo la línea de selección de esclavo. 4
Figura 2-5. Esquema de la UART. 5
Figura 3-1. Robot escorpión de Bioloid Premium. 6
Figura 3-2. Actuador Dynamixel AX-12A. 7
Figura 3-3. Conexión en serie de los actuadores. 7
Figura 3-4. Sensor DMS de Sharp. 8
Figura 3-5. Conexión del sensor en nuestra placa. 8
Figura 3-6. Controlador CM-530. 9
Figura 3-7. Arduino Mega2560. 9
Figura 3-8. “Smart Arduino Digital Servo Shield for Dynamixel AX”. 10
Figura 3-9. Pines dedicados a la conexión SPI de la placa Arduino Mega2560. 10
Figura 4-1. Interfaz de RoboPlus Manager. 13
Figura 4-2. Interfaz de RoboPlus Motion. 13
Figura 6-1. Vista de planta del shield. 22
Figura 6-2. Vista de planta del FTDI. 23
xv
1 INTRODUCCIÓN
E
n este proyecto, vamos as desarrollar el control del robot escorpión de BIOLOID. Vamos a introducir
brevemente el propósito y la estructura de este trabajo antes de entrar en detalle con las distintas partes
que conforman el proyecto.
1.1 Objetivos
El objetivo principal de este trabajo es analizar el movimiento del robot escorpión en su configuración original
de fábrica y sustituir el controlador original por un controlador desarrollado por nosotros que sea capaz de
implementar las mismas funcionalidades pero con un mayor control por parte del programador para tener un
manejo más completo del robot más allá de lo permitido por el fabricante con el controlador original.
1.2 Estructura de la memoria

La memoria está dividida en cinco capítulos que reflejan el trabajo de recopilación de información y la
aplicación de este conocimiento para lograr el control del robot. A continuación se explican de forma sucinta
los contenidos de cada uno:
 Conceptos previos: breve introducción a los sistemas de comunicación SPI y UART usados por los
dispositivos durante el trabajo para comprender el funcionamiento y la interacción entre éstos.
 Hardware: presentación de los diferentes componentes del robot así como del controlador original y el
elegido para sustituir a éste y sus características.
 Software: descripción de los programas usados para realizar el control del robot y discusión sobre la
elección de éstos frente a otras posibilidades.
 Librerías: explicación de las librerías usadas, tanto para el control de los motores como para la
comunicación entre los distintos elementos.
 Programación: presentación de los distintos programas creados hasta obtener el movimiento completo
del robot y su interacción con el exterior por medio del sensor.
1
2 CONCEPTOS PREVIOS
A
ntes de comenzar a describir el trabajo realizado, vamos a definir unos conceptos que van a ser
utilizados durante todo el proyecto y que han sido objeto de investigación para poder comprender los
mecanismos de comunicación y de envío y recepción de datos entre los distintos dispositivos.
Concretamente, vamos a explicar la comunicación SPI y la UART, ya que son los dos sistemas de
comunicación usados. La comunicación SPI se da entre la placa de Arduino y el shield y la UART entre el
shield y los actuadores del robot. Todo esto se explicará en detalle más adelante.
2.1 Comunicación SPI

Es un método de comunicación usado para intercambiar datos entre microcontroladores y periféricos [1].
Existen una gran variedad de protocolos de comunicación pero generalmente se dividen en dos categorías:
serie y paralelo.
Figura 2-1. Comunicación en paralelo.
Figura 2-2. Comunicación en serie.
3
Conceptos previos
La comunicación en paralelo permite transmitir varios bits a la vez mediante varios cables, de manera que la
cantidad de datos transferida es enorme y a gran velocidad. Por otro lado, la comunicación en serie sólo posee
una línea para enviar datos, por lo que se envía un solo bit de cada vez, sin embargo, sólo requieren un único
cable. Esto hace que la velocidad sea mucho menor pero cuando la velocidad no es un problema, son mucho
más rentables.
El problema de los puertos serie es que la comunicación es asíncrona, es decir, que no hay un control sobre el
flujo de datos ni una garantía de que ambos extremos de la comunicación estén funcionando a la misma
velocidad por lo que no se garantiza el éxito de la comunicación. La forma de solucionar este problema es con
un protocolo de envío en el que al paquete de datos a enviar se le añaden unos bits adicionales para marcar el
inicio y el fin del mensaje. Ambos lados de la comunicación deben haberse puesto de acuerdo previamente con
el protocolo a usar para que ambos entiendan correctamente el contenido del mensaje.
Esto funciona bastante bien, sin embargo, aumenta la capacidad requerida en los dispositivos ya que deben
tratar con una mayor cantidad de datos. El sistema SPI utiliza una comunicación serie síncrona, en la que,
además de la línea de transmisión de datos, hay una línea para el reloj, lo que permite mantener a emisor y
receptor perfectamente sincronizados.
No obstante, este tipo de comunicación sólo permite la comunicación en un sentido, de maestro a esclavo. Para
poder enviar datos desde el esclavo al maestro, se necesita una línea adicional.
Figura 2-3. Cableado para la comunicación entre maestro y esclavo.
Cuando el maestro envía datos al esclavo, lo hace a través de la línea MOSI (Master Out/ Slave In). Si el
esclavo quiere enviar una respuesta de vuelta, lo hará por el canal MISO (Master In/ Slave Out). Al ser un
sistema “full-duplex”, se puede enviar y recibir información al mismo tiempo.
Otra característica de la comunicación SPI es que el maestro puede comunicarse con más de un esclavo. Para
poder elegir el esclavo con el que establecer la comunicación, se añade una línea adicional SS (Slave Select),
que sirve para despertar al esclavo con el que se quiere comunicar.
Figura 2-4. Cableado para la comunicación SPI incluyendo la línea de selección de esclavo.
4
Conceptos previos
2.2 UART
Se trata de una herramienta que incluyen algunos microcontroladores para convertir datos de un bus en
paralelo a uno en serie y viceversa. Cuando los datos llegan en formato paralelo, el UART crea el paquete de
datos con la información adicional necesaria y lo envía por el bus serie de transmisión a la velocidad
previamente establecida. En el sentido contrario, muestrea la línea de recepción de datos a la velocidad
establecida, retira los bits de sincronización y envía el mensaje por el bus paralelo.
Figura 2-5. Esquema de la UART.
5
3 HARDWARE
E
n este proyecto vamos a trabajar con el robot escorpión de la compañía ROBOTIS, perteneciente al
kit de ROBOTIS PREMIUM. Este kit permite al usuario construir distintos robots con las mismas
piezas y obtener diferentes formas y funcionalidades. Nosotros vamos a trabajar con la configuración
de escorpión sextúpedo. En esta sección vamos a describir los distintos elementos de hardware que forman
nuestro robot.
Figura 3-1. Robot escorpión de Bioloid Premium.
3.1 Motores
Los motores usados por el robot son Dynamixel AX-12. Son actuadores inteligentes que incorporan un
mecanismo reductor, un motor DC y una circuitería de control. Tienen la capacidad de conocer su posición,
velocidad, temperatura, carga y voltaje, entre otras cosas, y actuar frente a cambios en las condiciones de estas
variables. Su protocolo de comunicación es serie asíncrono half-duplex, es decir, que requieren información
adicional con el mensaje principal (1 bit de stop y 0 bits de paridad) y sólo hay un cable para la transmisión de
datos por lo que el envío y la recepción no pueden ser simultáneos. Los otros dos cables presentes son para la
alimentación y la tierra [2].
6
Hardware
Figura 3-2. Actuador Dynamixel AX-12A.
Estos motores tienen dos conectores, lo que permite conectarlos en serie en un único bus de la siguiente
manera:
Figura 3-3. Conexión en serie de los actuadores.
Cada actuador posee un número de identificación que debe ser único ya que, como se explicará más adelante,
la instrucción se envía con el identificador del motor al que se le manda la orden. Gracias a esto se pueden
conectar tantos motores como queramos porque sólo ejecutará la orden el motor elegido. Si el identificador
está mal asignado o hay más de un actuador con el mismo identificador, puede haber problemas de
interferencia y el robot no funcionará.
Para operar con los actuadores, el controlador debe tener soporte para convertir las señales UART en señales
del tipo half-duplex. Esta es una de las razones por las que nos decantamos por el shield que comentaremos
más adelante, ya que satisface estas características.
Los actuadores Dynamixel AX-12 tienen dos tipos de paquetes: el paquete de instrucciones, enviado del
controlador al motor, y el paquete de estado, enviado desde el motor al controlador.
También cuentan con una tabla de control que contiene la información sobre el estado del actuador. El
actuador opera cuando se le escriben valores en esta tabla de control y devuelve los datos de esta tabla cuando
se le envían órdenes de lectura y que está desarrollada en el Anexo II.
7
Hardware
3.2 Sensores
Los sensores son los dispositivos que permiten al robot interaccionar con el medio en el que se encuentra y
responder ante estímulos y cambios en las condiciones. En nuestro robot, vienen incluidos una serie de
sensores como sensores infrarrojos o giróscopos que no vamos a usar ya que no presentan una interfaz
compatible con el nuevo controlador. Sin embargo, incopora un sensor DMS, que sí es compatible y es el que
vamos a usar para el control de nuestro robot.
3.2.1 Sensor DMS
Se trata de un sensor infrarrojo para medir distancias fabricado por SHARP, usado en automóviles, televisores
o fotocopiadoras. Este sensor tiene una resolución alta ya que tiene una tecnología en la que el color de los
objetos tiene poca influencia en la medida y es capaz de medir con precisión entre 10 y 80 cm.
Figura 3-4. Sensor DMS de Sharp.
Figura 3-5. Conexión del sensor en nuestra placa.
8
Hardware
3.3 Controlador
El controlador original del kit ROBOTIS PREMIUM es el CM-530. Está basado en un microcontrolador
STM32F103RE, que sólo se puede programar a través de los lenguajes suministrados por la propia empresa
fabricante. Por tanto, desde el punto de vista del programador no permite una gran flexibilidad y el usuario está
sujeto a lo que el fabricante le permita modificar. Para obtener más funcionalidades y explotar el potencial del
robot, se ha decidido sustituir este controlador [3].
Figura 3-6. Controlador CM-530.
El controlador elegido para hacer esta tarea fue el Arduino Mega2560. Este controlador tiene capacidad para
proyectos más complejos que los demás modelos de su gama y además, está recomendado para proyectos en
robótica por el fabricante [4].
Figura 3-7. Arduino Mega2560.
9
Hardware
No obstante, para poder controlar todos los motores del robot a la vez y realizar la comunicación UART con
ellos, se hizo necesario incorporar un shield. Los shields son placas que se conectan a los pines de una placa de
Arduino para extender sus capacidades.
Nos decantamos por el “Smart Arduino Digital Servo Shield for Dynamixel AX” de DFRobots ya que tiene
una interfaz compatible con nuestro controlador de Arduino y además está desarrollado específicamente para
el control de los servomotores Dynamixel AX y posee el mismo tipo de puerto para la conexión de los
actuadores que el controlador original. Esta placa tiene integrado un circuito half-duplex requerido por los
motores Dynamixel para la comunicación UART. Además, es capaz de dar potencia suficiente y permite la
conexión de hasta 200 servos [5].
Figura 3-8. “Smart Arduino Digital Servo Shield for Dynamixel AX”.
La placa también cuenta con unas librerías propias, específicas para el manejo de los servomotores de
Dynamixel, para poder hacer la programación directamente desde el software de Arduino. Por otro lado,
cuenta con un microcontrolador ATmega8A con una serie de funciones especialmente diseñadas para la
comunicación con los motores de Dynamixel ya que las funciones que incluye tienen incorporado el protocolo
de comunicación para enviar los datos de forma que los motores entiendan las instrucciones.
3.4 Comunicación entre los dispositivos

Tanto la placa de Arduino Mega 2560 como el shield cuentan con unos pines especiales dedicados a la
comunicación SPI.
Figura 3-9. Pines dedicados a la conexión SPI de la placa Arduino Mega2560.
10
Hardware
Cuando el maestro (el Arduino) quiere enviar datos al esclavo (el shield), pone a nivel bajo la línea SS (el pin
10) antes de comenzar la transmisión, de forma que el shield se despierte y espere la llegada de los datos que
se le envían a continuación. Dependiendo del primer dato enviado, el código del esclavo elige entre una
casuística y almacena en un vector el resto de datos para procesarlos antes de mandarlos a los servomotores.
Una vez que los datos están listos para enviarse, es decir, que se dividen y ordenan tal y como se especifica en
la sección “Comunicación con los motores”, comienza la comunicación entre el shield y los servomotores a
través de la línea UART de la interfaz de conexión presente en el shield. El protocolo de envío de datos está
establecido por el fabricante de los servos
.
11
4 SOFTWARE
E
l planteamiento inicial consistía en hacer la programación en lenguaje Matlab. Para ello, era necesario
traducir a este lenguaje las librerías que nos había proporcionado el fabricante del shield que ayuda al
Arduino en el control del robot, que estaban en lenguaje C++, para poder hacer la programación del
control desde esta plataforma. No obstante, tras buscar información sobre este tema, llegamos a la conclusión
de que la interfaz de Matlab con Arduino es relativamente reciente y la documentación no está muy
desarrollada. Después de revisar la documentación disponible, no fuimos capaces de lograr dicha traducción
debido a la complejidad del código y a la dificultad de la programación necesaria que se excedía del trabajo
pensado en la propuesta original [6].
Por tanto, decidimos hacer la programación en el software de Arduino IDE que posee las características
necesarias para empezar a trabajar con las librerías y funciones del robot directamente sin necesidad de realizar
ningún cambio. También fue necesario usar el programa de RoboPlus del fabricante para obtener información
de los actuadores en momentos puntuales.
4.1 Arduino IDE y C++

Dado que hemos elegido el controlador Arduino Mega2560 para realizar las funciones del anterior CM-530,
parece lógico elegir este software como lenguaje de programación para el control del robot.
El software de Arduino (Integrated Development Environment) permite escribir programas de forma sencilla y
cargarlos en la placa de Arduino de forma directa a través de la conexión entre la placa y el ordenador con un
cable USB. Este software permite, además de escribir programas, obtener datos de la placa, leer los sensores
conectados a ésta y comunicarse con ella. También cuenta con un monitor en el que se pueden leer por
pantalla los valores que se están recibiendo y escribir datos para que se envíen a la placa [5].
El software de Arduino IDE utiliza una serie de librerías, desarrolladas en lenguaje C++, por lo que ha sido
necesario utilizar este software para poder hacer las modificaciones y ampliaciones.
4.2 RoboPlus
Para obtener la secuencia de movimiento para cada función era necesario obtener el valor de la posición y
velocidad de los motores. Debido a la imposibilidad de leer la posición y obtener otros datos de los actuadores
por razones que se detallarán más adelante, se hizo necesario utilizar el programa RoboPlus proporcionado por
el fabricante del robot. Con este programa, aunque no podíamos hacer una programación en la que el robot
funcionase de forma autónoma o que interaccionase con la información de los sensores, sí podíamos
posicionar los motores en posiciones exactas o leer el estado de los actuadores y simular el movimiento que
luego íbamos a implementar en nuestro código de Arduino.
12
Software
Figura 4-1. Interfaz de RoboPlus Manager.
Figura 4-2. Interfaz de RoboPlus Motion.
Además, ha sido muy necesario y muy útil cuando los identificadores de los actuadores se modificaban y era
necesario volver a renombrarlos todos, para lo cual había que conectar el actuador de forma individual al
controlador, desconectándolo del resto de actuadores del robot. Este programa sólo funciona cuando el
controlador es el CM-530 del fabricante, no reconoce a la placa de Arduino.
13
5 LIBRERÍAS
A
continuación vamos a definir las librerías usadas para la programación en Arduino IDE. La librería
“ServoCds55” incluye las funciones a las que llama el programa instalado en la placa mientras que la
librería “Slave_servo” es el programa instalado en el shield.
5.1 ServoCds55
Esta librería ha sido proporcionada por el fabricante como ejemplo para mover los actuadores y realizar
algunas funciones básicas como resetear los valores de la tabla de control, cambiar el identificador (aunque
previamente debes conocer el actual) o cambiar entre el modo motor, en el que puede girar en los 360º, y el
modo servo, en el que está limitado el movimiento entre 0 y 300º.
Función de inicialización. Sirve únicamente para inicializar el actuador y establecer los límites de la posición y
la velocidad. La entrada de esta función es el pin dedicado al esclavo. En este caso, va a ser el 10 y se va a
guardar como una constante llamada “CS”.
ServoCds55::ServoCds55 (int CS):cs(CS)
{
velocity_temp = 150;
upperLimit_temp = 300;
}
Función para comenzar la comunicación SPI. Para ello, indica el pin dedicado al esclavo (cs), lo pone a nivel
alto para que no haya transferencia de datos al inicio y llama a la función de la librería SPI, integrada ya en el
software de Arduino IDE, para iniciar la comunicación, estableciendo la velocidad del reloj durante la
transmisión.
void ServoCds55::begin()
{
pinMode(cs,OUTPUT);
digitalWrite(cs,HIGH);
SPI.begin ();
SPI.setClockDivider(SPI_CLOCK_DIV8);
}
14
Librerías
Transmisión de un dato y espera un cierto tiempo. Manda un dato por el pin MOSI y guarda el valor que el
esclavo le manda de vuelta en la variable ‘a’. Espera un cierto tiempo antes de devolver este valor. El
parámetro de entrada es el byte a enviar y el de salida es el byte devuelto por el esclavo. Ambos parámetros
son bytes ya que es el formato de los datos para el envío por el SPI.
byte ServoCds55::transferAndWait (const byte what)
{
byte a = SPI.transfer (what);
delayMicroseconds (20);
return a;
}
Modificación la velocidad de giro del actuador. Como entrada se indica la nueva velocidad deseada.
void ServoCds55::setVelocity(int velocity){
velocity_temp = velocity;
}
Modificación la posición límite del servo. Igual que la anterior, el parámetro de entrada a la función es la
nueva posición límite.
void ServoCds55::setPoslimit(int posLimit)
{
upperLimit_temp = posLimit;
}
Función para modificar la posición llamada por el programa. Como parámetros hay que indicar el identificador
del actuador al que se quiere mandar la instrucción y la posición deseada. Llama a otra función
(“SetServoLimit”) para configurar el actuador en modo servo y a la función (“WritePos”) para moverlo a la
posición indicada. Estas funciones son las que establecen la comunicación con el shield. Ambas se detallan
más adelante.
void ServoCds55::write(int ID,int Pos)
{
SetServoLimit(ID,upperLimit_temp);
WritePos(ID,Pos);
}
Cambio de la configuración del actuador de servo a motor. Para ello, elimina el límite de la posición y hace
que gire sin fin a una velocidad determinada. La función “SetMotorMode” es la que se comunica con el shield
y configura al actuador como motor.
void ServoCds55::rotate(int ID,int velocity)
{
SetServoLimit(ID,0);
delay(100);
SetMotormode(ID,velocity);
}
Función para modificar la posición llamada por la función “write”. Esta es la función que establece la
comunicación con el shield por lo que incluye los datos necesarios para que luego el shield los envíe al
actuador según el protocolo establecido. Nótese que para este tipo de función, como sólo se pueden enviar los
15
Librerías
datos de byte en byte por el SPI, para enviar la posición y la velocidad, es necesario hacerlo en dos tiempos, ya
que pueden alcanzar valores superiores a 255 (0xFF en hexadecimal, 11111111 en binario). Las variables
“PosS” y “velocityS” guardan los ocho primeros bits y “PosB” y “velocityB” los ocho siguientes.
void ServoCds55::WritePos(int ID, int Pos)
{
int PosB = (Pos>>8 & 0xff);
int PosS = (Pos & 0xff);
int velocityB = (velocity_temp>>8 & 0xff);
int velocityS = (velocity_temp & 0xff);
digitalWrite(cs, LOW);
transferAndWait ('p');
transferAndWait (ID);
transferAndWait (PosB);
transferAndWait (PosS);
transferAndWait (velocityB);
transferAndWait (velocityS);
transferAndWait ('\t');
transferAndWait ('\r');
transferAndWait ('\n');
digitalWrite(cs, HIGH);
delay(10);
}
Función para establecer el límite superior de la posición que puede alcanzar el actuador. Hay que indicar el
identificador y la posición límite.
void ServoCds55::SetServoLimit(int ID, int upperLimit_temp){
int upperLimitB = (upperLimit_temp>>8 & 0xff);

int upperLimitS = (upperLimit_temp & 0xff);
transferAndWait ('s');
transferAndWait (upperLimitB);
transferAndWait (upperLimitS);
delay(10);
}
Llamada por “rotate”, manda los datos necesarios para que el actuador funcione como motor en lugar de como
servo. Los parámetros necesarios son el identificador y la velocidad de giro.
void ServoCds55::SetMotormode(int ID, int velocity)
{
int velocityB = (velocity>>8 & 0xff);
int velocityS = (velocity & 0xff);
transferAndWait ('m');
16
Librerías
transferAndWait (velocityB);
transferAndWait (velocityS);
delay(10);
}
Función para cambiar el identificador del actuador. Es necesario enviarle tanto el nuevo identificador como el
anterior para que la instrucción llegue al actuador elegido.
void ServoCds55::SetID(int ID, int newID)
{
transferAndWait ('i');
transferAndWait (newID);
delay(10);
}
Función para reestablecer los valores de la tabla de control del actuador a los valores de fábrica. Únicamente
hay que indicar el identificador del actuador que se quiere resetear.
void ServoCds55::Reset(int ID)
{
transferAndWait ('r');
delay(10);
}
Ya que no hay comunicación desde el actuador hacia el controlador, ninguna de las funciones anteriores
devuelve ningún dato.
17
Librerías
5.2 Slave_servo
El código que se detalla a continuación es el que está programado en el microcontrolador del shield. Debido a
su extensión, vamos a presentar sólo algunas funciones de ejemplo para que se comprenda el funcionamiento
básico del código. El resto del código se presentará en el Anexo I.
En primer lugar, tras la declaración de las variables globales y las constantes que se van a usar a lo largo del
código, comienza el código con la función “setup”, necesaria en todos los programas en Arduino IDE. Esta
función inicializa variables, los modos de los pines y librerías entre otras cosas. Esta función sólo se ejecuta
una vez tras cada encendido o reseteo de la placa.
En este programa en concreto, establece el canal por el que el maestro envía datos al esclavo, habilita el SPI
para establecer la comunicación y habilita las interrupciones por llegada de datos con la función
“SPI.attachInterrupt”.
void setup()
{
Serial.begin(1000000); // start serial port at 1000000 bps:
// Serial.begin(115200); // start serial port at 1000000 bps:
pinMode(MISO,OUTPUT);
SPCR |= _BV(SPE);
SPI.attachInterrupt();
Servomode=false;
PositionLimit=false;
Motormode=false;
IDreset=false;
ServoRst=false;
}
A continuación se define la rutina de interrupción. El microcontrolador integrado en la placa de Arduino

(Atmega2560) permite una serie de interrupciones ya sea por cambios en los pines definidos para
interrupciones o por otras causas como temporizadores, conversiones de analógico a digital o por la llegada de
datos por medio de los distintos sistemas de comunicación como el I2C o el SPI. En nuestro caso, es ésta
última la interrupción que se da. El dato que llega (SPDR), se guarda en la variable “c”. Inicialmente,
“command” es ‘0’ por lo que la primera vez siempre entrará en el caso ‘0’, donde se copia en “command”.
Como se ha modificado este valor, ya el programa no va a volver a entrar en este caso hasta que no se acabe la
comunicación por lo que “command” se mantiene constante durante la transimisión.
El primer dato que llega es la letra para elegir la casuística y es el que se almacena en “command”. Como ya
hemos dicho, este valor no va a volverse a cambiar, y se entrará siempre en el mismo caso hasta que la
transmisión se acabe.
ISR (SPI_STC_vect)
{
byte c = SPDR;
switch(command)
{
case 0:
command = c;
break;
...
18
Librerías
Vamos a poner el ejemplo del código para mover el actuador a una posición específica, cuando se envía ‘p’
por el monitor. Cada dato que llega se va almacenando en un vector “Command” que tiene tamaño variable. A
medida que van llegando datos, se va incrementando el número elementos. El programa considera que el
mensaje ha terminado cuando los tres últimos caracteres que llegan son ‘\t’, ‘\r’ y ‘\n’, de ahí que todas las
funciones de la librería ServoCds55 que establecen comunicación con el esclavo manden estos datos.
Una vez que el mensaje se considera concluido, se separan los datos. El primero en llegar es el identificador y
a continuación los parámetros que se quieren modificar. Como se ha comentado antes, los datos de la posición
y la velocidad se envían divididos por lo que hay que volver a unirlos mediante una operación. La variable que
guardaba los ocho primeros bits se mantiene igual pero la que guardaba los ocho siguientes se multiplica por
256 y luego se suman. Posteriormente, se mapean los valores ya que los valores máximos y mínimos que
pueden tomar están limitados. Lo último que se hace es cambiar el valor de la variable booleana “Servomode”
y reestablecer la longitud del vector donde se guardan los componentes a cero para la siguiente llegada de
datos.
...
case 'p':
Command[reclength]= c;
reclength ++;
if(Command[reclength-3]=='\t' && Command[reclength-2]=='\r' &&
Command[reclength-1]=='\n'){
ID=Command[0];
Pos=Command[1]*256+Command[2];
velocity=Command[3]*256+Command[4];
velocity=map(velocity,0,300,0x0000,0x03FF);
Pos=map(Pos,0,300,0x0000,0x03FF);
Servomode=true;
reclength=0;
}
break;
...
Lo siguiente que encontramos en el código es el bucle “loop”. Esta función se repite constantemente y
contiene las funciones que ejecuta la placa de Arduino. En nuestro programa, el bucle está continuamente
esperando a que alguna de las variables booleanas se active. Cuando alguna de ellas lo hace, se ejecuta la
función que es la que finalmente envía los datos al actuador. Cuando la comunicación se termina y la placa de
Arduino pone el pin del esclavo a nivel alto para no enviar más datos, se reinicia la variable “command” para
que pueda volver a entrar en el caso ‘0’ en la siguiente comunicación y poder empezar el procesado de los
datos que llegan de nuevo.
void loop()
{
if(Servomode){
WritePos(ID,Pos,velocity);
Servomode=false;
}
...
...
...
if (digitalRead (SS) == HIGH)
command = 0;
}
...
19
Librerías
Tras el bucle anterior, están declaradas las funciones a las que llama el bucle, que son las que finalmente
mandan los datos al actuador según el protocolo establecido con el fabricante. Como podemos observar en el
ejemplo, hay que volver a descomponer los valores de la posición y la velocidad y enviarlos de forma
separada. También se puede observar la implementación del protocolo de comunicación que se detallarán en la
siguiente sección, con los dos bytes de inicio, el identificador, la longitud del mensaje, etc.
Los datos se envían por el puerto serie ya que el shield envía los datos a los actuadores mediante la UART.
Nósete que los datos que se envían con caracteres y no con valores numéricos son constantes definidas al
inicio del programa y que se mostrarán en el código completo en el Anexo I. La función encargada de enviar
definitivamente los datos a los actuadores recibe como parámetros de entrada el identificador del servomotor
sobre el que se quiere actuar y los parámetros que se quieren modificar. Esta función vuelve a descomponer
los datos que toman valores mayores de 255 para obtener dos bytes y enviarlos de forma separada. Es
importante mantener el orden de envío tal y como lo especifica el fabricante ya que de otra forma el actuador
no interpretaría los datos correctamente.
void WritePos(int ID,int Pos,int velocity)
{
int messageLength = 7;
byte pos2 = (Pos>>8 & 0xff);
byte pos = (Pos & 0xff);
byte vel2 = (velocity>>8 & 0xff);

byte vel = (velocity & 0xff);
Serial.write(startByte);
Serial.write(ID);
Serial.write(messageLength);
Serial.write(INST_WRITE);
Serial.write(P_GOAL_POSITION_L);
Serial.write(pos);
Serial.write(pos2);
Serial.write(vel);
Serial.write(vel2);
Serial.write((~(ID + (messageLength) + INST_WRITE + P_GOAL_POSITION_L + pos +
pos2 + vel + vel2))&0xFF);
}
20
6 PROGRAMACIÓN
E
n esta sección, vamos a explicar los diferentes programas que hemos realizado hasta lograr el
funcionamiento completo del robot. Previamente, vamos a hablar del protocolo de comunicación que es
necesario incluir en la programación como hemos visto en la sección de Librerías para la correcta
comunicación entre el controlador y el actuador.
6.1 Comunicación con los motores

Como ya hemos dicho, la comunicación se realiza de forma asíncrona por un único cable de envío y recepción.
Por tanto, se hace necesario establecer un protocolo de comunicación, a parte del ya mencionado con 1 bit de
stop y sin bit de paridad. El controlador, para mandar datos al motor, tiene que hacerlo de una forma específica
y en un orden determinado.
En el paquete enviado del controlador al actuador (paquete de instrucción), en primer lugar se envían dos bytes
de inicio con valor 0xFF, seguidos del número del identificador del motor con el que se va a comunicar. Dado
que pueden existir varios actuadores conectados en serie, si todos los motores tuviesen el mismo identificador,
todos responderían a la vez y por la misma línea, con el consiguiente error de comunicación. Por esto, es muy
importante que cada actuador tenga su propio identificador y especificarlo al inicio del mensaje, para que sólo
reciba el que nos interesa. Si nos interesa mandar una misma instrucción a todos los actuadores y no nos
interesa el paquete de vuelta, se puede elegir como identificador el 0xFE.
Una vez especificado el identificador, hay que indicar la longitud del mensaje que se va a enviar y a
continuación, la instrucción a ejecutar por el actuador. Esta puede ser de lectura, de escritura o de reset por
ejemplo. Después de la instrucción, se indica la dirección de escritura, es decir, el lugar en la tabla de
contenido del actuador en el que se va a escribir, junto con los parámetros necesarios para la dirección
seleccionada (en el manual de los actuadores viene especificado cuántos parámetros con necesarios para cada
dirección) y finalmente, se envían un byte de comprobación que contiene el opuesto en binario de la suma de
todos los valores enviados en el mensaje.
De forma similar, el paquete enviado por el motor al controlador (paquete de estado) contiene los dos bytes de
comienzo con valor 0xFF, seguido por el identificador del motor que está enviando los datos, de la longitud
del mensaje enviado, del error, si es que lo ha habido, y de algunos parámetros adicionales, como por ejemplo,
el dato de la posición que se pidió en la instrucción enviada por el controlador. Igual que en el caso del paquete
de instrucción, el último byte contiene la suma de comprobación de todos los bytes enviados previamente.
Vamos a ilustrar esto con un ejemplo para mandar una instrucción con el objetivo de que el motor se mueva a
la posición de 180º a una velocidad de 57 RPM:
0xFF 0xFF 0x01 0x07 0x03 0x1E 0x00 0x02 0x00 0x02 0xD2
Bytes de ID Longitud Instrucción Dirección Posición Velocidad Checksum

inicio del mensaje de escritura
dd
21
Programación
A la hora de escribir datos sobre los motores, como la posición o la velocidad, hay que tener en cuenta que los
valores del motor están mapeados entre 0 y 1023 (0x00 y 0x3FF en hexadecimal). Por ejemplo, 0x00 es la
posición de 0º y 0x3FF es la posición máxima de 300º. De la misma forma, 0x00 indica una velocidad angular
de 0 RPM y 0x3FF una velocidad máxima equivalente a 114 RPM.
La dirección de escritura es la posición en la tabla de control que contiene los datos almacenados de los
motores. Por ejemplo, la dirección 0x00 es el número de modelo del motor, y la dirección 0x03 es el
identificador del motor. Entonces, cuando queremos cambiar algún dato en los motores, hay que especificar la
dirección de escritura para que el motor sepa qué parámetro estamos modificando y sobreescribirlo en su tabla
de control. Además, dependiendo de la dirección elegida, el número de parámetros a enviar varía. En este
ejemplo, al sumar los datos enviados se obtiene 45, 00101101 en binario. El opuesto sería 11010010, que
equivaldría a 210, 0xD2 en hexadecimal.
Las principales instrucciones que se pueden enviar a los actuadores son para escribir datos en la tabla de
control para cambiar el estado del actuador (posición, velocidad, identificador…) y para leer datos de la tabla
sobre el estado (temperatura, carga, posición…). Pero también tiene otras posibilidades:
- La instrucción PING, que no ejecuta ninguna acción, solo sirve para recibir un paquete de estado.
- Las instrucciones REG_WRITE y ACTION. La primera escribe el dato que se desea modificar pero
no se ejecuta hasta que no se dé la segunda.
- La instrucción RESET, para devolver los valores de la tabla de control a los valores predeterminados
de fábrica.
- Y la instrucción SYNC_WRITE, para poder mandar órdenes a más de un actuador y que se ejecuten
de forma simultánea.
6.2 Reprogramación del shield

Para obtener las características que deseábamos para nuestro control y poder mejorar las prestaciones del
controlador del fabricante, pensamos en modificar la librería del shield. Sin embargo, tras un poco de
investigación, llegamos a la conclusión de que no se trataba de una librería como las de los ejemplos de
Arduino, sino que se trataba de la programación interna del microcontrolador integrado en el shield. Según el
fabricante, esta placa cuenta con una interfaz UART para poder hacer la reprogramación.
Interfaz UART para

ATmega8A
Figura 6-1. Vista de planta del shield.
22
Programación
Para poder llevarlo a cabo, el fabricante recomendaba usar una placa FTDI FT232RL que convierte las señales
USB en UART. Es decir, que con esta placa conectada al ordenador se cargaba el programa a través del cable
USB y ésta convertía la señal en UART para modificar el código del microcontrolador.
Figura 6-2. Vista de planta del FTDI.
Debido a la complejidad de la reprogramación y al aumento de la carga de trabajo, decidimos no seguir con

este procedimiento y decidimos continuar el control del motor con las funciones que ya venían dadas por el
fabricante.
6.3 Ampliación de la librería ServoCds55

Para implementar el movimiento del robot con las funciones de las que disponíamos, decidimos introducir una
serie de funciones nuevas que ejecutaban las anteriores de forma que el resultado final fuese el mismo que el
que se obtendría con el controlador original.
La primera función que se ejecuta cuando se resetea el robot o se carga un nuevo programa es “Init”. Esta
función recuerda a lo actuadores su número de identificación y los “despierta”. Tras un tiempo de espera, que
es relativamente largo pero que, tras varios intentos, determinamos que era necesario para que el robot
funcionase correctamente, mueve a todos los actuadores a una posición de 150º para que el robot comience el
movimiento desde una posición determinada.
void ServoCds55::Init()
{
int i=1;
for (int buf = 1; buf <= 255; buf++) {
SetID(buf, i);
i++;
}
delay(4000);
for (i=0;i<20;i++){
Serial.print("moviendo ID");
Serial.print(i-1);
23
Programación
Serial.print(" a la posicion: ");

Serial.println(150);
write(i-1, 150);
delay(2000);
}
}
A continuación, los siguientes programas contienen vectores con las posiciones de los 18 actuadores y van
ejecutando las órdenes para el movimiento de cada actuador. Las instrucciones se mandan de una en una ya
que la función para mandar varias instrucciones a la vez (SYNC_WRITE) no está disponible. Sin embargo,
dado que el tiempo de ejecución del programa es mucho más rápido que el tiempo que tardan los actuadores
en llegar a las posiciones marcadas, a simple vista el movimiento parece simultáneo. Estas posiciones han sido
determinadas con el programa RoboPlus como se mencionó en la sección de “Software”.
Como se puede observar en todas las funciones que se presentan posteriormente, los vectores que indican las
posiciones objetivo tienen dos elementos más que el número de servos. Estos valores no tienen ninguna
función especial ni representan nada pero experimentalmente, sin estos dos valores al inicio el robot no
ejecutaba correctamente el movimiento ya que no movía los dos primeros actuadores.
La función ejecuta un bucle que recorre el vector de posiciones y envía el elemento del vector al actuador
correspondiente por el puerto serie. Para ello, se usa la función “write” descrita en el apartado anterior.
void ServoCds55::Base()
{
int i=1;
int base[]={0, 0, 126, 173, 120, 179, 150, 150, 150, 150, 150, 150, 179, 120,
150, 150, 188, 195, 219, 234};
for (i=0;i<20;i++){
Serial.print("moviendo ID");
Serial.print(i-1);
Serial.print(" a la posicion: ");
Serial.println(base[i]);
write(i-1, base[i]);
}
}
Para las funciones que implican un movimiento más contínuo del robot, se ha incluido la variable “rep” que
indica el número de veces que se repite el movimiento. Este valor se indica como parámetro de entrada a la
función.
void ServoCds55::Forward(int rep)
{
int i=1;
int paso1[]= {0, 0, 126, 173, 147, 197, 150, 170, 150, 150, 124, 150, 188,
146, 150, 170, 188, 175, 204, 235};
int paso2[]= {0, 0, 126, 173, 147, 197, 130, 150, 150, 150, 150, 176, 188,
146, 130, 150, 188, 175, 204, 235};
int paso3[]= {0, 0, 126, 173, 103, 153, 130, 150, 150, 150, 150, 176, 154,
112, 130, 150, 188, 175, 204, 235};
int paso4[]= {0, 0, 126, 173, 103, 153, 150, 170, 150, 150, 124, 150, 154,
112, 150, 170, 188, 175, 204, 235};
setVelocity(25);
24
Programación
while(rep){
for (i=0;i<20;i++){
write(i-1, paso1[i]);
}
for (i=0;i<20;i++){
}
for (i=0;i<20;i++){
}
for (i=0;i<20;i++){
}
rep--;
}
}
void ServoCds55::Backward(int rep)

{
int i=1;
int paso1[]= {0, 0, 126, 173, 103, 153, 150, 170, 150, 150, 129, 158, 153,
112, 150, 170, 188, 175, 204, 235};
int paso2[]= {0, 0, 126, 173, 103, 153, 129, 149, 150, 150, 141, 176, 153,
112, 129, 150, 188, 175, 204, 235};
int paso3[]= {0, 0, 126, 173, 146, 196, 129, 149, 150, 150, 141, 176, 187,
146, 129, 150, 188, 175, 204, 235};
int paso4[]= {0, 0, 126, 173, 146, 196, 150, 170, 150, 150, 129, 158, 187,
146, 150, 170, 188, 175, 204, 235};
setVelocity(20);
while(rep){
for (i=0;i<20;i++){
}
for (i=0;i<20;i++){
}
for (i=0;i<20;i++){
}
for (i=0;i<20;i++){
}
rep--;
}
}
25
Programación
Para la función de ataque (“Attack”), al ser un movimiento más rápido y agresivo es necesario modificar la
velocidad de los actuadores.
void ServoCds55::Attack()
{
int i=1;
int paso1[]= {0, 0, 150, 149, 120, 179, 109, 190, 150, 150, 129, 170, 179,
120, 150, 150, 188, 190, 185, 178};
int paso2[]= {0, 0, 150, 150, 120, 179, 91, 208, 150, 150, 114, 182, 167,
132, 156, 144, 188, 255, 233, 166};
int paso3[]= {0, 0, 150, 149, 120, 179, 109, 190, 150, 150, 129, 170, 179,
120, 150, 150, 188, 191, 211, 217};
setVelocity(100);
for (i=0;i<20;i++){
}
for (i=0;i<20;i++){
}
for (i=0;i<20;i++){
}
}
void ServoCds55::Turn(int rep)

{
int i=1;
int paso1[] = {0, 0, 126, 173, 120, 150, 129, 150, 150, 150, 150, 150, 179,
120, 129, 150, 188, 175, 204, 235};
int paso2[] = {0, 0, 126, 173, 106, 150, 129, 150, 150, 150, 150, 150, 150,
120, 129, 150, 188, 175, 204, 235};
int paso3[] = {0, 0, 126, 173, 106, 150, 150, 150, 150, 150, 150, 150, 150,
120, 150, 150, 188, 175, 204, 235};
int paso4[] = {0, 0, 126, 173, 106, 150, 150, 150, 150, 150, 129, 150, 150,
120, 150, 150, 188, 175, 204, 235};
int paso5[] = {0, 0, 126, 173, 150, 150, 150, 150, 150, 150, 129, 150, 179,
120, 150, 150, 188, 175, 204, 235};
int paso6[] = {0, 0, 126, 173, 150, 150, 150, 150, 150, 150, 150, 170, 179,
120, 150, 150, 188, 175, 204, 235};
int paso7[] = {0, 0, 126, 173, 150, 194, 150, 150, 150, 150, 150, 170, 179,
150, 150, 150, 188, 175, 204, 235};
int paso8[] = {0, 0, 126, 173, 150, 194, 150, 150, 150, 150, 150, 150, 179,
150, 150, 150, 188, 175, 204, 235};
int paso9[] = {0, 0, 126, 173, 150, 194, 150, 170, 150, 150, 150, 150, 179,
150, 150, 170, 188, 175, 204, 235};
int paso10[]= {0, 0, 126, 173, 150, 179, 150, 170, 150, 150, 150, 150, 179,
120, 150, 170, 188, 175,
204, 235};
setVelocity(100);
26
Programación
while(rep){
for (i=0;i<20;i++){
}
delay(500);
for (i=0;i<20;i++){
}
for (i=0;i<20;i++){
}
for (i=0;i<20;i++){
}
for (i=0;i<20;i++){
}
for (i=0;i<20;i++){
}
for (i=0;i<20;i++){
}
for (i=0;i<20;i++){
}
for (i=0;i<20;i++){
}
for (i=0;i<20;i++){
}
rep--;
}
}
6.4 Programación de la placa

Para obtener el movimiento completo, hemos desarrollado una serie de programas para ir comprobando paso a
paso que el robot obedecía las instrucciones que se le mandaban. No vamos a presentar todos los ensayos que
hicimos pero sí vamos a mostrar un programa muy simple que comprueba que todas las funciones de la
ampliación de la librería que hemos mencionado antes funcionan correctamente y posteriormente el programa
final que logra el movimiento completo.
En este código, el bucle principal espera a que llegue un comando por el monitor enviado por teclado y a
continuación, ejecuta la función “controlServo” y envía el comando que se ha escrito por teclado como
entrada. Esta función es la que elige la función a ejecutar en el robot según el comando de entrada.
27
Programación
char inputCommand ;
boolean inputComplete = false;
int rep=4;
void setup (){

Serial.begin (115200);
myservo.begin ();
}
void loop (){

serialEvent();
if (inputComplete) {
Serial.print("Your command is: ");
Serial.println(inputCommand);
Serial.println("");
controlServo(inputCommand);
inputCommand = 0;
inputComplete = false;
}
}
void serialEvent() {
while (Serial.available()) {
char inChar = (char)Serial.read();
if (inChar == '\n') {
inputComplete = true;
break;
}
inputCommand += inChar;
}
}
void controlServo(char val) {

switch (val) {
case 'b':
myservo.Base(); //Posición base
delay(2000);
break;
case 'i':
myservo.Init(); //inicio del robot
delay(2000);
break;
case 'f':
myservo.Forward(rep); //movimiento de avance
delay(2000);
break;
case 'k':
myservo.Backward(rep); //movimiento de retroceso
delay(2000);
break;
case 'a':
myservo.Attack(); //ataque
delay(2000);
break;
28
Programación
case 't':
myservo.Turn(5); //giro
delay(2000);
break;
default:
Serial.println("Please give me an available instruction:");
}
}
Una vez conseguida la ejecución por separado de estas nuevas funciones, había que integrarlas en un programa
de forma que el robot pudiese actuar de forma autónoma. Para ello, incluimos el sensor DMS, para obtener
información del entorno y que el robot actuase en consecuencia.
El planteamiento es que el robot camine de forma contínua y cuando encuentre un objeto a una determinada
distancia, gire para evitarlo. También se le ha incluido la funcionalidad de que si el sensor detecta un objeto a
menos de una cierta distancia que se considere pequeña y muy cercana al robot, lo detecte como una amenaza
y se pare para atacar.
El principal problema encontrado fue la necesidad de realizar dos acciones simultáneamente, leer los datos del
sensor y avanzar. Para ello, se incluyó la librería “Scheduler” que permite mantener dos bucles funcionando al
mismo tiempo. De esta forma, una tarea no interrumpe a la otra.
Como ya comentamos, todos los códigos en el software de Arduino comienzan con la función “setup” para
inicializar variables, otras librerías, etc. En este caso, inicializa la comunicación serie y la comunicación SPI
con el shield, ejecuta la función para establecer el primer contacto con los actuadores y por último, incluye el
segundo bucle y lo ejecuta. Para este segundo bucle es necesario declarar otra función “setup”, a la que hemos
llamado “setup1” con las variables e inicializaciones que requiera este bucle secundario, llamado “robot”. En
nuestro caso, está vacía ya que no tiene ningún requerimiento especial.
#include <Scheduler.h>
#include <SPI.h>
#include <ServoCds55.h>
ServoCds55 myservo;
#define PIN 46
const int DMS=0;
int flag=0;
void setup(){
Serial.begin (115200);
myservo.begin ();
Serial.println("Iniciando");
myservo.Init();
delay(1000);
Scheduler.start(setup1,robot);
}
El primero de los bucles que se ejecuta es que de muestreo del sensor DMS. El sensor está conectado al pin
analógico ‘A0’ y el programa va leyendo los datos de éste en forma de voltaje, donde 0 equivale a 0V, es
decir, que no se detecta nada, y 1023 equivale a 5V, que sería el punto más cercano. Tal y como se especifica
en el datasheet del sensor, el valor máximo real es alrededor de 3V y el valor máximo se alcanza cuando el
objeto está a 10cm del sensor. Si está más próximo que este valor, el sensor no funciona bien y devuelve datos
que no se corresponden con la distancia real al objeto.
29
Programación
Para el control hemos decidido que si el valor detectado por el sensor es mayor de 500, el robot lo detecta
como un objeto demasiado cercano y ataca. Si el valor devuelto por el sensor está entre este valor y 250, quiere
decir que hay un objeto a una cierta distancia pero no lo detecta como amenaza, sino como un obstáculo. En
este caso, el robot comienza a girar para evitarlo.
void loop(void)
{
int value = analogRead(DMS);
Serial.println(value);
delay(500);
if(value>250 && value<500){
Serial.println("coming closer");
flag=1;
}
if(value>500){
Serial.println("too close!");
flag=2;
}
}
El segundo bucle es el que ejecuta las órdenes que se envían al robot. Tenemos una variable llamada “flag”
que generalmente está a cero, lo que permite que el robot avance con normalidad. Cuando se detecta un
obstáculo, se cambia el valor de esta variable a ‘1’ para que se ejecute la acción de giro y se evite. En el caso
de que se detecte el objeto como amenaza, la variable tomará el valor ‘2’ y ejecutará la instrucción de ataque.
Tras cada caso, se devuelve la variable a ‘0’ para que el robot siga avanzando.
void setup1()
{
}
void robot(void)
{
while(flag==0){
Serial.println("Moving");
myservo.Forward(1);
}
while(flag==1){
Serial.println("Turning");
myservo.Turn(5);
flag=0;
}
while(flag==2){
Serial.println("don't touch me!");
myservo.Attack();
delay(2000);
myservo.Base();
delay(7000);
flag=0;
}
delay(100);
}
30
7 CONCLUSIONES
En este trabajo hemos conseguido los siguientes objetivos:

 Sustitución de la placa original por otra desarrollada por nosotros constituida por una placa de
Arduino Mega2560 con un “shield” específico para los actuadores del robot, capaz de obtener las
mismas funcionalidades que la original e incluso superarlas, por ejemplo, en potencia y en número de
actuadores conectados.
 Implementación de un código capaz de simular el mismo movimiento del robot que el obtenido con el
controlador original del fabricante. Además este código es modificable y ampliable para obtener el
control deseado, sin las restricciones impuestas por el fabricante.
La principal dificultad que encontramos y que no nos permitió explotar todas las características y posibles
funcionalidades de los actuadores fue la complejidad para obtener una reprogramación del microchip
integrado en el shield. Sin la posibilidad de modificar el programa del microchip, no pudimos obtener una
comunicación bilateral con los actuadores ya que sólo era posible mandar datos pero no recibirlos, mermando
por tanto el potencial del robot.
Otro contratiempo fue la falta de documentación para obtener la traducción de las librerías para realizar el
control del robot desde el lenguaje de Matlab. Debido al poco desarrollo en la literatura sobre la interacción
entre Arduino y Matlab, el tiempo gastado buscando información fue muy grande para luego no obtener un
resultado significativo.
Como desarrollos futuros, y para seguir en la línea de la intención inicial de este trabajo, se propone continuar
con la investigación para obtener la reprogramación del microchip integrado en el shield elegido para ser
capaces de explotar todas las funcionalidades de los actuadores de Dynamixel.
También se podría intentar continuar con este proyecto usando un robot con una configuración más compleja
como podría ser un robot humanoide que requiera un control de estabilidad aparte del propio control de las
articulaciones.
31
REFERENCIAS
[1] Documentación SPI.: https://learn.sparkfun.com/tutorials/serial-peripheral-interface-spi
[2] User’s Manual Dynamixel AX-12 by ROBOTIS
[3] ROBOTIS site: http://support.robotis.com/
[4] DFROBOT site: https://www.dfrobot.com/
[5] Arduino site: https://www.arduino.cc/en/Reference/HomePage
[6] Tutoriales de Matlab: https://es.mathworks.com/help/supportpkg/arduinoio/index.html
32
ANEXO I
Slave_servo
#include <SPI.h>
#include "pins_arduino.h"
byte command = 0;
boolean Servomode;
boolean PositionLimit;
boolean Motormode;
boolean IDreset;
boolean ServoRst;
int Command[20];
int reclength = 0;
int Pos;
int ID,newID;
int velocity;
int PosLimit;
int upperLimit;
#define startByte 0xFF
#define P_MODEL_NUMBER_L 0
#define P_MODEL_NUMBER_H 1
#define P_VERSION 2
#define P_ID 3
#define P_BAUD_RATE 4
#define P_RETURN_DELAY_TIME 5
#define P_CW_ANGLE_LIMIT_L 6
#define P_CW_ANGLE_LIMIT_H 7
#define P_CCW_ANGLE_LIMIT_L 8
#define P_CCW_ANGLE_LIMIT_H 9
#define P_SYSTEM_DATA2 10
#define P_LIMIT_TEMPERATURE 11
#define P_DOWN_LIMIT_VOLTAGE 12
#define P_UP_LIMIT_VOLTAGE 13
#define P_MAX_TORQUE_L 14
#define P_MAX_TORQUE_H 15
#define P_RETURN_LEVEL 16
#define P_ALARM_LED 17
#define P_ALARM_SHUTDOWN 18
#define P_OPERATING_MODE 19
#define P_DOWN_CALIBRATION_L 20
#define P_DOWN_CALIBRATION_H 21
#define P_UP_CALIBRATION_L 22
#define P_UP_CALIBRATION_H 23
33
Anexo I
#define P_TORQUE_ENABLE (24)

#define P_LED (25)
#define P_CW_COMPLIANCE_MARGIN (26)
#define P_CCW_COMPLIANCE_MARGIN (27)
#define P_CW_COMPLIANCE_SLOPE (28)
#define P_CCW_COMPLIANCE_SLOPE (29)
#define P_GOAL_POSITION_L (30)
#define P_GOAL_POSITION_H (31)
#define P_GOAL_SPEED_L (32)
#define P_GOAL_SPEED_H (33)
#define P_TORQUE_LIMIT_L (34)
#define P_TORQUE_LIMIT_H (35)
#define P_PRESENT_POSITION_L (36)
#define P_PRESENT_POSITION_H (37)
#define P_PRESENT_SPEED_L (38)
#define P_PRESENT_SPEED_H (39)
#define P_PRESENT_LOAD_L (40)
#define P_PRESENT_LOAD_H (41)
#define P_PRESENT_VOLTAGE (42)
#define P_PRESENT_TEMPERATURE (43)
#define P_REGISTERED_INSTRUCTION (44)
#define P_PAUSE_TIME (45)
#define P_MOVING (46)
#define P_LOCK (47)
#define P_PUNCH_L (48)
#define P_PUNCH_H (49)
//— Instruction —
#define INST_PING 0x01
#define INST_READ 0x02
#define INST_WRITE 0x03
#define INST_REG_WRITE 0x04
#define INST_ACTION 0x05
#define INST_RESET 0x06
#define INST_DIGITAL_RESET 0x07
#define INST_SYSTEM_READ 0x0C
#define INST_SYSTEM_WRITE 0x0D
#define INST_SYNC_WRITE 0x83
#define INST_SYNC_REG_WRITE 0x84
void setup()
{
Serial.begin(1000000); // start serial port at 1000000 bps:
// Serial.begin(115200); // start serial port at 1000000 bps:
pinMode(MISO, OUTPUT);
SPCR |= _BV(SPE);
SPI.attachInterrupt();
Servomode=false;
34
Anexo I
Motormode=false;
IDreset=false;
ServoRst=false;
}
ISR (SPI_STC_vect)
{
byte c = SPDR;
switch (command)
{
case 0:
command = c;
case 'p':
reclength ++;
ID=Command[0];
Pos=Command[1]*256+Command[2];
velocity=map(velocity,0,300,0x0000,0x03FF);
Pos=map(Pos,0,300,0x0000,0x03FF);
Servomode=true;
reclength=0;
}
break;
case 's':
reclength ++;
ID=Command[0];
PosLimit=Command[1]*256+Command[2];
PosLimit=map(PosLimit,0,300,0x0000,0x03FF);
PositionLimit=true;
reclength=0;
}
break;
case 'm':
reclength ++;
if( Command[reclength-3]=='\t' && Command[reclength-2]=='\r' &&
ID=Command[0];
if(velocity>=0) velocity=map(velocity,0,300,0x0000,0x03FF);
else velocity=map(velocity,0,-300,0x0400,0x07FF);
35
Anexo I
// Serial.writeln(velocity,HEX);
Motormode=true;
reclength=0;
}
break;
case 'i':
reclength ++;
ID=Command[0];
newID=Command[1];
IDreset=true;
reclength=0;
}
break;
case 'r':
reclength ++;
ID=Command[0];
ServoRst=true;
reclength=0;
}
break;
default:
reclength=0;
break;
}
void loop()
{
if(Servomode){
WritePos(ID,Pos,velocity);
Servomode=false;
}
if(PositionLimit){
SetServoLimit(ID,PosLimit);
36
Anexo I
if(Motormode){
ServoRotate(ID,velocity);
Motormode=false;
}
if(IDreset){
SetID(ID,newID);
Motormode=false;
}
if(ServoRst){
Reset(ID);
ServoRst=false;
}
if (digitalRead (SS) == HIGH)

command = 0;
void WritePos(int ID,int Pos,int velocity){

byte pos2 = (Pos>>8 & 0xff);
byte pos = (Pos & 0xff);
byte vel2 = (velocity>>8 & 0xff);

byte vel = (velocity & 0xff);
Serial.write(startByte); // send some data

Serial.write(ID);
Serial.write(P_GOAL_POSITION_L);
Serial.write(pos);
Serial.write(pos2);
Serial.write(vel);
Serial.write(vel2);
Serial.write((~(ID + (messageLength) + INST_WRITE + P_GOAL_POSITION_L + pos
+ pos2 + vel + vel2))&0xFF);
}
void SetServoLimit(int ID, int PosLimit){

byte PosLimitB = (PosLimit>>8 & 0xff);
byte PosLimitS = (PosLimit& 0xff);
37
Anexo I

Serial.write(ID);
Serial.write(0x08);
Serial.write(PosLimitB);
Serial.write(PosLimitS);
Serial.write((~(ID + (messageLength) + INST_WRITE+ 0x08 + PosLimitB+
PosLimitS))&0xFF);
}
void ServoRotate(int ID, int velocity){

byte velocityB = (velocity>>8 & 0xff);
byte velocityS = (velocity & 0xff);
Serial.write(ID);
Serial.write(0x20);
Serial.write(velocityS);
Serial.write(velocityB);
Serial.write((~(ID + (messageLength) + INST_WRITE+ 0x20 + velocityB+
velocityS))&0xFF);
}
void SetID(int ID, int newID){

Serial.write(ID);
Serial.write(P_ID);
Serial.write(newID);
Serial.write((~(ID + (messageLength) + INST_WRITE+ P_ID + newID))&0xFF);
}
void Reset(int ID){

// renew ID:1 Baund:1M
Serial.write(ID);
Serial.write(0x02);
Serial.write(0x06);
Serial.write((~(ID + 0x02 +0x06))&0xFF);
}
38
ANEXO II
Tabla de contenido de los actuadores
39

Proyecto Del Robot Escorpion

Uploaded by

Document Information

Original Description:

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Proyecto Del Robot Escorpion

Uploaded by

Copyright:

Available Formats

Proyecto Fin de Carrera

Grado en Ingeniería de Tecnologías Industriales

Control de un robot escorpión con hardware Arduino

Autor: Fernando Carlos García García

Equation Chapter 1 Section 1

Dep. Sistemas y Automática

Control de un robot escorpión con hardware

Dep. Sistemas y Automática

Autor: Fernando Carlos García García

Tutor: José Ángel Acosta Rodríguez

Acuerdan otorgarle la calificación de:

Figura 2-1. Comunicación en paralelo. 3

1.2 Estructura de la memoria

2.1 Comunicación SPI

Figura 2-1. Comunicación en paralelo.

Figura 2-2. Comunicación en serie.

Figura 2-3. Cableado para la comunicación entre maestro y esclavo.

Figura 2-5. Esquema de la UART.

Figura 3-1. Robot escorpión de Bioloid Premium.

Figura 3-2. Actuador Dynamixel AX-12A.

Figura 3-3. Conexión en serie de los actuadores.

3.2.1 Sensor DMS

Figura 3-4. Sensor DMS de Sharp.

Figura 3-5. Conexión del sensor en nuestra placa.

Figura 3-6. Controlador CM-530.

Figura 3-7. Arduino Mega2560.

3.4 Comunicación entre los dispositivos

Figura 3-9. Pines dedicados a la conexión SPI de la placa Arduino Mega2560.

4.1 Arduino IDE y C++

Figura 4-1. Interfaz de RoboPlus Manager.

Figura 4-2. Interfaz de RoboPlus Motion.

int upperLimitB = (upperLimit_temp>>8 & 0xff);

A continuación se define la rutina de interrupción. El microcontrolador integrado en la placa de Arduino

byte vel2 = (velocity>>8 & 0xff);

6.1 Comunicación con los motores

Bytes de ID Longitud Instrucción Dirección Posición Velocidad Checksum

6.2 Reprogramación del shield

Interfaz UART para

Figura 6-1. Vista de planta del shield.

Figura 6-2. Vista de planta del FTDI.

Debido a la complejidad de la reprogramación y al aumento de la carga de trabajo, decidimos no seguir con

6.3 Ampliación de la librería ServoCds55

Serial.print(" a la posicion: ");

void ServoCds55::Backward(int rep)

void ServoCds55::Turn(int rep)

6.4 Programación de la placa

void setup (){

void loop (){

void controlServo(char val) {

En este trabajo hemos conseguido los siguientes objetivos:

[1] Documentación SPI.: https://learn.sparkfun.com/tutorials/serial-peripheral-interface-spi

[2] User’s Manual Dynamixel AX-12 by ROBOTIS

[3] ROBOTIS site: http://support.robotis.com/

[4] DFROBOT site: https://www.dfrobot.com/

[5] Arduino site: https://www.arduino.cc/en/Reference/HomePage

[6] Tutoriales de Matlab: https://es.mathworks.com/help/supportpkg/arduinoio/index.html

#define P_TORQUE_ENABLE (24)

if (digitalRead (SS) == HIGH)

void WritePos(int ID,int Pos,int velocity){

byte vel2 = (velocity>>8 & 0xff);

Serial.write(startByte); // send some data

void SetServoLimit(int ID, int PosLimit){

Serial.write(startByte); // send some data