Estudio y análisis de las comunicaciones en el robot Scorbot ER4u : desarrollo de un driver e implementación para su uso en interfaz web

Proyecto Fin de Carrera Estudio y análisis de las comunicaciones en el robot Scorbot-ER 4u. Desarrollo de un driver e implementación para su uso en interfaz web. Para la obtención del título de Ingeniero en Informática Titulación: Ingeniería en Informática Autor: Yeray Miranda Betancor Tutor: Pedro Medina Rodríguez Marzo, 2017 ii Proyecto Fin de Carrera Escuela de Ingeniería Informática Título del proyecto Estudio y análisis de las comunicaciones en el robot Scorbot-ER 4u. Desarrollo de un driver e implementación para su uso en interfaz web. Autor: Yeray Miranda Betancor Tutor: Pedro Medina Rodríguez iii iv Resumen Durante el proyecto se ha desarrollado un driver para controlar el robot Scorbot-ER 4u obteniendo la información relativa al control del mismo a través del estudio de los mensajes enviados en el puerto USB al controlador mediante un proceso de captura y decodificación. Una vez desarrollado el driver se han creado una serie de aplicaciones a modo de herramientas para demostrar su funcionamiento mediante diferentes pruebas y se ha creado además una interfaz web y un servidor que permiten su uso remoto. Estas pruebas han confirmado que el driver desarrollado consigue operar de forma correcta el robot. Abstract During this proyect we’ve developed a driver to control the Scorbot-ER 4u robot obtaining all the information about its control through the study of the messages sent through the USB port after capturing and decoding them. Once the driver has been developed we’ve also created a series of programs to serve as a toolset to test it and created a web interface and server that allow a remote use. These tests have confirmed that the driver is capable of correctly operating with the robot. v iii ÍNDICE 1. Introducción ................................................................................................................................. 1 1.1 La Robótica ............................................................................................................................... 4 1.2 El protocolo USB ....................................................................................................................... 7 1.3 Estado del arte ....................................................................................................................... 13 1.4 Objetivos ................................................................................................................................ 14 2. Metodología ............................................................................................................................... 15 2.1 Metodología de prototipado .................................................................................................. 15 2.2 Recursos necesarios ............................................................................................................... 17 2.3 Temporalización ..................................................................................................................... 18 3. Análisis ....................................................................................................................................... 20 3.1 Uso de la librería LibUsb ......................................................................................................... 21 3.2 Diagrama de conexión al dispositivo ..................................................................................... 22 3.3 Comunicación con LibUsb ...................................................................................................... 23 3.4 Estudio de los mensajes USB ................................................................................................. 30 3.5 Estudio de transferencia en estado ocioso ............................................................................ 30 3.6 Estudio de transferencia de datos, E/S digital ....................................................................... 33 Salida Digital ............................................................................................................................... 33 Entrada Digital ............................................................................................................................ 35 3.7 Estudio de transferencia de datos en la E/S analógica .......................................................... 38 Salida Analógica ......................................................................................................................... 38 Entrada Analógica ...................................................................................................................... 40 3.8 Estudio de movimiento de motores ....................................................................................... 43 Velocidad de movimiento .......................................................................................................... 52 3.9 Encontrando la posición Home .............................................................................................. 53 Los microinterruptores .............................................................................................................. 54 Desconexión al forzar motor ...................................................................................................... 57 Codificadores motores y Home ................................................................................................. 58 3.10 Dispositivos Externos ........................................................................................................... 61 Implementación del uso de una Cinta Transportadora ............................................................. 61 4. Diseño ........................................................................................................................................ 65 iv 5. Desarrollo ................................................................................................................................... 66 5.1 Funciones de conexión ........................................................................................................... 66 5.2 Funciones de Estado ............................................................................................................... 67 5.3 Funciones entrada / salida ..................................................................................................... 69 5.4 Funciones de movimiento ...................................................................................................... 71 5.5 Aplicaciones ............................................................................................................................ 73 Pequeña aplicación para uso simple del robot .......................................................................... 73 Intérprete de comandos ............................................................................................................ 73 Conector PHP / Intérprete ......................................................................................................... 84 Interfaz gráfica HTML5 + Javascript + JQuery UI ....................................................................... 86 El intérprete servidor ................................................................................................................. 89 5.6 Pruebas realizadas .................................................................................................................. 92 5.7 Trabajos futuros ................................................................................................................... 101 6. Conclusiones ............................................................................................................................ 102 7. Anexos ...................................................................................................................................... 103 7.1 Instalación ............................................................................................................................ 104 7.2 Contenido del CD .................................................................................................................. 107 7.3 Función ejemplo: abrePinza( ) .............................................................................................. 108 Funciones de la librería ................................................................................................................ 110 Comandos para el intérprete ....................................................................................................... 118 8. Bibliografía ............................................................................................................................... 121 v ÍNDICE DE FIGURAS Figura 1 - Controlador del Scorbot-ER 4u ............................................................................................ 1 Figura 2 - Codificador Incremental, Fuente: orientalmotor.com ........................................................ 1 Figura 3 - Brazo robot del Scorbot-ER 4u ............................................................................................. 2 Figura 4 - Robot Unimate trabajando con una pieza de metal salida de la forja. ............................... 4 Figura 5 - Fábrica de vehículos donde las soldaduras son realizadas por brazos robóticos. .............. 5 Figura 6 - Dos cirujanos utilizan terminales para trabajar juntos con el robot da Vinci. .................... 6 Figura 7 - Brazos del robot da Vinci. .................................................................................................... 6 Figura 8 - Conectores USB .................................................................................................................... 7 Figura 9 - Cable doble alimentacion .................................................................................................... 8 Figura 10 - Descriptor USB, Fuente: http://www.beyondlogic.org ................................................... 10 Figura 11 - Formatos mensaje usb ..................................................................................................... 11 Figura 12 - Pipes y endpoints, fuente: wikimedia commons. ............................................................ 12 Figura 13 - Controles e información en scorbase .............................................................................. 13 Figura 14 - Diagrama de creación de prototipos ............................................................................... 16 Figura 15 - DIVISION TEMPORAL DEL PROYECTO .............................................................................. 19 Figura 16 - CAPTURA DE DATOS USB ................................................................................................. 20 Figura 17 - DIAGRAMA CONEXIÓN ..................................................................................................... 22 Figura 18 - Tráfico USB en estado ocioso ........................................................................................... 25 Figura 19 - Estructura de un mensaje USB de salida ......................................................................... 26 Figura 20 - Mensaje de salida ............................................................................................................ 30 Figura 21 - Mensaje de entrada ......................................................................................................... 31 Figura 22 - Mensaje de salida 2 ......................................................................................................... 31 Figura 23 - Mensaje de entrada 2 ...................................................................................................... 32 Figura 24 - Programa de prueba para la salida 1 ............................................................................... 33 Figura 25 - Activación de la salida digital 1 ........................................................................................ 33 Figura 26 - Desactivación de la salida digital 1 .................................................................................. 34 Figura 27 - Programa de prueba para la salida 2 ............................................................................... 34 Figura 28 - Activación de la salida digital 5 ........................................................................................ 34 Figura 29 - Desactivación de la salida digital 5 .................................................................................. 34 Figura 30 - Prueba activación las entradas durante una captura de datos ....................................... 35 Figura 31 - Captura con entrada digital 1 activa ................................................................................ 36 Figura 32 - Captura con entrada digital 5 activa ................................................................................ 36 Figura 33 - Captura con entradas 1 y 5 activas .................................................................................. 36 Figura 34 - Salida analógica 1 a 0. ...................................................................................................... 39 Figura 35 - Salida analógica 1 a 127. .................................................................................................. 39 Figura 36 - Salida analógica 1 a 255. .................................................................................................. 39 Figura 37Entrada analógica 1 a 10 voltios. ...................................................................................... 40 Figura 38 - Valor de entrada indicado por Scorbase.......................................................................... 40 Figura 39 - Entrada analógica 1 a 5 voltios. ....................................................................................... 41 6 FIGURA 6 - DOS CIRUJANOS UTILIZAN TERMINALES PARA TRABAJAR JUNTOS CON EL ROBOT DA VINCI. FIGURA 7 - BRAZOS DEL ROBOT DA VINCI. 7 1.2 El protocolo USB Breve historia del protocolo USB El protocolo USB nació en 1994 aunque no fue lanzado al mercado hasta el año 1996 y fue diseñado con la intención de proporcionar un puerto que permitiese conectar diferentes dispositivos hardware a ordenadores utilizando una interfaz común. Su principal objetivo era el de ser un puerto universal que se convirtiera en un estándar entre los fabricantes y que a la vez fuera capaz de proporcionar no sólo una interfaz de comunicación sino poder suministrar también voltaje a los dispositivos conectados de manera que se eliminase la dependencia de los cargadores. En la siguiente tabla podemos ver las diferentes versiones del protocolo USB que se han implementado a lo largo de los años y su velocidad de transferencia. Versión Velocidades soportadas Fecha de lanzamiento 1.0 12 Mbps Enero 1996 1.1 1.5 , 12 Mbps Septiembre 1998 2.0 1.5, 12 , 480 Mbps Abril 2000 3.0 1.5, 12, 480 Mbps y 4.8 Gbps Agosto 2008 Como vemos existe una retro compatibilidad entre las diferentes versiones ya que cada una soporta el modo de velocidad ofrecido por la anterior. El estándar define no sólo el protocolo de comunicación sino también la fisonomía del puerto y de los cables. Actualmente existen 8 tipos de conectores. Fuente: www.l-com.com FIGURA 8 - CONECTORES USB 8 Limitaciones del protocolo USB El protocolo USB aparte de su velocidad (según la versión) tiene otras dos limitaciones: ● El número máximo de dispositivos que se pueden conectar al bus. ● La longitud del cable entre el dispositivo y el puerto. Actualmente se pueden conectar a un puerto USB hasta 127 dispositivos. Esto se debe a que en el protocolo USB la dirección del paquete está compuesta por 7 bits. En cuanto a la longitud del cable normalmente se establece una longitud máxima de 3 metros para dispositivos que utilizan el protocolo USB 1.0 y de 5 metros a partir de la versión USB 2.0 Las limitaciones en cuanto a la longitud del cable vienen impuestas por el propio diseño del protocolo y no por las características del cable, interferencias, etc. Se debe a que cuando se diseñó se llegó a la conclusión de que un dispositivo que iba a ser utilizado en un ordenador no iba a estar nunca más lejos de esos 3-5 metros. La limitación en la longitud del cable se lleva a cabo mediante el uso de un delay máximo en la interfaz y no puede ser solucionada utilizando cables de mejor calidad o más aislados sino utilizando hubs o extensores usb. Esto se debe a que el delay de la señal se controla entre cada segmento del cable USB y no sobre el total. El bus USB está limitado a 5 hubs por lo tanto utilizando cable de 5 metros en la práctica la longitud máxima para un cable USB será de 30 metros. Alimentación a través de USB El protocolo USB proporciona corriente a los dispositivos conectados de manera que éstos no necesiten de un cargador externo. Sin embargo esta corriente es de baja intensidad y por lo tanto no siempre se podrá alimentar cualquier tipo de dispositivo satisfactoriamente. Un ejemplo son los discos duros externos de gran capacidad que aún a día de hoy siguen necesitando un cargador externo para funcionar o cables especiales que permiten conectar más de un puerto USB al dispositivo para tomar corriente de dos puertos. FIGURA 9 - CABLE DOBLE ALIMENTACION 9 La corriente proporcionada por un Host USB viene definida por unidades de carga o unit loads y por cuántas de éstas es capaz de proporcionar. Con el paso del tiempo se ha ido incrementando la corriente proporcionada por el conector desde que apareciera el primer conector USB capaz de proporcionar 100mA de unidad de carga hasta la última versión USB 3.0 que ya es capaz de proporcionar una unidad de carga de 150mA. Además del valor máximo de una unidad de carga también se define cuántas de éstas se pueden obtener del host. Las primeras versiones del protocolo hasta la 2.0 podían consumir 5 unidades de carga del host, teniendo en cuenta que cada unidad proporciona 100mA podemos consumir hasta 500mA. de un mismo host. En cuanto a la versión 3.0 además de proporcionar una unidad de carga mayor, de 150mA, también es capaz de proporcionar hasta 6 de ellas por lo que la corriente máxima que nos proporciona un host USB 3.0 es de 900mA. Los dispositivos al conectar siempre requerirán una unidad de carga al estar en modo low-power siempre que se conecta un nuevo dispositivo y luego durante la configuración si son dispositivos que requieren más corriente, es decir, trabajan en modo high-power, el host les proporcionará hasta 500mA o 900mA en el caso de USB 3.0. En el caso del controlador del Scorbot se trata de un dispositivo auto alimentado al disponer de su propia fuente de alimentación y por lo tanto no hace uso del puerto USB para alimentarse sino únicamente para transmitir datos por lo que hará uso de una única unidad de carga. Configuración del dispositivo Los dispositivos USB conectados al bus deben configurarse antes de realizar cualquier tipo de transferencia de datos. El dispositivo dispone de una o más configuraciones de las que una es seleccionada al conectar. Esta configuración indicará requerimientos de corriente y ancho de banda, además cada configuración puede definir múltiples interfaces que a su vez definen una serie de endpoints. Todas las interfaces que proporciona una configuración están disponibles una vez establecida la misma. Una vez configurado y seleccionado el interface con el que el driver va a trabajar éste dispondrá de una serie de endpoints sobre los que realizar las transferencias. Los endpoints proporcionan acceso unidireccional a través de búfers con el dispositivo. Toda la información relativa a las configuraciones se encuentra en los descriptores a los que podemos acceder a través de peticiones de tipo Get Descriptor al dispositivo. 10 Descriptores  Descriptor de dispositivo: Todo dispositivo tiene un único device descriptor y éste almacena información como la versión del protocolo que utiliza, identificadores del producto y su fabricante, y el número de configuraciones de las que dispone.  Descriptor de configuración: Contiene el número de interfaces de la configuración, si se soporta o no la funcionalidad de suspensión y requerimientos de energía.  Descriptor de interfaz: Clase y subclase de la interfaz, donde se definen una serie de drivers no específicos que pueden ser utilizados por todos los dispositivos, por ejemplo un pen drive no requiere que el fabricante cree un driver ya que existe una clase que define el funcionamiento de todos los dispositivos de este tipo. Además de definir clases y subclases de control de dispositivos comunes también indicará el número de configuraciones alternativas para la interfaz y endpoints.  Descriptor de endpoint: El descriptor del endpoint indicará la dirección, sentido de transferencia y tipo del mismo. Además nos proporciona otros datos de interés como el tamaño máximo de paquete y la frecuencia para el polling si es un dispositivo que funciona mediante interrupciones.  Descriptor de string: Contiene información relativa al dispositivo en un formato legible. FIGURA 10 - DESCRIPTOR USB, FUENTE: HTTP://WWW.BEYONDLOGIC.ORG 11 Transferencia a través del bus USB Las transferencias a través del bus USB se realizan enviando y recibiendo paquetes que podemos agrupar en 4 categorías. 1. Token: Indican al dispositivo USB de que el host desea comunicarse para realizar transferencia de lectura, escritura o control. 2. Data: Hay dos tipos de paquetes de datos que permiten transferencias de hasta 1024 bytes. Los tamaños máximos de datos en estos mensajes es de 8 bytes para dispositivos de baja velocidad y 1023/1024 para full y high speed respectivamente. 3. Handshake: Paquetes simples envían únicamente un identificador. Existen tres tipos ACK para indicar que se ha recibido correctamente, NAK para indicar que no es posible el envío o recepción de datos y STALL para indicar que se requiere la intervención del Host. 4. Start of frame: Consiste en un número de 11 bits envíado por el host para sincronizar con los dispositivos. Se envía cada 1ms. Aunque en los dispositivos High speed se divide en 8 microframes de 125 uS. Estos 8 microframes tendrán el mismo número de frame que aumenta cada microsegundo. Los formatos de estos 4 tipos de mensajes son los siguientes: FIGURA 11 - FORMATOS MENSAJE USB Como vemos todos los mensajes tienen en común los campos de sincronización (Sync), el identificador del paquete (PID) y el fin del mismo (EOP). 12 ENDPOINTS Los endpoints actúan como búfers unidireccionales donde enviar y recibir paquetes que pueden ser de cualquiera de los 4 tipos vistos anteriormente. Estos paquetes se envían a través de dichos endpoints que pueden ser de entrada o salida y se mantienen en el búfer de manera que podamos leerlos en cuanto el software lo desee. Por ejemplo, si se envía un paquete al endpoint 4 de un dispositivo ya configurado se quedará en el búfer de entrada del endpoint 4 del mismo hasta que éste lo lea y a su vez nos responderá a través del endpoint 4 pero en el búfer de salida donde se almacenará hasta ser leído. PIPES Los pipes se dividen en dos tipos y representan una conexión lógica para la transferencia de datos a nivel de software que además tiene asociada la configuración actual que se ha establecido entre el host y un endpoint. Un pipe es en esencia un endpoint ya configurado.  Stream pipes que no tienen formato USB definido pueden enviar/leer datos de los mismos aunque con una dirección predefinida y de forma secuencial. Soportan modos de transferencia bulk, asíncrono e interrupt.  Message pipes tienen un formato definido y a diferencia de los stream pipes son controlados siempre por el host. Sólo soportan transferencias de tipo control. FIGURA 12 - PIPES Y ENDPOINTS, FUENTE: WIKIMEDIA COMMONS. 13 1.3 Estado del arte El robot Scorbot-ER 4U nos proporciona por parte del fabricante INTELLITEK el driver original para controlar la comunicación entre el computador y el controlador del robot, además nos proporciona el programa Scorbase para operar con el robot. El driver está definido por un archivo .inf que contiene la información referente al dispositivo. No se nos proporciona acceso a las funciones del robot desde entornos externos a Scorbase de manera que estamos limitados a este programa para operar con el robot. Scorbase nos proporciona 3 niveles de funcionamiento, las funciones aparecen agrupadas por niveles complejidad. También se nos permite el movimiento de cualquiera de los ejes a través de unos controles sencillos mediante una paleta de control digital. FIGURA 13 - CONTROLES E INFORMACIÓN EN SCORBASE Finalmente, el aspecto más importante y por el cual se ha desarrollado este driver es que no podemos acceder a los motores del robot directamente, sino a través de las funciones Scorbase y por lo tanto nos limita a la hora de trabajar con el robot de manera directa o desarrollando cinemáticas propias (cambio del actuador, de algún elemento del brazo) si cambiáramos algún parámetro físico del robot. 14 1.4 Objetivos Los objetivos del proyecto se han dividido en varias etapas: ETAPA 1: Estudio y análisis detallado del protocolo del bus USB. 1. Estudio de la arquitectura del bus USB. 2. Evaluación y selección de herramientas para el análisis USB. 3. Instalación y puesta a punto de las herramientas, aprendizaje de las mismas y análisis de la información de interés en cada tipo de transferencia. ETAPA 2: Análisis de las comunicaciones entre el robot y el ordenador. 4. Análisis de flujo de información entre el dispositivo robot y el ordenador con el fin de averiguar la estructura que presentan las transferencias. 5. Identificación de los diferentes tipos de comandos, primitivas de control y/o respuestas por parte del robot. Verificación de funcionamiento. 6. Desarrollo, depuración y puesta a punto de una librería lo suficientemente completa para poder operar con el robot. 7. Ensayo, depuración y verificación del correcto funcionamiento de las comunicaciones utilizando un driver alternativo al del fabricante. ETAPA 3: Aplicaciones 8. En base a los resultados anteriores diseñar e implementar un interfaz gráfico que proporcione al usuario desde un ordenador la posibilidad de programar el robot y acceder a las diferentes funcionalidades del nuevo controlador. 15 2. Metodología La metodología empleada ha sido la metodología de prototipado debido a la facilidad de dividir en funciones pequeñas nuestro proyecto y probar por separado los diferentes componentes que forman el software que vamos a desarrollar. Además nos proporcionaba una manera sencilla de saber en una fase todavía inicial del proyecto, si el mismo era factible, y por lo tanto se podía continuar con su desarrollo. Esto se puede ver en el ejemplo de la primera función desarrollada, nos ha permitido verificar que podíamos conectarnos, luego se ha ampliado a mantener la conexión abierta y finalmente en una última iteración incluso se ha encargado del movimiento en algunos casos. 2.1 Metodología de prototipado La metodología empleada ha sido el prototipado debido a las siguientes características que presenta el proyecto:  Múltiples funciones pequeñas que se pueden probar por separado.  Independencia entre funciones, exceptuando la de conexión, permite probar de forma separada.  Al tratarse de un driver no tenemos la posibilidad de simplemente fijar unos requisitos de usuario respecto al sistema que vamos a desarrollar más allá de permitir el uso del dispositivo. Por lo tanto, una vez establecido un diseño global para todo el proyecto, seguiremos el mismo tras la fase de análisis con el objetivo de mantener la coherencia entre funciones, seguir unas pautas lógicas y de programación similares y por lo tanto lograr una integración mejor al preparar el sistema final. Las fases a seguir para el desarrollo de cada función serán:  Análisis de especificaciones del prototipo  Diseño del prototipo  Desarrollo del prototipo  Refinamiento/optimización del prototipo desarrollado  Verificación de funcionamiento para el prototipo Cuando una función pase todas estas fases la consideraremos terminada y podrá ser añadida a la librería que compone el driver. El siguiente diagrama de creación de prototipos muestra las fases y como se desarrolla el proceso iterativo que nos permite ir mejorando el prototipo hasta obtener un producto que satisfaga los requisitos definidos por nuestro análisis. 22 3.2 Diagrama de conexión al dispositivo FIGURA 17 - DIAGRAMA CONEXIÓN Podemos apreciar como durante los dos últimos pasos nos aseguramos de que configuramos y tomamos correctamente el control de la interfaz. No debemos confundir estas funciones de conexión de LibUSB con las funciones de conexión al controlador propiamente dicho. Estas nos proporcionan un handle al dispositivo a bajo nivel y nuestra librería luego lo utilizará para comunicarse y efectivamente enviar los comandos tanto de conexión como de sincronización, así como los utilizados para dar órdenes al controlador. 23 3.3 Comunicación con LibUsb ‘LibUsb’ nos proporciona varias funciones para una vez establecida la conexión con el dispositivo poder realizar transferencias de datos. Las funciones para el envío y recepción de mensajes se distinguen por el modo de transferencia: Interrupt o Bulk. Vamos a repasar sus características brevemente. Modo Bulk Las transferencias en modo bulk son utilizadas cuando los datos que se van a transmitir son intensivos y la latencia no es un factor determinante. Un ejemplo de transferencia Bulk es la transferencia de archivos a una memoria USB. Se trata de un modo de transferencia que no garantiza la latencia aunque sí la transferencia de los datos a través de la detección de errores y el reenvío que garantiza la entrega. La detección de errores se lleva a cabo a través de la inclusión de un campo CRC16. El algoritmo de detección de errores mediante redundancia cíclica tendrá por lo tanto un polinomio de 17 bits. Concretamente en las transferencia USB se utiliza el protocolo CRC-16-IBM. Este tipo de transferencia está limitada a dispositivos de velocidades full-speed y high-speed pudiendo enviar paquetes de datos con un tamaño máximo de 64 y 512 bytes respectivamente. Modo Interrupt A diferencia de otros dispositivos donde son estos los que generan la interrupción, en el protocolo USB este tipo de transferencia requiere que el host realice un poll sobre el dispositivo y que el mismo indique que va a transmitir datos. Este tipo de transmisión es la utilizada por dispositivos como teclados y ratones y la comunicación se produce a través de un pipe unidireccional de manera periódica. El tamaño máximo de un paquete varía según la velocidad del dispositivo conectado y son las siguientes: 8 bytes para un dispositivo low-speed, 64 para un dispositivo full-speed y 1024 para un dispositivo high-speed. 24 Funciones proporcionadas por libUSB A continuación repasamos las funciones que nos proporciona libUSB a la hora de realizar estos dos tipos de transferencia. Las dos funciones disponibles nos proporcionan la capacidad de realizar lecturas y escrituras en cada uno de los modos y todas requieren los mismos parámetros. Modo Bulk int usb_bulk_write(usb_dev_handle *dev, int ep, char *bytes, int size, int timeout); int usb_bulk_read(usb_dev_handle *dev, int ep, char *bytes, int size, int timeout); Modo interrupt int usb_interrupt_write(usb_dev_handle *dev, int ep, char *bytes, int size, int timeout); int usb_interrupt_read(usb_dev_handle *dev, int ep, char *bytes, int size, int timeout); Los parámetros para llamar a estas funciones son los siguientes:  usb_dev_handle *dev: puntero al dispositivo.  int ep: endpoint al que va destinado la transferencia.  char *bytes: vector de bytes que se van a escribir o sobre los que volcar una lectura.  int size: número de bytes a enviar/leer.  int timeout: timeout en milisegundos de la operación. El valor de retorno es un entero con el número de bytes leídos en caso de éxito y < 0 en caso de error. En el caso de nuestra librería, para la comunicación con el controlador del Scorbot vamos a utilizar el modo bulk. Las variables y parámetros que se utilizarán serán: char temp[64]={0}; int bytesread = usb_bulk_read(handle, 0x83, temp, 64, 1000); Donde el parámetro temp está definido como un vector de 64 bytes inicializados cero y bytesread es nuestra variable con el valor de retorno de la indicando bytes leídos. 25 Conexión y sincronización con el controlador El primer paso antes de empezar a desarrollar las funciones que van a componer nuestra librería para el robot Scorbot-ER 4u es conectarnos al mismo. Para ello vamos a capturar la secuencia de comandos que envía Scorbase para conectarse al robot, en este caso no nos importa analizar exhaustivamente qué significa cada mensaje de los que se envían siempre y cuando podamos conectarnos y mantener la conexión abierta. Una captura durante el proceso de conexión nos indica que para conectarse al robot Scorbase intercambia con el mismo una serie de mensajes. Por lo tanto y ya que el análisis de los mismos no nos va a ayudar a la hora de desarrollar nuestras funciones vamos a simplemente reproducirlos al conectar nosotros con el fin de que el controlador logre conectar con nuestro programa satisfactoriamente. Una vez enviada la misma secuencia que envía Scorbase hemos conseguido conectar con el controlador, sin embargo nos encontramos con un problema ya que cada vez que establecemos la conexión y estamos en estado ocioso esperando para dar alguna orden el controlador se desconecta. Este error también nos lo comunica a través del cambio de color del led de estado que pasa de verde a naranja. Aunque Scorbase esté en estado ocioso siempre existe un intercambio de mensajes: FIGURA 18 - TRÁFICO USB EN ESTADO OCIOSO Vimos como antes se capturaron una serie de transmisiones tras hacer Home y con todas las entradas/salidas desactivadas para evitar que otros datos ocupasen el búfer haciéndonos más complicada la tarea de descubrir cómo se sincronizaban Scorbase y el robot. 26 Vimos que los mensajes que enviamos en estado ocioso tenían la siguiente estructura: FIGURA 19 - ESTRUCTURA DE UN MENSAJE USB DE SALIDA Se ha podido imitar el comportamiento de estos mensajes para no perder la conexión con el controlador creando un array de 64 bytes con la misma estructura que se ha utilizado como mensaje de salida al controlador. Tras realizar múltiples lecturas y estudiar anteriormente su estructura llegamos a la conclusión de que lo único que ocurre es que se aumenta en +1 el valor del primer byte con cada mensaje hasta el valor 0xFF donde se empieza otra vez de cero. Ahora se nos presenta otro problema porque la entrada/salida con el controlador es bloqueante y no nos permite comunicarnos con el controlador de manera continua (necesario a la hora de enseñarle las posiciones) o esperar una entrada del usuario para un movimiento discreto ya que durante la lectura de la misma se perdería la conexión. La solución es una vez establecida la conexión crear un hilo independiente que se encargue de estos mensajes de sincronización evitando así el bloqueo. Este hilo se encargará de ir aumentando el byte que sincroniza con el controlador y del movimiento continuo como veremos posteriormente. Por ejemplo, en el caso de que queramos activar una salida digital la función debe comportarse de la siguiente manera: Manipular el búfer en el hilo de sincronización Se envía el mensaje Devolver el búfer al estado ocioso Esta solución ha sido probada y funciona satisfactoriamente manteniendo abierta la conexión y sin perder el control del robot. 27 El hilo de sincronización Utilizando Windows el hilo se ha creado como una función y se ha lanzado como un hilo independiente utilizando la función createThread de la que disponemos en la librería windows.h. HANDLE hThread; printf("Lanzando HILO..."); DWORD a; hThread=CreateThread(NULL,0,idleThread,handle,0,&a); El tercer parámetro contiene el nombre de nuestra función, idleThread, de la cual detallaremos el funcionamiento a continuación. Vamos a dividir la función en dos partes para así ver cómo se encargará de tanto del problema de la sincronización con el controlador como del movimiento continuo para alguno de los ejes. Nuestra primera necesidad y más importante que debe cubrir esta función es la de no perder la sincronización y por lo tanto el control del robot. Este primer objetivo tiene fácil solución, como veremos a continuación lo único que exige el controlador para mantener la conexión abierta es que enviemos los mensajes con “identificadores” en el primer byte que sean secuenciales lo que haremos será aumentar el valor del mismo tras cada escritura. En cuanto al desbordamiento del byte al llegar al valor 0xFF se ha observado que vuelve a empezar de cero y no tiene ningún efecto sobre ningún otro parámetro. Como vemos en el código de la misma es una función muy básica pero sin la cual la conexión al controlador se perdería continuamente. DWORD WINAPI idleThread(LPVOID lpParam){ array[4]=0x0d; while(1){ usb_bulk_write((struct usb_dev_handle*)lpParam,0x02,array,64,1000); array[0]=array[0]+1; Sleep(Retardo); usb_bulk_read((struct usb_dev_handle*)lpParam, 0x83, temp, 64, 1000); } }; Esta función aun así no soluciona todos los problemas ya que todavía no se encarga de mantener la conexión abierta durante el envío de comandos. Sin embargo lo que sí es capaz de hacer es mantener la conexión abierta y ya no nos desconectamos del controlador. 28 Movimiento continuo de los ejes El movimiento a través de un mando, interfaz web o programa es algo que todo robot debe proporcionar al usuario ya que es la única manera eficiente de enseñar posiciones nuevas a un robot. Podríamos ir dando pequeños incrementos en los ejes y llegar a una posición final deseada pero esto sería poco práctico y de ahí que sea necesario el poder controlar el movimiento de los ejes de forma directa. Al realizar un movimiento, por ejemplo, de 100 pasos en el eje 1 siempre sabemos de qué posición vamos a partir y a cual tenemos que llegar. Conocemos los valores de inicio y finalización, lo que no ocurre con los movimientos continuos donde únicamente conocemos la posición de partida. Se ha decidido incluir el control directo de los ejes en la función del hilo independiente ya que siempre está activa para mantener la conexión abierta y por lo tanto nos permite actualizar el estado del movimiento con cada mensaje que se envía. Sin entrar en los detalles del movimiento, que veremos más adelante, podemos ver la parte donde se controla este movimiento y que hace uso de dos variables globales para determinar si el usuario ha iniciado un movimiento continuo de un eje y en qué dirección. A continuación vamos a ver las partes más relevantes de la función: if(movContinuo!=0){ codMotor = 12 + (movContinuo-1)*4; ... if(movContinuoDir > 0){ posSalida += vel[velFactor]; if(posSalida>65535){ posSalida -= 65535; array[codMotor+2]=0; array[codMotor+3]=0; } }else{ posSalida -= vel[velFactor]; if(posSalida<0){ posSalida += 65535; array[codMotor+2]=0xff; array[codMotor+3]=0xff; } } ... 29 Aquí se verá si la variable global movContinuo está activa y por lo tanto se debe mover el robot en este modo, además de ser así también se calcula el código del motor que se mueve y la dirección. Una vez conocida tanto la dirección como el motor que se va a mover (en ocasiones dos como en el caso de roll e inclinación de la pinza) podemos enviar estos parámetros a través del búfer de salida al dispositivo: //Colocamos nuestros valores de salida en el búfer. hexa = (char*)&posSalida; array[codMotor]=hexa[0]; array[codMotor+1]=hexa[1]; if(movContinuo > 3){ hexa2 = (char*)&posSalida2; array[codMotor+4]=hexa2[0]; array[codMotor+5]=hexa2[1]; } En el siguiente ciclo de escritura al dispositivo el búfer ya contendrá los nuevos valores, produciendo el movimiento. Debido a que esta función es la que también mantiene la conexión abierta no hay necesidad de enviar los valores explícitamente una vez se encuentran en el búfer ya que esta función nunca deja de comunicarse con el controlador. 30 3.4 Estudio de los mensajes USB Los mensajes USB que vamos a analizar tienen un campo de datos de 64 bytes, como nos indicó el descriptor del dispositivo y por lo tanto es en estos bytes donde tendremos que encontrar tanto los diferentes códigos de operación como los parámetros. Para empezar a comprender cómo se organiza el contenido de estos mensajes y descifrar sus campos vamos a comenzar con el tipo de transferencia más simple posible realizando capturas de datos de la comunicación entre controlador y PC cuando el robot está conectado y se ha tomado control del mismo mediante la función control-on. En esta primera captura todavía no se habrá hecho Home. A partir de ahora nos referiremos a los mensajes que van del PC al controlador como mensajes de salida y a los que provienen del controlador como mensajes de entrada. 3.5 Estudio de transferencia en estado ocioso Una vez inicializado Scorbase y nuestra aplicación de captura USB vamos a ordenar a Scorbase tomar el control del robot y permanecer en estado ocioso. Si todo ha ocurrido normalmente se iluminarán en verde los dos leds de estado y conexión del controlador. Realizamos una serie de capturas y extraemos el siguiente mensaje: FIGURA 20 - MENSAJE DE SALIDA Este mensaje de salida se repite de forma ininterrumpida durante toda la conexión con el controlador. Se puede apreciar como prácticamente todo el mensaje está compuesto por ceros y únicamente encontramos dos bytes con un valor diferente en el primer y quinto byte del mismo. Múltiples capturas han confirmado que éste es el mensaje de salida que empieza a enviar Scorbase una vez conectado y en estado ocioso. Se han estudiado y analizado multitud mensajes y la conclusión es que se trata de algún tipo de mensaje de sincronización que simplemente mantiene la conexión abierta ya que el primer byte de la secuencia de datos aumenta con cada respuesta enviada por el controlador. Esta sincronización sólo se da cuando hemos tomado el control del robot y por lo tanto el no enviar estos mensajes supone la pérdida del control y el tener que realizar un nuevo control-on. 31 La siguiente parte del mensaje que nos interesa es donde encontramos el byte 0x0D que se mantiene sin cambios durante toda la conexión. A pesar de no saber su finalidad todavía se ha apreciado que siempre se da el mismo byte en dicha posición durante el estado ocioso del robot. Hemos visto que el mensaje de salida no varía mucho en estado ocioso, a excepción del primer byte, vamos a analizar ahora un mensaje de entrada: FIGURA 21 - MENSAJE DE ENTRADA En este mensaje ocurre lo mismo con el primer byte, incrementa con cada respuesta al igual que pasaba con el primer byte del mensaje de salida, sin embargo, en este caso también ocurre con el sexagésimo primer byte del mensaje (0xE3). El resto de bytes no varían y su contenido de momento nos supone una incógnita. Como vemos los primeros bytes tanto de salida como de entrada indican una secuencia numérica que se incrementa de uno en uno. Además podemos apreciar que este mismo byte también se encuentra al final de la secuencia de datos en las respuestas. Aparte de estos números tenemos una secuencia de bytes que parece ser la misma en todos los mensajes. Al no estar haciendo nada con el robot podemos suponer que es su funcionamiento normal. ¿ Cambiarán estos mensajes al hacer el Home ? Esta es la única manera que tenemos ahora mismo de comparar dos estados ociosos del robot sin intentar tocar nada más que pudiera complicarnos más descifrar los mensajes. Haremos el Home del robot llamando a su correspondiente función en Scorbase y volveremos a realizar una captura de los datos para comparar con el estado ocioso preHome que ya estudiamos. Las capturas obtenidas han sido las siguientes: FIGURA 22 - MENSAJE DE SALIDA 2 Se puede ver que el mensaje de salida sigue conteniendo el valor 0x0D en el quinto byte y al igual que ocurría antes de hacer HOME sigue enviando la secuencia numérica en el primer byte. Esto nos supondrá una ventaja a la hora de empezar a trabajar con el robot y mantener una conexión al mismo debido a la simplicidad de estos mensajes, eso sí, no debemos olvidar que se trata de mantener abierta una conexión con el robot en estado ocioso por lo que aun siendo una parte importante para poder empezar a programar un controlador no es más que el principio y mantener una conexión abierta es lo mínimo que se puede pedir al driver. 38 3.7 Estudio de transferencia de datos en la E/S analógica Al igual que hicimos para la entrada/salida digital se va a analizar ahora cómo se comporta la entrada/salida analógica tanto a la hora de realizar lecturas en cualquiera de sus 4 entradas como de asignar un voltaje a sus 2 puertos de salida. Salida Analógica El primer análisis lo vamos a hacer para el voltaje de salida ya que este siempre lo podremos asignar nosotros a través de Scorbase lo que nos permitirá realizar dos lecturas en los valores extremos (0-10 v.) y otra en un valor intermedio (5 v.) para intentar identificar el código de operación y el parámetro que envía Scorbase al controlador para que éste establezca la tensión de salida. Realizamos una primera lectura tras asignar una salida de 0 a la salida analógica 1, las salidas analógicas ya se encuentran a cero al empezar a trabajar con el robot y no deberían haber cambiado pero nos interesa saber si el código de operación siempre es el mismo y encontrar el byte del parámetro por lo que haremos también esta primera lectura. Posteriormente activaremos la salida con el valor 127 y finalmente tras asignar una salida de 255. Scorbase nos proporciona el comando y la posibilidad de elegir la salida a activar, por lo que hemos creado un pequeño programa con el fin de analizar los datos en el búfer para los diferentes valores de salida. Se encargará de ir cambiando el valor de la salida desde 0 hasta 255 pasando por el valor intermedio 127 con el fin de encontrar los mismos en el búfer al analizar el tráfico. 39 Los datos capturados y filtrados han sido los siguientes: FIGURA 34 - SALIDA ANALÓGICA 1 A 0. FIGURA 35 - SALIDA ANALÓGICA 1 A 127. FIGURA 36 - SALIDA ANALÓGICA 1 A 255. En estos mensajes y tras verificar su repetitividad ejecutando el programa en modo cíclico podemos ver como hicimos antes que para la E/S digital el código de operación se encuentra en el mismo byte, el sexto, y que el código de operación es el 0x41. Es interesante apuntar que de momento parece que todos los códigos de operación se transmiten en el quinto byte. En cuanto a dónde envía Scorbase el parámetro si nos fijamos en las operaciones vemos que el único byte que varía al enviar el código de operación es el séptimo. Repasando los valores para donde creemos se encuentra codificada la salida vemos que para el valor de 0 es efectivamente 0x00, para el de 127 es 0x7F (127 en hexadecimal) y en el último para 255 es 0xFF (255 en hexadecimal) por lo tanto podemos afirmar que es este el byte que identifica el parámetro. Podemos resumir el comando de salida analógica como un mensaje con el código de operación 0x41 y con un parámetro entre 0 y 255 que se envía en el sexto byte del mensaje. 40 Entrada Analógica Para las entradas analógicas vamos a realizar una serie de pruebas en las que conectamos un voltaje a cada una de las mismas con el fin de identificar en qué parte del estado del búfer del controlador aparecen. La primera prueba consistirá en conectar una fuente a 10 voltios a la entrada 1 con el fin de identificar dónde se encuentra el parámetro en el búfer a través de uno de sus valores límite, ya que el cero aparece muchas veces. Vamos a analizar el contenido del búfer y la información que nos proporciona Scorbase: FIGURA 37ENTRADA ANALÓGICA 1 A 10 VOLTIOS. FIGURA 38 - VALOR DE ENTRADA INDICADO POR SCORBASE. Una vez realizadas las lecturas vemos que para la entrada 1 parece que varía el byte 7 del búfer. Se ha confirmado realizando la prueba conectando y desconectando la entrada mientras se actualiza el búfer. Ahora vamos a bajar la tensión a 5 voltios y volveremos a realizar una lectura para ver si este valor cambia y lo hace indicando el valor de entrada que podríamos esperar para una entrada de 5 voltios. FOTO 1 - CONEXIÓN AL PUERTO 1 DE LA ENTRADA ANALÓGICA Y FUENTE PROBANDO VALOR EN EL EXTREMO DE LA ENTRADA ANALÓGICA. 41 FIGURA 39 - ENTRADA ANALÓGICA 1 A 5 VOLTIOS. FIGURA 40 - VALOR DE ENTRADA INDICADO POR SCORBASE. Vemos que de nuevo coincide el valor del búfer con el que nos indica Scorbase por lo que el byte 7 del búfer indica sin lugar a dudas el valor de entrada analógica 1. Realizando los mismos pasos para las demás entradas vamos a ver si el controlador nos indica el número de la entrada a través de algún otro parámetro o valor o de qué otra manera codifica los valores para las demás entradas analógicas con el fin de distinguirlas. Para ello vamos a realizar una captura con la entrada de 10 voltios en las 3 que faltan por analizar. FIGURA 41 - ENTRADA 2 CONECTADA A 10 VOLTIOS. FIGURA 42 - ENTRADA 3 CONECTADA A 10 VOLTIOS. FIGURA 43 - ENTRADA 4 CONECTADA A 10 VOLTIOS. 42 Viendo como varía el búfer de entrada podemos extraer la conclusión de que las entradas se mapean a los siguientes bytes del búfer de estado: Posición del byte Entrada analógica 7 1 8 2 9 3 10 4 Hemos descifrado por lo tanto cómo realizar lecturas de los puertos de entrada analógicos del controlador y obtener el valor hexadecimal que representa la tensión de entrada. Resumen Hemos iniciado el estudio del Robot Scorbot-ER 4u centrándonos en el controlador y sus diferentes funcionalidades. Una vez estudiado el búfer y los códigos de operación enviados por Scorbase al controlador sabemos cómo interactuar con el mismo y podremos desarrollar las funciones necesarias para nuestro driver. Ya podemos dar por finalizado el estudio de los mensajes relacionados con el controlador y empezar a estudiar todo lo relacionado con el brazo robótico. 43 3.8 Estudio de movimiento de motores Vamos a estudiar ahora cómo se mueven los motores del brazo robótico y para ello vamos a comenzar por centrarnos en movimientos que hagan uso de un único motor, en este caso el de la cintura del brazo robótico. Se ha elegido la cintura ya que al mover el resto de la estructura los movimientos pequeños son más fácilmente perceptibles a simple vista y además tiene el rango de movimiento más amplio de todos los ejes, sin tener en cuenta claro el giro de la muñeca, aunque ésta se ha descartado a la hora de realizar este estudio debido a que su movimiento viene definido por dos motores lo que complicaría en este momento inicial el análisis de los mensajes. Parece lógico que todos los motores vayan a tener un comportamiento similar y una vez estudiado el comportamiento de uno se verá si las conclusiones son extrapolables a todos los demás. Para estudiar el movimiento de este motor se va a utilizar el control directo de los ejes en Scorbase que se nos proporciona a través de la ventana de movimiento manual. FIGURA 44 - VENTANAS DE MOVIMIENTO MANUAL DEL ROBOT. 44 Vamos a realizar pequeños movimientos en las dos direcciones y hasta los extremos. Al igual que antes se realizarán múltiples capturas y una vez identificados los mensajes que nos interesan se procederá descifrar que hacen y si son consistentes durante varias pruebas. Tomaremos una primera captura tras hacer Home y nos moveremos hacia valores positivos del codificador. Se ha decidido realizar el primer movimiento hacia valores positivos ya que estos deberían ser más fáciles de analizar en los bytes que componen el estado del robot. Realizando un movimiento pequeño vemos que el estado del robot cambia y filtramos los mensajes enviados al controlador para descifrarlos y descubrir si se repiten en posteriores movimientos. Se ha tomado una se las capturas realizadas como ejemplo una vez confirmado que el patrón que siguen todas las realizadas era idéntico. CAPTURA 1 - INICIO MOVIMIENTO. En el momento de iniciar el movimiento vemos que se envía un código diferente a 0x0D que hemos visto que es el enviado en estado ocioso. Se envía el código 0x47 y a partir de ese momento vemos como en la captura se empiezan a producir aumentos en el byte número 13. 45 Analicemos ahora qué ocurre durante el transcurso del movimiento: CAPTURA 2 - MOVIMIENTO MOTOR Durante el transcurso del movimiento vemos que sigue aumentando el valor que se encuentra en el byte 13, pero vemos que al aumentar pasado el valor 0xFF sigue aumentando pero en este caso usando el byte 14 como el más significativo. Aparte del aumento de ese valor no ocurre nada más que pudiera resultarnos de interés, no se envían más códigos de operación durante el movimiento. 46 Finalmente veamos que ocurre al finalizar el movimiento: CAPTURA 3 - FIN MOVIMIENTO. En el momento en que finaliza el movimiento vemos que sí se produce un envío de códigos de operación diferentes al que se envía normalmente (0x0D). Además podemos apreciar como el valor que aumentaba anteriormente ya ha parado. Se ha verificado realizando pequeños movimientos consecutivos que los comandos que paran el movimiento son los mismos siempre. Ahora vamos a hacer la misma prueba para valores negativos devolviendo el robot a su posición de Home y partiendo de la misma pero esta vez hacia posiciones negativas. Esto nos servirá para saber si hay grandes diferencias a la hora de realizar movimientos en direcciones opuestas ya sea bien al iniciar, durante, o al finalizar un movimiento. Como antes partiremos de la posición de Home por lo que hemos visto en los mensajes analizados para movimientos positivos, el movimiento debería empezar con un código de operación 0x47 siempre y cuando los movimientos hacia valores negativos se comporten de igual manera que los realizados a valores positivos y no hagan uso de un código diferente. 47 CAPTURA 4 – INICIO MOVIMIENTO NEGATIVO. Efectivamente, podemos apreciar como los movimientos negativos también utilizan el código de operación 0x47. Sin embargo si ocurre algo más curioso con los valores del codificador, ahora en lugar de 2 bytes vemos que el movimiento parece ocupar ahora 4 bytes siendo los valores de los dos últimos 0xFF. Una vez realizadas varias capturas y estudiados los valores de las mismas se ha llegado a la conclusión de que los dos últimos bytes indican el signo del encoder, en este caso dos bytes con valor 0x00 para números positivos y 0xFF para negativos. Durante la parte intermedia del movimiento no debería ocurrir nada más que el cambio en el valor de los bytes 13 y 14, ya que los que indican, al menos de momento por lo que sabemos hasta ahora el signo, cambiaron al pasar de 0 a valores negativos. 54 FOTO 5 - MICRINTERRUPTOR CORRESPONDIENTE AL ROLL DE LA PINZA. Sin embargo encontrar la posición de Home simplemente llegando a los microinterruptores no es una tarea simple ya que en ocasiones como el encendido del robot donde todos los contadores motores están a cero no sabemos a ciencia cierta hacia qué dirección debe rotar el eje para encontrar el interruptor. Otro problema que ha surgido durante este proceso es el hecho de que cuando intentamos mover un eje más allá de lo que permite el mismo nos encontramos con que se produce una pérdida de conexión con el robot. Vamos a realizar un estudio de los mensajes transmitidos por el controlador con el fin de entender cómo detectar el estado de los microinterruptores o una colisión. Los microinterruptores El primer paso será averiguar cómo comunica el controlador a Scorbase que se ha pulsado un micro interruptor. Para ello se llevará a cabo una captura continua en estado ocioso y se intentará extraer de los mensajes capturados los bytes y el formato utilizados para comunicar el estado de los microinterruptores a Scorbase. Durante este proceso se va a iniciar una captura para posteriormente ir pulsando uno a uno cada uno de los 5 microinterruptores del robot con la ayuda de un destornillador. FOTO 6 - MICROINTERRUPTOR. 55 La primera captura se realiza en estado ocioso y utilizamos los controles de Scorbase para colocar el robot en una posición donde no esté pulsando ninguno de los interruptores. Ahora vamos activando los 5 microinterruptores uno a uno durante unos segundos hasta haberlos activado todos y una vez filtrados los mensajes veremos que podemos extraer de los mismos. FIGURA 49 - ESTADOS DEL CONTROLADOR MIENTRAS SE PULSABAN LOS DIFERENTES MICROINTERRUPTORES. 56 Una vez estudiados los mensajes capturados apreciamos que el sexto byte del estado es el único que varía si no contamos los de sincronización. Como fuimos tocando los interruptores por el orden indicado podemos saber qué valor en el mensaje corresponde con cada interruptor. Micro interruptor Valor byte hexadecimal Valor byte decimal 1 0x01 1 2 0x02 2 3 0x04 4 4 0x08 8 5 0x10 16 Estos valores se codifican al igual que las entradas digitales donde se interpretaban las 8 entradas como los bits individuales de un byte. En este caso nos encontramos con el mismo tipo de codificación. Aunque no sabemos cómo interpreta Scorbase internamente estos valores no nos afecta para nada a la hora de utilizar nosotros el método que creamos adecuado. La única duda que nos queda respecto a los microinterruptores es ver si también se comportan como las entradas digitales cuando tenemos múltiples activados a la vez. Para ello vamos a realizar una captura activando dos o más entradas. FIGURA 50 - ACTIVACIÓN DE MICROINTERRUPTORES CORRESPONDIENTES A CODO Y HOMBRO DEL ROBOT. Al activar los microinterruptores del hombro 0x02 y el codo 0x04 del robot podemos ver como recibimos la entrada 0x06, es decir la suma de las dos entradas. Ocurre igual para cualquier otra combinación de las mismas por lo que podemos asegurar que efectivamente se comporta igual que las entradas digitales. Ya sabemos por lo tanto dónde y cómo nos indica el robot el estado de los microinterruptores. 57 Desconexión al forzar motor Ahora que ya sabemos trabajar con los microinterruptores debería ser más sencillo desarrollar una función capaz de guiar el robot a la posición de Hard Home. Sin embargo nos surge una pregunta, Si todos los ejes encuentran su posición de Home a través de un micro interruptor, ¿ Cómo lo hace el eje de apertura y cierre de la pinza ? Durante las capturas pudimos apreciar que cuando un eje intentaba sobrepasar físicamente su ángulo máximo de giro y chocaba aparecía un valor diferente a 0 en el byte 59 del estado. Al producirse este choque y aparecer este valor se pierde la capacidad de movimiento y el robot se para por lo que Scorbase nos indica el error y en que eje se produjo mientras que también nos informa de que tenemos que volver a hacer un control-on para poder volver a operar el robot. FIGURA 51 - COLISIÓN DEL MOTOR DEL EJE 1. FIGURA 52 - COLISIÓN DE LOS MOTORES 4 Y 5 (0X10 + 0X08) DURANTE INCLINACIÓN DE LA PINZA. La única manera de saber cómo hace Scorbase para retomar el control una vez ocurre esto es engañar al programa original encendiendo el robot en una posición próxima a la de choque, por lo que tendremos todos los codificadores a cero, y realizando un movimiento manual en esa misma dirección. Se ha observado que al colisionar un eje se pierde el control momentáneamente pero luego Scorbase es capaz de recuperarlo y para ello realiza una re sincronización con el controlador de manera que su mensaje de salida codifique los motores con los valores que el robot tiene internamente. Así es como llega a la posición de Home la pinza, realizando una apertura indefinida hasta que el controlador informa de la colisión del eje y luego re sincronizando con ella a la posición que nos está devolviendo en su estado. Este método de re sincronización se ha probado para el resto de los motores y se ha visto que es utilizado por todos los motores del robot en todos los ejes. 58 Codificadores motores y Home Una vez llegamos a la posición de Home debemos realizar un reset de los codificadores motores al igual que hace Scorbase ya que en la posición de Home se considera que los codificadores están a cero. FIGURA 53 - PESTAÑAS DE INFORMACIÓN TRAS REALIZAR HOME. Normalmente no vamos a tener los motores a cero al llegar a la posición de Home ya que los motores se encuentran a cero al encender el robot y siempre se van a realizar una serie de movimientos para llegar a la posición de Home que no va a dejar todos los codificadores con el valor inicial que deseamos, en ocasiones incluso tras hacer Home con el programa original Scorbase veremos como algún valor puede pasar a ser 1 ó -1 al cabo de un tiempo en estado ocioso. Lo que nos interesa ahora es averiguar cómo resetea los motores Scorbase para poder luego implementar nosotros esta funcionalidad en nuestra función de Home. Realizar capturas del proceso de Home nos da un exceso de mensajes en la captura ya que se realizan movimientos de motores durante el mismo. Sin embargo existe una función que nos ofrece Scorbase que podría ser interesante. FIGURA 54 - FUNCIONES SCORBASE. La función Poner Eje A Cero está disponible como vemos en la sección Control de ejes de Scorbase. Vamos a realizar una llamada a la función y el eje 1 ya que Scorbase nos ofrece un menú donde elegir y al parecer está disponible, al menos Scorbase nos la deja elegir. 59 FIGURA 55 - ERROR DE SELECCIÓN DE EJE EN SCORBASE. Sin embargo podemos ver que existe un problema al intentar su ejecución y es que esta función sólo nos permite resetear los codificadores asociados a los puertos externos 7 y 8. Aun así vamos a realizar una captura y ver qué se envía al controlador durante el reset de los codificadores asociados a los puertos 7 y 8. Tenemos una secuencia de mensajes de la que hemos extraído todos los que se han enviado al controlador que no fueran los del estado ocioso (0x0D) y han sido los siguientes: CAPTURA 7 - SE ENVÍAN LOS COMANDOS 4F 40 54 Y 48 40 00 CAPTURA 8 - AL RECIBIR RESPUESTA DE LOS ANTERIORES ENVÍA 47 00 00 Y 4F 40 53 60 CAPTURA 9 - FINALMENTE ENVÍA LOS COMANDOS 73 20 00 Y 42 20 00 Una vez realizadas múltiples capturas y analizados los mensajes tanto para el reset del motor 7 y posteriormente para el 8 se ha apreciado que el único de los mensajes que se envían que varía es el segundo (0x48 0x40 0x00) más concretamente el segundo parámetro y por lo tanto debe ser el que codifica el motor que se va a resetear. Se han probado diferentes motores y resumido en la siguiente tabla: Motor 1 2 3 4 5 6 7 8 Valor 12 16 20 24 28 32 36 40 Los bytes del búfer que codifican la posición se resetean en nuestro mensaje, es decir, en nuestro búfer del mensaje de salida vamos a codificar la posición de dicho motor con los cuatro bytes a cero. Los mensajes finales coinciden con los dos últimos que se envían al controlador al finalizar un movimiento, no se ha realizado ningún movimiento pero al estar reseteando el contador para Scorbase es como si se hubiera producido por lo que no resulta extraño que estos dos mensajes que indican un fin de movimiento se envíen aquí también al finalizar el reset del contador. Resumen En este apartado hemos descubierto cómo leer los microinterruptores y cómo salir de un bloqueo generado debido a una colisión en un eje. Además hemos visto como resetear cualquier codificador motor. Esta información nos será de gran utilidad a la hora de implementar una función que lleve a cabo el Home del robot con éxito. 61 3.10 Dispositivos Externos Implementación del uso de una Cinta Transportadora El robot Scorbot-ER 4u nos proporciona dos salidas adicionales donde conectar ejes motores. Estos pueden ser con contador o sin el mismo como es el caso de la cinta transportadora de la que vamos a hacer uso durante la prueba de la interfaz Web. Sabemos cómo controlar motores basándonos en los incrementos que debemos enviar en el estado al controlador, sin embargo en este caso no podemos controlar la cinta del mismo modo al no utilizar codificador. Para saber cómo arranca y para la cinta Scorbase vamos a realizar una serie de capturas y analizar los datos obtenidos. También debemos tener en cuenta que Scorbase nos proporciona la posibilidad de indicar en que eje se encuentra la cinta y la velocidad a la cual queremos que se mueva por lo que se debe intentar averiguar también éstos parámetros con el fin de incluirlos en nuestro driver. Al igual que hicimos para estudiar el resto de funciones, vamos a conectar la cinta y activarla en Scorbase. Para conectar la cinta es necesario conocer el esquema de conexión que nos proporciona el controlador a la hora de añadir motores externos, sin embargo, estos pines corresponden con el conector de un controlador Mark IV como podemos en la imagen: FIGURA 56 - CONEXIÓN MOTOR CINTA Conectaremos por lo tanto la salida al motor de la cintra transportadora en los puertos 9 y 7, que a su vez conectaremos a la salida de voltaje del controlador Scorbot. 62 FOTO 7 - CONEXIÓN MOTOR CINTA Una vez filtrados los mensajes hemos desarrollado las funciones que emulan los mismos con el fin de nosotros poder controlar con la misma precisión que lo hace Scorbase el funcionamiento de la cinta. Nuestra función aceptará como parámetros el eje y la velocidad, el eje se traduce simplemente en el código de operación que enviamos en el tercer byte del primer mensaje dependiendo del eje y el segundo será la velocidad que se pasa como parámetro a través del quinto byte del segundo mensaje. Estos datos, como en los demás casos los conocemos gracias al estudio del tráfico y se han verificado como correctos los dos parámetros. La velocidad de salida está originalmente establecida por Scorbase entre 0 y 10, cuando leímos el mensaje en la captura hemos visto como 0x64 (100 en decimal) se correspondía a la velocidad 5, y realizando otra posterior con el fin de ver el límite del parámetro vimos que 0xC8 (200) se corresponde a la velocidad 10. Por lo tanto podemos calcular el parámetro del mensaje en nuestra función antes de enviarlo como VELOCIDAD*20. Finalmente para terminar la conexión de la cinta hemos conectado a una entrada digital el sensor que se encuentra en la cinta, su diagrama es el siguiente: 63 FIGURA 57 - ESQUEMA SENSOR INFRARROJO Ha sido conectado a un puerto de 12 voltios que nos proporciona el controlador para su alimentación y su salida al puerto de entrada digital 3 del mismo: FOTO 8 - SENSOR INFRARROJO CINTA Por lo tanto las conexiones finales han quedado como podemos apreciar en la siguiente imagen: 70 Las dos primeras funciones tienen como objetivo el activar y desactivar la salida que se indique por parámetro y la tercera cambiar el estado actual de la salida que se indique. La última función recibirá como parámetro un byte donde se codifica el estado de las 8 salidas y este estado será aplicado sobre las mismas. E/S Analógicas En el controlador Scorbot como vimos anteriormente durante el estudio de los mensajes codifica la entrada/salida analógica en un byte. El controlador por lo tanto permite 255 posibles salidas cuyos valores podemos estimar a como múltiplos de 10 V./255 = 0.039 V. ahora vamos a ver dónde podemos redondear el valor para quedarnos lo más cerca posible de los 255 de los que disponemos, así conseguimos además una mejor resolución en la salida. Para calcular la salida debemos aplicar la siguiente regla donde vamos a probar una serie de valores para la unidad de salida que es la cantidad más pequeña con la que podemos operar: 0.04: 0.04*255 = 10.2 0.0393: 0.0393*255 = 10.0215 0.0392: 9.996 Como vemos el valor unitario más cercano que nos permite trabajar con 10 voltios es 0.0392 V. Esta función enviará una salida al controlador que más se acerque a lo que indica el usuario, por lo tanto trabajará con cualquier valor entre 0 y 10v. Para trabajar con los valores analógicos que vamos a leer y escribir se van a crear un par de funciones que nos devolverán el valor del byte de salida dado un voltaje y el voltaje cuando lo que leemos es el byte con la entrada. Las dos funciones son voltajeAByte y byteAVoltaje que se encuentran en utils.h de nuestra librería. Ambas han sido probadas y se ha verificado su correcto funcionamiento como podemos ver a continuación: FIGURA 60 - PRUEBA DE LA FUNCIÓN VOLTAJE A BYTE 71 Entrada Analógica La entrada analógica se puede leer en cualquier momento en el byte 7 y 9 del estado del controlador. Se dispone de dos entradas analógica por lo que la función tomará como parámetro cuál de las dos entradas deseamos leer.  float leeEntradaAnalógica(int entrada); Salida Analógica La salida analógica al igual que la digital se controlar a través del envío de un código de operación y un byte con el valor codificado para el voltaje de salida. Sólo disponemos de una salida analógica por lo que trabajar con la misma resulta sencillo y podemos operar utilizando una única función.  int activaSalidaAnalogica(float voltaje); El parámetro requerido es un valor entre 0 y 10 voltios, el rango de operación de la salida analógica. 5.4 Funciones de movimiento Las funciones de movimiento son todas aquellas que nos permitirán mover los ejes del robot.  int moverEje (int eje, int pasos)  int moverEjeGrados (int eje, float grados)  int moverEjes (int [])  int moverEjesGrados (int []) Las dos primeras funciones se encargan de mover un eje el número de pasos o grados indicado. moverEjeGrados realizará una llamada a moverEje una vez convertidos los grados a pasos para el eje concreto, del mismo modo también es la función llamada por las dos siguientes (moverEjes y moverEjesGrados) que mueven todos los ejes por lo que el correcto funcionamiento de la primera era fundamental para que todas las demás funcionasen correctamente. Otras funciones de movimiento también importantes son las que hacen uso de los puntos almacenados en memoria y nos permiten movernos entre los mismos.  int irPos(int []) irPos simplemente recibe como parámetro un vector con los valores objetivo para los 6 ejes y se encarga de que el robot realice el movimiento adecuado para alcanzar ese estado. 72 La última función de movimiento desarrollada debido a su complejidad y que además necesitaba de una sincronización especial con el controlador ha sido aquella que permite el movimiento directo de los ejes del robot.  int moverEjeCont(int eje, int direccion) Como hemos visto anteriormente se llama a través de esta función para variar el valor de las variables globales de movContinuo y movContinuoDir pero el movimiento realmente se realiza a través del hilo de sincronización. 73 5.5 Aplicaciones A continuación se detallan las aplicaciones desarrolladas aparte del driver y que hacen uso del mismo. Pequeña aplicación para uso simple del robot Esta aplicación nos proporciona una manera muy simple de trabajar ya que dispone de menús que nos van informando de lo que hace cada función y proporciona la posibilidad de utilizar el robot de una forma simple. FIGURA 61 - APLICACIÓN DE PRUEBA Esta aplicación además ha servido de base para ir probando todas las funciones de la librería durante su desarrollo. Esta aplicación es similar a un intérprete de comandos a excepción de que el intérprete no dispone de ningún tipo de menú y sólo se encarga de pasar nuestras órdenes al controlador robot. Intérprete de comandos El intérprete de comandos nos da la posibilidad de utilizar el robot a través de una serie de órdenes simples y por lo tanto supone una forma rápida y simple de poder operar con el dispositivo. Esto no era posible con el controlador original del Scorbot-ER 4U ya que sólo nos ofrecía la operación a través de su interfaz gráfica así como otras limitaciones siendo la más relevante, sobretodo en un robot educativo, el control directo de los ejes motores. Disponemos de dos versiones del intérprete de manera que podemos utilizarlo directamente a través de comandos que enviamos a través de terminal como en modo servidor donde escuchará los mensajes que le sean retransmitidos desde la interfaz realizada en HTML5 a través del conector PHP. 74 La mayor parte de las funcionalidades que nos proporcionan las dos versiones del intérprete están incluidas en una librería (interprete.h) que hemos utilizado en ambas versiones por lo que sus diferencias se limitan a la entrada de mensajes y retorno de resultados. La principal función de la librería es ejecutaComando(comando,parametros,resultado) donde cómo podemos ver se le pasa el comando a ejecutar, los parámetros del mismo y una variable donde se nos devolverá el resultado si la función en cuestión retorna algún valor. int ejecutaComando(char *comando, char *params,char *resultado){ retornar = 0; if(funcEjes(comando,params))return 1; if(funcPinza(comando,params))return 1; if(funcMovimiento(comando,params))return 1; if(funcES(comando,params))return 1; if(funcSalida(comando,params))return 1; if(funcEstado(comando,params))return 1; return 0; } Como vemos se van llamando a una serie de funciones que en caso de reconocer el comando retornan 1 indicando que el comando ha sido reconocido. No debemos confundir esto con el valor que retorna el intérprete a la terminal ya que este se devuelve siempre en la variable resultado siempre y cuando la variable global retornar nos indique que existe dicho retorno por parte de la función llamada. Se ha decidido dividir el reconocimiento de los comandos en diferentes funciones para poder categorizar en cierta manera las diferentes funciones dependiendo de su objetivo y si interactúan únicamente con el controlador o en caso contrario operan con el brazo robot. Por ejemplo para la entrada/salida digital podemos ver el siguiente fragmento de código: int funcES(char *comando,char *params){ if(strcmp(comando,"SALIDADIGITAL")==0){ int salida = atoi(params); char *parametro2 = strtok(params," "); parametro2 = strtok(NULL,""); int estado = atoi(parametro2); SetSalidaDigital(salida,bool(estado)); return 1; } else if(strcmp(comando,"SALIDAANALOGICA")==0){ int salida = atoi(params); char *parametro2 = strtok(params," "); parametro2 = strtok(NULL,""); float estado = atof(parametro2); activaSalidaAnalogica(salida,estado); return 1; } ... 75 Intérprete Terminal En la versión terminal el intérprete operará con el robot enviándole los mensajes que el usuario vaya introduciendo haciendo uso de nuestra librería. Su principal diferencia con el programa simple que hemos diseñado para operar con el robot donde disponemos de menús y guiamos al usuario es que el intérprete se limita a reconocer los comandos y si éstos son válidos enviarlos al controlador directamente. Intérprete Servidor En esta versión el intérprete no recibirá la entrada del usuario a través de la terminal sino que se mantendrá escuchando en un puerto a la espera de que el servidor PHP le envíe mensajes. Podemos apreciar en el siguiente fragmento de código como se abrirá un socket y se esperará que vayan llegando para realizar las llamadas a la función ejecutaComando, estos comandos llegan a través del servidor Apache. Se hará una explicación extensa sobre esta comunicación en un apartado propio que explica toda la conexión entre el navegador web con la interfaz e intérprete. A continuación repasamos los comandos del intérprete y desarrollamos con un ejemplo de uso su funcionalidad. Comandos del Intérprete Comandos asociados a los ejes Los comandos de control de ejes son todos aquellos relacionados con el movimiento de los ejes del robot, la memorización de posiciones y su estado. Inicialización HardHome: Realiza el Hard Home del robot. HardHome SoftHome: Mueve el robot hasta la posición de softHome. Si al llamar a softHome todavía no se ha hecho HardHome se irá a la posición de inicio que tenía el robot al encender el controlador. SoftHome 76 Poner eje a cero: este comando resetea el contador interno del robot para el motor indicado. PonerCero motor Ejemplos: PonerCero 1 PonerCero 2 FijaSoftHome: La posición actual del robot será la nueva posición de Soft Home para el robot. FijaSoftHome HomeEje: Busca la posición de Home únicamente para el eje indicado por parámetro. HomeEje eje Ejemplos: HomeEje 2 Control de la pinza Los comandos descritos a continuación se encargan del control del actuador del robot, en el caso del robot Scorbot-ER 4u se trata de una pinza. Abrir Pinza: Abre completamente la pinza. AP | AbrirPinza Cerrar Pinza: Cierra completamente la pinza. CP | CerrarPinza Estos dos comandos no requieren ningún parámetro. Mordaza: Permite establecer una apertura de la pinza determinada por milímetros de apertura. Mordaza mm Movimiento de ejes y motores Los comandos de movimiento de ejes se encargan del movimiento de los ejes que componen el 77 brazo robótico mientras que el movimiento de motores realiza movimientos sobre los motores independientemente del eje al que pertenezcan. MoverEje: Esta orden tiene dos versiones ya que como su nombre indica se encarga de realizar movimientos sobre los ejes del robot y éstos se pueden definir tanto en pasos de motor como en grados de rotación del eje. MoverEjePasos eje pasos MoverEjeGrados eje grados MoverMotor: El comando moverMotor a diferencia de moverEjes no tendrá en cuenta si un eje debe mover más de un motor para realizar un movimiento ni en qué dirección o si basta con mover un único motor. Este comando puede dar problemas al trabajar con los motores 4-5 ya que como vimos durante el desarrollo de funciones estos dos motores realizar diferentes movimientos en función de su dirección. En este caso para evitar confusiones se va a limitar el movimiento a pasos de motor y no grados ya que estamos tratando directamente con los motores. MoverMotor motor pasos Memorización de posiciones La memorización de posiciones nos permite almacenar en memoria la posición actual del brazo robótico de manera que podamos recuperarla en operaciones posteriores. Memorizar Posición: nos permitirá almacenar la posición actual del robot en la variable indicada, si no existe, se creará. MemPos nombre Ir a Posición: llevará al robot a la posición almacenada en la variable indicada siempre y cuando ésta exista, de no existir no se realizará ningún movimiento. IrPos nombre Comandos de control de flujo Estos comandos son utilizados en el intérprete servidor ya que éste es el que tendrá capacidad para tener esas etiquetas y saltos a los que vamos a hacer referencia. 78 Control de flujo Espere: Pausa la ejecución del programa el número de milisegundos indicado. Espere milisegundos Salta A: Salta a la subrutina indicada. SaltaA nombreRutina Si: Ejecuta el comando indicado si se cumple la condición. Este comando se puede combinar con otros para tener el funcionamiento equivalente a una sentencia IF de un lenguaje de programación de alto nivel. Si Condición Comando Ejemplos: Ejemplo de utilización del comando Si para funcionamiento equivalente a un if/else entrada = leeEntradaDigital 3 Si entrada==1 SaltaA rutina1 SaltaA rutina2 79 Comandos Entrada/Salida Activación y desactivación de salidas digitales SalidaDigital: El control sobre las salidas digitales se puede realizar a través de este comando donde se indicará el número de la salida y el estado que deseamos que tome. salidaDigital [0-7] (0|1) SD [0-7] (0|1) Ejemplos: salidaDigital 2 1 SD 6 0 En el primer caso hemos puesto la salida 2 en estado activo y en el segundo hemos desactivado la salida 6. Activación de la salida analógica SalidaAnalogica: Las salidas analógicas se controlan indicando el voltaje de salida y la salida que lo va a proporcionar. salidaAnalogica (0|1) voltaje SA (0|1) voltaje Ejemplos: salidaAnalogica 0 5 SA 1 7.5 En este ejemplo estamos poniendo la salida 0 a 5 voltios y la salida 1 a 7.5 voltios. El voltaje puede ser tanto entero como coma flotante siempre y cuando respete los valores dentro del rango de operación del controlador, en este caso 0-10 Voltios. Lectura de entradas LeeEntradaDigital: Utilizando este comando junto a una asignación se puede almacenar el valor de una entrada digital en una variable. Si no se realiza asignación se mostrará por pantalla. 86 Interfaz gráfica HTML5 + Javascript + JQuery UI Disponemos de una interfaz gráfica que nos proporciona la posibilidad de trabajar con el brazo robótico de manera más cómoda e intuitiva. La interfaz se ha desarrollado utilizando la librería Jquery UI para los elementos gráficos y Javascript para el resto de la programación web. Las diferentes ventanas y diálogos se han desarrollado como archivos HTML independientes y el aspecto final se ha aplicado a través de un estilo CSS. FIGURA 63 - INTERFAZ COMPLETA Toda la interfaz está compuesta por 21 archivos HTML, 8 archivos .JS, la librería Jquery UI y un archivo de estilo CSS. Se ha hospedado todo el contenido en un servidor Apache local que incluye PHP. Una parte de la interfaz nos informa del estado actual de las entradas y nos permite modificar las salidas tanto digitales como analógicas: 87 FIGURA 64 - E/S DIGITALES Y ANALÓGICAS. La interfaz también nos permite conocer en cualquier momento la posición actual del robot: FIGURA 65 - VALORES CODIFICADORES MOTORES. También tenemos la posibilidad de almacenar estos valores en las diferentes posiciones que podemos añadir a nuestra lista: FIGURA 66 - LISTA DE POSICIONES. La mayoría de comandos disponen de diálogos para hacer el trabajo más sencillo y corregir posibles errores antes de enviarlos a la lista que compone el programa actual. Esto nos da la ventaja de que prácticamente no se van a enviar mensajes erróneos o con valores no válidos al intérprete por lo que supone además una capa extra de protección frente al uso incorrecto del dispositivo. 88 FIGURA 67 - DIÁLOGO PARA MOVIMIENTO DE UN EJE. El movimiento directo de los ejes que nos proporcionaba Scorbase también se encuentra presente en nuestra interfaz a través de un diálogo: FIGURA 68 - CONTROL DIRECTO DE LOS EJES. 89 El intérprete servidor El intérprete que se ejecuta en el servidor para escuchar mensajes del conector PHP es un ejecutable que se comunicará a través de un socket con el servidor PHP Apache que le retransmitirá los mensajes. Una vez parseado el mensaje se llamará al intérprete de manera que ya sea éste el encargado de utilizar el mensaje recibido como vea conveniente, llamando a una función, desechándolo si no es válido, etc… Vamos a ver cómo funciona el servidor: int main(void) { …INICIALIZACIÓN Y CONTROL-ON CON EL ROBOT… //Variables asociadas al servidor. WSADATA wsaData; int iResult; SOCKET ListenSocket = INVALID_SOCKET; SOCKET ClientSocket = INVALID_SOCKET; struct addrinfo *result = NULL; struct addrinfo hints; int iSendResult; //Búfer para datos recibidos. char recvbuf[DEFAULT_BUFLEN]; memset(recvbuf,0,sizeof(recvbuf)); int recvbuflen = DEFAULT_BUFLEN; //Inicialización del servidor. iResult = WSAStartup(MAKEWORD(2,2), &wsaData); if (iResult != 0) { printf("WSAStartup failed with error: %d\n", iResult); return 1; } ZeroMemory(&hints, sizeof(hints)); hints.ai_family = AF_INET; hints.ai_socktype = SOCK_STREAM; hints.ai_protocol = IPPROTO_TCP; hints.ai_flags = AI_PASSIVE; //Definimos el puerto que vamos a utilizar. iResult = getaddrinfo(NULL, DEFAULT_PORT, &hints, &result); if ( iResult != 0 ) { printf("getaddrinfo failed with error: %d\n", iResult); WSACleanup(); return 1; 90 } //Escuchamos en un socket. ListenSocket = socket(result->ai_family, result->ai_socktype, result->ai_protocol); if (ListenSocket == INVALID_SOCKET) { printf("socket failed with error: %ld\n", WSAGetLastError()); freeaddrinfo(result); WSACleanup(); return 1; } //Terminamos de preparar el socket para recibir conexiones. iResult = bind( ListenSocket, result->ai_addr, (int)result->ai_addrlen); if (iResult == SOCKET_ERROR) { printf("bind failed with error: %d\n", WSAGetLastError()); freeaddrinfo(result); closesocket(ListenSocket); WSACleanup(); return 1; } freeaddrinfo(result); //Escuchamos en el socket. iResult = listen(ListenSocket, SOMAXCONN); if (iResult == SOCKET_ERROR) { printf("listen failed with error: %d\n", WSAGetLastError()); closesocket(ListenSocket); WSACleanup(); return 1; } … if(scorbotConnect){ printf("Conexion al controlador Scorbot OK!\n"); }else{ //Si falla la conexión al controlador USB cerramos el servidor. printf("Error al intentar conectar al controlador!\n"); closesocket(ListenSocket); WSACleanup(); return 1; } //Empezamos a escuchar la conexión en el socket para el comando que se nos envíe. do { // Aceptamos el cliente. ClientSocket = accept(ListenSocket, NULL, NULL); if (ClientSocket == INVALID_SOCKET) { printf("accept failed with error: %d\n", WSAGetLastError()); closesocket(ListenSocket); WSACleanup(); return 1; } iResult = recv(ClientSocket, recvbuf, recvbuflen, 0); if (iResult > 0) { char *comando = strtok(recvbuf," "); char *parametros = strtok(NULL,""); char *resultado; 91 int res = ejecutaComando(comando,parametros,resultado); memset(recvbuf,0,sizeof(recvbuf)); printf("Response size: %d",respL); //Envíamos respuesta al cliente. iSendResult = send( ClientSocket, resp, respL, 0 ); if (iSendResult == SOCKET_ERROR) { printf("Error envío: %d\n", WSAGetLastError()); closesocket(ClientSocket); WSACleanup(); return 1; } } else if (iResult == 0) printf("Cerrando conexión...\n"); else { printf("Transm. error: %d\n", WSAGetLastError()); closesocket(ClientSocket); WSACleanup(); return 1; } } while (iResult > 0); // Cierre de conexión y limpieza de recursos. iResult = shutdown(ClientSocket, SD_SEND); if (iResult == SOCKET_ERROR) { printf("Error al cerrar: %d\n", WSAGetLastError()); closesocket(ClientSocket); WSACleanup(); return 1; } closesocket(ClientSocket); WSACleanup(); return 0; } Como vemos gran parte del programa se encarga de montar el servidor tras conectar con el controlador, una vez abierto el socket nos metemos en un bucle desde el cual empezamos a parsear los mensajes que nos llegan en conjuntos de comandos y parámetros, además de pasar una variable resultado para el mismo. Con este servidor y el conector PHP se ha solucionado la comunicación a través de la interfaz web. 92 5.6 Pruebas realizadas Esta parte del documento está dedicada a las diferentes pruebas que se fueron realizando en forma de pequeños subprogramas con el fin de comprobar el correcto comportamiento de las funciones durante su desarrollo. Entrada / Salida Digital Para comprobar la salida digital se han activado y desactivado varios puertos como podemos ver en la siguiente sección de código: … printf("Salida digital a activar: "); scanf("%d",&i); activaSalidaDigital(i); … En todos los casos se han activado correctamente cada una de las salidas digitales. Como era de esperar, al ordenar que se active una salida ya activa no se produce ninguna variación. La entrada digital se ha probado activando los puertos mediante el uso de un cable conectado al puerto de tierra que emula lo que sería una entrada digital como pudimos ver durante el estudio de transferencia de la entrada/salida digital. El código utilizado para leer los puertos de entrada ha sido el siguiente: … printf("\nEstado actual de la entrada digital:\n"); for(int e=1;e<9;e++){ printf("Entrada %d = %d\n",e,leeEntradaDigital(e)); } … Todas las pruebas fueron satisfactorias al leer los puertos de entrada, tanto para pruebas con una única entrada activa como cuando se han activado varias a la vez. 93 Entrada / Salida Analógica La entrada analógica se prueba enviando una serie de tensiones a los puertos utilizando una fuente variable de las que disponemos en el laboratorio y verificando que los valores leídos son los indicados por la misma. El único parámetro necesario es el que indica la entrada que vamos a leer: … printf(“Seleccione entrada analógica: ”); scanf("%d",&i); leeEntradaAnalogica(i); printf("Tensión en el puerto %d = %f",i,voltaje); } … Durante las pruebas la entrada analógica en todos los casos ha coincidido con la indicada por la fuente. La salida analógica se ha probado indicando el voltaje de salida a aplicar en cada uno de los puertos y realizando mediciones de las mismas con un voltímetro: … printf(“\nPrueba de salida analógica\n”); scanf("%d",&i); float voltaje; printf("Tensión a aplicar en la salida: "); scanf("%f",&voltaje); activaSalidaAnalogica(voltaje); … Todas las pruebas realizadas nos han activado la salida con el valor correspondiente. Para los voltajes indicados por parámetro que estén fuera de rango se indica al usuario por pantalla y la salida no varía. 94 Apertura / Cierre de pinza Se ha decidido probar como primer paso al movimiento de los motores el funcionamiento de la pinza ya que se puede probar fácilmente para valores límites en los extremos de funcionamiento cuando la pinza se encuentra abierta o cerrada. El siguiente fragmento de código nos ha permitido probar tanto el cierre como la apertura de la pinza: … int i; scanf("%d",&i); if(i==1)abrePinza(); if(i==2)cierraPinza(); } … Una vez comprobado el funcionamiento del cierre de la pinza hemos probado la apertura de la misma realizando todas las pruebas varias veces de manera satisfactoria. También se ha probado ordenar el cierre de la pinza con la pinza ya cerrada y una apertura con la pinza abierta, en ambos casos no ha habido problemas ya que la función detecta la colisión del eje y retoma el control del robot automáticamente. En ambos casos la pinza se ha comportado igual que cuando se da esta orden con el programa original Scorbase. 95 Estado del robot Para acceder al estado del robot donde se incluye el estado de los microswitches y los encoders tenemos a nuestra disposición una serie de funciones encargadas de leer del búfer de entrada del controlador el estado de los mismos. Vamos a hacer unas pruebas simples donde realizaremos una lectura del estado tras Home y otras donde una vez colocado el robot en una posición por Scorbase y visto el valor para los codificadores que nos indica realizaremos luego una lectura con nuestro programa con el fin de comprobar si la lectura de los codificadores se realiza de forma correcta. Si nos fijamos en el estado tras Home vemos que coincide con los valores que indica Scorbase, es decir, todos los codificadores han sido reseteados a cero: FIGURA 69 - ESTADO DEL ROBOT TRAS HOME. En este otro caso vamos a irnos a una posición aleatoria donde tengamos valores tanto negativos como positivos para los codificadores. FIGURA 70 - CAPTURA DE POSICIÓN EN SCORBASE. FIGURA 71 - CAPTURA EN NUESTRA PRUEBA. Vemos que los valores de codificador que muestran Scorbase y nuestra función coinciden por lo que esta función está funcionando correctamente. Ahora vamos a probar el estado de los microswitches activando primero uno y posteriormente dos para comprobar que la lectura funciona en ambos casos. 102 6. Conclusiones El objetivo de lograr desarrollar un controlador para el robot que cubriese todas las funcionalidades básicas que también proporciona Scorbase ha sido satisfecho y además hemos logrado dotar al mismo de un entorno web que permite el uso desde otro programa que no sea el original. Durante las pruebas se ha verificado el funcionamiento por separado de cada una de las funciones desarrolladas con el fin de ver si cumplían su objetivo como mínimo igual de bien que al ser llamadas por Scorbase. Todas las funciones han cumplido con su cometido y no son distinguibles de sus análogas en Scorbase. La creación de un entorno web ha permitido demostrar como la comunicación no se ha extendido únicamente a tener una librería que poder utilizar en C/C++ sino de poder interactuar con el robot en un entorno tan remoto como puede ser un navegador web que acceda al servidor donde se encuentra en ejecución un intérprete remoto a la espera de recibir los mensajes. Finalmente, uno de los puntos más importantes a tener en cuenta es que se ha desarrollado el driver a partir de la observación del tráfico en el bus USB y no accediendo al código original ni desensamblando ningún archivo del Scorbot-ER 4u proporcionado por Intellitek. Aunque este método de ingeniería inversa es arriesgado ya que podríamos habernos encontrado con mensajes codificados de manera más complicada o algún tipo de autentificación por parte del controlador, este no ha sido el caso, y aunque algunas funciones han sido más complicadas que otras, sobre todo las relativas al movimiento y mantenimiento de la conexión, se ha conseguido un driver aceptable y funcional. 103 7. Anexos A continuación se incluyen los siguientes anexos a este documento:  Instalación  Configuración como filtro.  Instalación Standalone.  Contenido del CD.  Ejemplo función: abrePinza().  Listado de funciones de la librería.  Listado de comandos para el intérprete. 104 7.1 Instalación La librería elegida para comunicarnos utilizando el protocolo USB ha sido ‘LibUsb’ ya que vamos a desarrollar el driver utilizando sistema operativo Windows XP donde disponemos de la instalación tanto de el driver original como el programa Scorbase necesarios durante el estudio de los comandos que se envían al controlador. La librería libusb proporciona la instalación de drivers filtro que permiten interactuar con el dispositivo de manera transparente. Esto supone una gran ventaja a la hora de trabajar con el controlador ya que nos da la posibilidad de trabajar con el software original “Scorbase” utilizando su driver propietario mientras a través del filtro tenemos la posibilidad de inyectar y leer mensajes utilizando nuestra aplicación. El driver está implementado como una biblioteca de enlace dinámico (DLL) y se instalará en la carpeta “win32” a través de su programa de instalación. Configuración utilizando el filtro Una vez instalado el driver debemos indicar al mismo qué dispositivo es el que vamos a filtrar. Para ello se nos proporciona una aplicación que nos mostrará un desplegable con los dispositivos conectados actualmente y que nos permite tanto la instalación como la eliminación del driver. FIGURA 77 - CONFIGURACIÓN FILTRO Si el filtro ha sido configurado correctamente podremos ver el siguiente mensaje informándonos sobre su correcta instalación. Adicionalmente podemos obtener información detallada sobre el dispositivo filtrado 105 utilizando una de las aplicaciones proporcionadas por ‘LibUsb’ llamada ‘TestLibUsb’ que nos muestra todos los dispositivos que están siendo filtrados actualmente y datos sobre los mismos. FIGURA 78 - DISPOSITIVO FILTRADO. Salida del programa TestLibUsb DLL version: 1.2.6.0 Driver version: 1.2.6.0 bus/device idVendor/idProduct bus-0/\\.\libusb0-0001--0x09f1-0x0007 09F1/0007 - Unable to fetch manufacturer string bLength: 18 bDescriptorType: 01h bcdUSB: 0100h bDeviceClass: 00h bDeviceSubClass: 00h bDeviceProtocol: 00h bMaxPacketSize0: 08h idVendor: 09F1h idProduct: 0007h bcdDevice: 0000h iManufacturer: 1 iProduct: 0 iSerialNumber: 0 bNumConfigurations: 1 wTotalLength: 41 bNumInterfaces: 1 bConfigurationValue: 1 iConfiguration: 140 bmAttributes: 40h MaxPower: 50 bInterfaceNumber: 0 bAlternateSetting: 0 bNumEndpoints: 2 bInterfaceClass: 255 bInterfaceSubClass: 0 bInterfaceProtocol: 0 iInterface: 186 bEndpointAddress: 83h bmAttributes: 02h wMaxPacketSize: 64 bInterval: 1 bRefresh: 0 bSynchAddress: 0 bEndpointAddress: 02h bmAttributes: 02h wMaxPacketSize: 64 bInterval: 1 bRefresh: 0 bSynchAddress: 0 106 Vemos que en nuestro caso el dispositivo es detectado y además se nos proporciona una serie de detalles sobre el mismo como los endpoints, su configuración y su número de interfaz. Instalación Standalone Para instalar nuestro driver sin tener presente en el equipo el driver original de Scorbase debemos utilizar uno de los dos instaladores generados por libUSB o directamente utilizar el archivo .inf también creado con este fin. El archivo .inf contiene toda la información relativa al dispositivo y se instala como cualquier otro driver con la única diferencia de que al ser un driver de desarrollo propio no dispone de firma y puede causar algún problema al instalar en sistemas operativos como Windows 8 en adelante donde para instalar drivers sin firmar debemos entrar en el equipo en modo seguro. A continuación tenemos parte de la información del archivo .inf del driver, que como vemos apunta a que el driver del dispositivo será libusb, por lo que ya no es necesario el filtro el driver original: ; Scorbot_ER-4u.inf ; Copyright (c) 2010 libusb-win32 (GNU LGPL) [Strings] DeviceName = "Scorbot ER-4u" VendorName = "Intelitek" SourceName = "Scorbot ER-4u Install Disk" DeviceID = "VID_09F1&PID_0007" DeviceGUID = "{E8B13CEA-3CB3-436B-A20C-12C55F8962D4}" [Version] Signature = "$Windows NT$" Class = "libusb-win32 devices" ClassGuid = {EB781AAF-9C70-4523-A5DF-642A87ECA567} Provider = "libusb-win32" CatalogFile = Scorbot_ER-4u.cat DriverVer = 01/17/2012, 1.2.6.0 107 [ClassInstall32] Addreg = libusb_class_install_add_reg [libusb_class_install_add_reg] HKR,,,0,"libusb-win32 devices" HKR,,Icon,,-20 [Manufacturer] %VendorName% = Devices, NT, NTAMD64, NTIA64 FIGURA 79 - INSTALADORES DEL DRIVER PROPORCIONADOS. 7.2 Contenido del CD El código fuente de todas las aplicaciones desarrolladas se encuentra en las siguientes carpetas del archivo proporcionado, codigo.7z:  Driver (driver.c, driver.h)  InterfazTerminal (main.c)  Interprete/lib (interprete.h, interprete.c)  Interprete/InterpreteLocal (intLocal.c)  Interprete/InterpreteServidor (intServ.c)  InterfazGrafica (Archivos HTML, JS y CSS)  ConectorWeb (conectorWeb.php) Además se han incluido muestras de las capturas utilizadas durante el análisis de los mensajes USB entre Scorbase y el robot así como durante las pruebas de éste driver en la carpeta capturas. 108 7.3 Función ejemplo: abrePinza( ) A continuación se incluye el código de la función abrePinza(), podemos ver como a su vez hace uso de una función llamada grip(cierra) donde pasamos como parámetro si queremos cerrar la pinza, si es falso se entiende que queremos realizar una apertura. Esta optimización se hizo con el fin de no tener que escribir dos funciones tan parecidas para la apertura y el cierre cuando podíamos usar un condicional para saber si el codificador debe aumentar o disminuir. //Abre la pinza. int abrePinza(){ grip(false); codes(); } //Abre o cierra dependiendo del parámetro. int grip(bool cierra){ usb_bulk_read(handleRobot, 0x83, temp, 64, 1000); enviaOpCode(0x47,0,0); enviaOpCode(0x4f,0x20,0x53); enviaOpCode(0x4c,0x20,0x00); enviaOpCode(0x42,0x20,0x01); int posActual = leeMotor(5); int salida = posActual++; int v = 1; int codMotor = 32; int i=0; int velocidad=150; while(limitSwitch()!=0x20){ char* hexa; hexa = (char*)&salida; array[codMotor]=hexa[0]; array[codMotor+1]=hexa[1]; printf("."); enviaOpCode(0x0d,0,0); if(cierra){ salida -= velocidad; if(salida<0){ salida += 65535; array[codMotor+2]=0xff; array[codMotor+3]=0xff; } }else{ salida += velocidad; if(salida>65535){ salida -= 65535; array[codMotor+2]=0; 109 array[codMotor+3]=0; } } } enviaOpCode(0x73,0x20,0x00); enviaOpCode(0x42,0x20,0x00); enviaOpCode(0x0d,0x00,0x00); leeEstado(); } En negrita podemos ver los mensajes enviados al controlador. Todas las funciones de la librería hacen uso de esta función con el fin de enviar los códigos, veamosla en detalle: //Envia un mensaje con el código de operación definido por los 3 bytes. int enviaOpCode(char op1,char op2, char op3){ array[4]=op1; array[5]=op2; array[6]=op3; usb_bulk_write(handleRobot,0x02,array,64,1000); Sleep(Retardo); array[0]=array[0]+1; usb_bulk_read(handleRobot, 0x83, temp, 64, 1000); Sleep(Retardo); } Se puede ver que su cometido es colocar los parámetros del mensaje en el búfer y enviarlo, una vez esperamos y aumentamos la sincronización en 1 podemos volver a leer el búfer y volcarlo en temp para acceder al estado del controlador. 110 Funciones de la librería Las funciones incluidas en la librería desarrollada se encuentran descritas a continuación indicando sus parámetros de entrada y salida. Comunicación y conexión PC-Controlador Nombre conectar Entrada usb_dev_handle *h Salida int resultado Descripción A partir del handle del dispositivo se encarga de iniciar la comunicación creando el hilo de sincronización y enviando los primeros comandos de reconocimiento al controlador. Nombre control_on Entrada Salida int resultado Descripción Nos permite tomar el control del controlador de manera que se sincronice el estado interno en software con el del robot. Además envía una serie de comandos de inicialización necesarios para el posterior control. Nombre idleThread Entrada LPVOID lpParam Salida Descripción Esta función define el hilo de sincronización y recibe el handle al controlador como parámetro a través de un puntero. Esta función siempre debe ser utilizada al crear el hilo ya que de lo contrario se cortará la conexión al controlador. 111 Funciones de control Nombre buscaHomeRobot Entrada Salida int resultado Descripción Lleva a cabo el Hard Home del robot llamando a buscaHome para cada eje. Nombre buscaHome Entrada int eje Salida int resultado Descripción Busca la posición de Hard Home para el eje indicado por parámetro. Nombre irSoftHome Entrada Salida int resultado Descripción Lleva al robot a la posición Soft Home definida tras realizar un Hard Home del mismo o por el usuario a través de fijarSoftHome. Nombre fijarSoftHome Entrada Salida int resultado Descripción Fija la posición actual del robot como posición Soft Home sobrescribiendo el Soft Home actual. Nombre leeSwitches Entrada Salida int resultado Descripción Lee el estado actual de todos los microSwitches y lo coloca en el vector booleano microSwitches. 118 Comandos para el intérprete Comandos asociados a los ejes Inicialización Hard Home HH, HardHome Realiza el Hard Home del robot. Soft Home SH, SoftHome Mueve el robot a posición de Soft Home. Poner eje a cero PonerCero motor Resetea a cero el contador del codificador motor. Fijar Posición Soft Home FijaSoftHome La posición actual del robot es la nueva posición de Soft Home. Busca posición de Home de un eje HomeEje eje Busca a través de los microswitches la posición Home de un eje concreto. Control de la pinza Abrir Pinza AP, AbrirPinza Abre la pinza. Cerrar Pinza CP, CerrarPinza Cierra la pinza. Mordaza Mordaza milímetros Establece apertura pinza. Movimiento de ejes y motores Mover Eje MoverEjePasos eje pasos MoverEjeGrados eje grados Mueve el eje indicado una cantidad de pasos de motor o grados. Mover Motor MoverMotor motor pasos Mueve el motor indicado. Memorización de posiciones Memorizar posición MemPos nombrevar Memoriza la posición actual 119 del robot en la variable. Ir a posición IrPos nombrevar Mueve el robot a la posición almacenada en la variable. Comandos de Control de flujo Control de flujo Espere Espere milisegundos Pausa durante los milisegundos indicados. Salta A SaltaA nombreRutina Salta a una subrutina. Si Condición Salta A Si condición Comando Evalúa una condición y ejecuta el salto si se cumple. Comandos de Entrada/Salida Activación y desactivación de salidas digitales Salida Digital SalidaDigital [0-7] (0|1) SD [0-7] (0|1) Cambia el estado de la salida indicada. Activación de la salida analógica Salida Analógica SalidaAnalogica [0-3] voltaje SA [0-3] voltaje Cambia el voltaje de la salida analógica. 120 Lectura de entradas Lee Entrada Digital LeeEntradaDigital [0-7] ED [0-7] Lee la entrada digital. Lee Entrada analógica LeeEntradaAnalogica [0-3] EA [0-3] Lee la entrada analógica. Estado del robot Lee Switch LeeSwitch [0-5] LS [0-5] Devuelve estado de microinterruptor. Lee Motor LeeMotor [0-6] LM [0-6] Devuelve el número de pasos del codificador. Lee Eje LeeEje [0-5] LE [0-5] Devuelve ángulo de giro del eje respecto a Home. Estado Robot EstadoRobot ER Muestra por pantalla resumen del estado del robot. Salida a consola y aviso del controlador Salida a consola Escribir Escribir “ristra” Escribir var. Muestra por consola la ristra o variable indicada. Aviso controlador Timbre Timbre Hace sonar el timbre de aviso del controlador. 121 8. Bibliografía Bibliografía Online  www.libusb.com  www.jquery.com  http://www.eeherald.com/section/design-guide/esmod14.html  http://www.l-com.com/content/USB-Tutorial.html  http://en.wikipedia.org/wiki/Universal_Serial_Bus  http://www.beyondlogic.org/usbnutshell/usb3.shtml  http://www.theoldrobots.com/book45/ER4u_User_Manual.pdf  http://www.usbmadesimple.co.uk/ums_4.htm  http://www.freebsd.org/doc/en/books/arch-handbook/usb-dev.html  http://www.keil.com/pack/doc/mw/USB/html/_u_s_b__endpoints.html  https://touch-base.com/documentation/USB%20Basics.htm  http://www.robotics.org/joseph-engelberger/unimate.cfm Bibliografía Física  Scorbot-ER 4u, Manual de usuario. Intellitek, STI Soluciones Tecnológicas Integradas.  Controlador USB, Manual de usuario. Intellitek, STI soluciones tecnológicas Integradas.  Esquemas Cinta transportadora y sensor infrarrojo. Rhino