Diseño, realización y control de un robot autoequilibrado de bajo coste basado en una Raspberry Pi

Proyecto Fin de Carrera Ingeniería de Telecomunicación Formato de Publicación de la Escuela Técnica Superior de Ingeniería Autor: F. Javier Payán Somet Tutor: Juan José Murillo Fuentes Dep. Teoría de la Señal y Comunicaciones Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2013 Trabajo Fin de Grado Grado en Ingeniería de Tecnologías Industriales Diseño, realización y control de un robot autoequilibrado de bajo coste basado en una Raspberry Pi Autor: José Ignacio Cámara Molina Tutores: Ignacio Alvarado Aldea Pablo Krupa García Dep. de Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2020 Trabajo Fin de Grado Grado en Ingeniería de Tecnologías Industriales Diseño, realización y control de un robot autoequilibrado de bajo coste basado en una Raspberry Pi Autor: José Ignacio Cámara Molina Tutor: Ignacio Alvarado Aldea Pablo Krupa García Dep. de Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2020 Trabajo Fin de Grado: Diseño, realización y control de un robot autoequilibrado de bajo coste basado en una Raspberry Pi Autor: José Ignacio Cámara Molina Tutores: Ignacio Alvarado Aldea Pablo Krupa García El tribunal nombrado para juzgar el trabajo arriba indicado, compuesto por los siguientes profesores: Presidente: Vocal/es: Secretario: acuerdan otorgarle la calificación de: El Secretario del Tribunal Fecha: Resumen El presente trabajo trata de diseñar, construir y controlar un robot autobalanceado basado en la configuración del péndulo invertido, donde se aplicarán y compararán diferentes estrategias de control aprendidas durante los años de carrera. En resumidas cuentas, el objetivo del trabajo es conseguir fabricar un vehículo de bajo coste que pueda ser replicado, complementado y, en su caso, mejorado por otros usuarios. Se parte de la idea de trabajos anteriores desarrollados en el departamento de Ingeniería Automática de la Escuela superior de Ingeniería de Sevilla (ETSI), donde a pesar de sus similitudes, presenta grandes diferencias que se irán vislumbrando a lo largo de los capítulos. Se pretende conseguir que el vehículo, basado en la configuración del péndulo invertido, sea capaz de mantener su posición de equilibrio en todo momento, pudiendo desplazarse y girar sin que pierda dicha posición. En primer lugar, se analizarán todos los componentes que formarán parte del vehículo, para poder diseñar el cuerpo acorde a sus dimensiones y características. Posteriormente, se diseñará el cuerpo del vehículo con programas de modelado tridimensional para su impresión en 3D. Se ha diseñado una PCB que aglutinará gran parte de los componentes electrónicos, reduciendo el uso de cables, mejorando con ello sus conexiones. Una vez conseguido el hardware del vehículo, se procederá al análisis del modelo matemático y su posterior linealizado para aplicar técnicas de control. Se diseñarán diferentes controladores para mantener la estabilidad. Se diseñará un controlador LQR, posteriormente se añadirá un término integral al LQR y se finalizará con la implementación de un control MPC. La última etapa del trabajo consistirá en implementar el control del vehículo en las dos plataformas utilizadas para ello, el microcontrolador Arduino para el control de los motores y la microcomputadora Raspberry Pi para el control en alto nivel. Aquí radica la gran diferencia con respecto a trabajos anteriores y que abre un gran abanico de posibilidades para trabajos futuros. La potencia de cálculo de la Rpi en comparación con Arduino nos posibilita la implementación de todo tipo de controladores, mejorando las prestaciones del vehículo. Por último, se añade en la memoria un capítulo de resultados donde se puede apreciar el funcionamiento de las técnicas de control ante diferentes tipos de experimentos, como puede ser perturbaciones tipo pulso, variaciones en la referencia de velocidad de las ruedas o ascenso y descenso de una rampa, entre otros. I Índice Resumen I Índice de Figuras VII Índice de Tablas XI Notación XIII 1 Introducción 1 1.1 Motivación 1 1.2 Objetivo 1 1.3 Punto de partida 2 1.4 Requisitos 3 1.5 Estructura de la memoria 4 1.6 Herramietas Sofware 4 2 Componentes 7 2.1 Raspberry Pi 7 2.1.1 Historia 7 2.1.2 Especificaciones Raspberry Pi modelo 3 9 2.1.3 Sistema operativo 9 Xenomai 10 2.1.4 Configuración inicial 10 2.1.5 Conexión remota 11 2.1.6 Comunicación 15 2.1.7 I2C 15 2.1.8 GPIO 17 2.2 Arduino Nano 18 2.3 Motores paso a paso 19 2.3.1 Características específicas 20 2.4 Driver de control Easydriver HW135 v4.4 21 2.4.1 Descripción de los pines 22 2.4.2 Características 23 2.5 MPU6050 23 2.5.1 Descripción del fabricante 24 2.5.2 Descripción de los pines 25 2.5.3 Características del dispositivo 25 III Índice de Tablas 2.1 Características primeros modelos RPi 8 2.2 Características RPi 3 generación 9 2.3 Características RPi 3 modelo B 9 2.4 Configuración IP estática RPi 14 2.5 Características Arduino Nano 19 2.6 División de pasos 20 2.7 Conexión motor 22 2.8 Resolución del paso 23 2.9 Conexión MPU-RPi 26 2.10 Características Acelerómetro 27 2.11 Características Giroscopio 28 2.12 Presupuesto 35 3.1 Parámetros del sistema 43 3.2 Velocidad máxima teórica 43 5.1 Velocidades módulo HC-06 77 XI Notación RPi Raspberry Pi RTC Real Time System (Sistema en Tiempo Real) 3DTridimensional A.CCorriente alterna D.CCorriente continua PWM Modulación por Ancho de Pulso NEMA National Electrical Manufacturers Association SPR Steps Per Second (Pasos por segundo) SPI Serial Peripheral Interface (Interfaz de periféricos en serie) IDE Integrated Development Environment (entorno de desarrollo integrado) ADC Analog to Digital Converter (convertidor analógicodigital) DAC Digital to Analog Converter (convertidor digitalanalógico) I2CProtocolo de comunicación inter-integrado de circuito CPU Central Processing Unit (unidad central de procesos) USB Universal Serial Bus (bus serie universal) SRAM Static Random Acess Memory (Memoria estática de acceso aleatorio ) MHz Megahercios I/OInput/Output (entrada/salida) mm Milímetro AAmperio VVoltio mV Milivoltio ms Milisegundo ns Nanosegundo µsmicrosegundo GND Ground (Tierra/Masa) IMU Inertial Measurement Unit (Unidad de Medida Inercial) PLA Ácido poliláctico XIII 1 Introducción Todo ingeniero debe aspirar a poder crear su propio mundo, su propia tecnología. El mundo de la electrónica permite crear, analizar y simular cualquier sistema físico. Plasmar una idea en un proyecto físico es un desafío complejo, pero a la vez intrépido y gratificante. 1.1 Motivación Mu chas veces hemos intentado mantener en equilibrio una barra sobre la palma de nuestra mano, ante la inestabilidad que presenta, automáticamente nuestra respuesta es mover la mano para evitar que esta caiga, manteniéndola así en su posición de equilibrio. El mecanismo que actúa es simple, el cerebro actuará como un sensor que capta la inclinación de la barra, percibiendo que esta se va a caer, y automáticamente lanzará un estimulo para que se produzca un movimiento de la mano, evitando así que se produzca la caída. Sin darnos cuenta estábamos delante de lo que hoy es este trabajo fin de grado. La idea anterior se asemeja al concepto de equilibrio de un péndulo invertido. Sin duda alguna, el péndulo invertido es uno de los problemas clásicos dentro del campo de la automática y del control que más se ha estudiado y que mayor interés suscitan entre los estudiantes y profesores de ingeniería. Todo ello debido a su relativa facilidad en comparación con otros experimentos, la posibilidad de implementar diferentes estrategias de control, así como la aplicación práctica de los diferentes conceptos teóricos estudiados en diferentes asignaturas a lo largo de los cursos de estudio universitario. En resumidas cuentas, es un gran sistema de pruebas para implementar diferentes estrategias de control. El mecanismo del vehículo basado en el péndulo invertido se compone de una parte fija y una parte móvil, cuyo objetivo es mantener estable el vehículo, entendiendo como estable la propiedad que presenta el vehículo de mantenerse en equilibrio o de volver a dicho estado tras sufrir una perturbación. Si no dotáramos al mecanismo de un sistema de control, al desplazarlo de su punto de equilibrio, por la fuerza de la gravedad, simplemente se caería. Estamos presente ante un sistema no lineal e inestable. 1.2 Objetivo El documento ante el que estamos presente trata de construir un robot autobalanceado sobre dos ruedas basado en la configuración del péndulo invertido, donde su centro de masas se encontrará por encima de su eje de rotación, lo que hace que se produzca un equilibrio inestable, es decir, cualquier perturbación haría al sistema perder su posición. Por ello, nuestro esfuerzo será devolver al vehículo a esta posición de equilibrio, siendo capaz de compensar las perturbaciones que pueda sufrir en su funcionamiento. 1 2Capítulo 1. Introducción Por tanto, el objetivo será dotar a un vehículo tipo péndulo invertido del mecanismo de control que evite que este se caiga, manteniéndolo en movimiento y en su posición vertical de equilibrio. Para ello, contaremos con un sensor que nos proporcione en cada momento la información sobre la velocidad de caída e inclinación del vehículo con respecto a su vertical. Con los datos obtenidos por el sensor, y mediante dos motores paso a paso conectados a cada una de las ruedas, dotaremos a estas de la aceleración necesaria para mantener la posición estable. El objetivo de control será por tanto la aceleración a proporcionar a cada rueda por parte de los motores, dicha aceleración será implementada a través del microcontrolador Arduino, siendo la Raspberry Pi el cerebro de nuestro proyecto, y por ello, el que realice las tareas de control a alto nivel calculando la aceleración que se debe aplicar. Cada rueda deberá tener la posibilidad de girar a diferente velocidad para dotar al vehículo de capacidad de giro, por ello las aceleraciones deberán ser independientes para ambas ruedas. Se pretende poder controlar remotamente el vehículo, para ello, se va a añadir un módulo bluetooth. De tal manera que, mediante una App y desde nuestro teléfono móvil, se pueda controlar un joystick que muevan el vehículo en ambos lados y hacia delante y atrás. 1.3 Punto de partida El presente trabajo trata de presentar una variante de los continuos trabajos que el departamento de automática de la Escuela Técnica Superior de Ingeniería de Sevilla, en adelante ETSI, ha ido realizando en los últimos años. En trabajos anteriores se ha diseñado, simulado y fabricado un vehículo basado en el péndulo invertido de bajo coste mediante el microcontrolador Arduino. Bajo las mismas premisas que en trabajos anteriores, se ha optimizado para mantener el coste económico de fabricación a sus niveles más bajos, utilizando para ello componentes electrónicos de bajo coste y amplio uso en el mundo electrónico, testados en multitud de experimentos. En la figura 1.1 podemos apreciar el vehículo utilizado en los trabajos realizados por Antonio Croche Navarro [ 18 ] y Cecilia González González [ 8 ], siendo este uno de los primeros vehículos con los que se inició esta línea de experimentos. Vemos que presentan grandes diferencias con respecto a nuestro proyecto, tanto en el hardware como en software. En ambos trabajos se utilizó el microcontrolador Arduino Mega como cerebro del proyecto. En cuanto al chasis del vehículo, se utilizo una estructura de aluminio y madera y dos motores de corriente continua con reductora para el movimiento de las ruedas, lo cual hacía superior el volumen y peso del vehículo. Los inconvenientes de dichos materiales fueron explicados por Antonio Borja Conde en su trabajo [4] Figura 1.1 Vehículo autobalanceado Antonio Croche-Cecilia González. 1.4 Requisitos 3 Posteriormente, Jose Antonio Borja Conde [ 4 ] y Nicolás Cortés Fernández [ 6 ] realizaron una serie de cambios sustanciales, pasando de utilizar motores de corriente continua a motores paso a paso, los cuales son mucho más precisos al poder aplicar velocidades similares a la referencia, prescindiendo del uso de reductoras. Ambos trabajos continuaban con el uso del microcontrolador Arduino para las tareas de control, cambiando la versión del Arduino Mega al Arduino Uno. En cuanto al chasis del vehículo, este paso a ser de PLA obtenido mediante impresión 3D el primero de ellos, y de madera el segundo. (a) (b) Figura 1.2 (a) Vehículo Jose A. Borja. (b) Vehículo Nicolás Cortés . Como hemos indicado anteriormente, el presente trabajo trata de continuar con la línea iniciada, mejorando las características del vehículo. Todos los trabajos citados hacían uso del microcontrolador Arduino para implementar las tareas de control, aquí radica la principal diferencia, pues para el control de nuestro vehículo se ha optado por el uso de la microcomputadora Raspberry Pi modelo 3, más adelante citaremos las mejoras que aporta este cambio. El uso del microcontrolador Arduino seguirá presente para el control de los motores, usaremos la versión Nano, de características similares a la versión Uno pero de tamaño reducido. Al igual que el anterior vehículo, seguirá usándose dos motores paso a paso. 1.4 Requisitos Todo proyecto robótico debe cumplir una serie de requisitos con el fin de que la tarea para la que ha sido diseñada sea funcional. Entre los diferentes requisitos del presente trabaja podemos destacar: 1. Autonomía: Permitiendo que el robot funcione sin intervención humana después de su calibrado. 2. Bajo Coste: Debe ser un proyecto que se abastezca de componentes de bajo coste para que pueda ser replicado en trabajos futuros. La RPi, así como los drivers y motores que utilizaremos cumplen este requisito. 3. Portabilidad: Debe ser un proyecto capaz de ser retomado y replicado por otros alumnos. 4Capítulo 1. Introducción 4. Ligero y de fácil transporte: No dotando al robot de elementos voluminosos ni pesados, facilitando con ello movimientos más veloces. 5. Funcional: Debemos asegurar su funcionalidad, estudiando en particular todas las posibles opciones que nos podrían dar fallos. 1.5 Estructura de la memoria Podemos dividir el trabajo en tres grandes bloques bien diferenciados, pero dependiente entre ellos. El primero de ellos será el estudio de todos los componentes que forman parte de vehículo, así como el diseño y fabricación del chasis que los englobe. La segunda parte contendrá el estudio del modelo y de los diferentes métodos de control a aplicar, es decir, el diseño y simulación del sistema de control. La última parte consistirá en la implementación del sistema de control elegido, así como la discusión de los resultados obtenidos. Dentro de los bloques comentados anteriormente, vamos a dividir la presente memoria en una serie de capítulos con el objetivo de dotarla de una mayor simplicidad y así facilitar la lectura de la misma. • Capítulo 1: Se realiza una breve introducción del trabajo, dando una imagen general de lo que nos vamos a encontrar en los capítulos venideros. Explica la motivación, así como los objetivos y estructura del trabajo. • Capítulo 2: Dedicaremos íntegramente este capítulo a exponer y explicar los diferentes elementos electrónicos que formarán parte del proyecto, desde el acelerómetro y giroscopio para conocer la posición y velocidad del vehículo, hasta los motores que dotarán de movilidad a nuestro robot. • Capítulo 3: Realizaremos un estudio cinemático del modelo, obteniendo las ecuaciones del modelo físico-matemático, así como el modelo linealizado para poder aplicar controladores en variables de estado. • Capítulo 4: Dedicaremos este capítulo a mostrar las diferentes estrategias de control que hemos aplicado al proyecto. Implementaremos un control LQR, LQR con efecto integral y MPC. • Capítulo 5: Este capítulo estará dedica a explicar el software del vehículo. Se explicará las estrategias de programación en ambos dispositivos, mostrando el código y detallando su funcionamiento. • Capítulo 6: En este capítulo se mostrarán y analizarán detenidamente los resultados obtenidos mediante gráficas que muestran su funcionamiento. Se expondrá el vehículo a diferentes experimentos como pueden ser perturbaciones de su posición de equilibrio, variaciones en la referencia de velocidad de las ruedas o ascenso y descenso de un rampa. • Capítulo 7: Para finalizar la memoria se incluirá este capítulo donde se mostrarán las conclusiones del trabajo y se expondrán futuras lineas de investigación y mejora. 1.6 Herramietas Sofware A lo largo del trabajo necesitaremos el uso de diferentes herramientas software que nos conduzcan a la resolución de todos los apartados. A continuación recopilaremos los diferentes programas utilizados. • PuTTy: Es un cliente SSH, sirve para la conexión remota a servidores. En resumidas cuentas, PuTTy nos permite tener acceso al terminal de comandos de un servidor remoto. 1.6 Herramietas Sofware 5 • VNC Viewer: Con este programa podremos controlar remotamente la Raspberry Pi desde nuestro ordenador, teniendo acceso a todos los documentos, mostrándonos el escritorio, actuando como si conectásemos directamente la Raspberry a un monitor. • Autpdesk Fusion 360: Programa de diseño CAD con el que se ha diseñado el cuerpo del vehículo en 3D. • EasyEda. Programa web destinado al diseño del esquema de conexiones y diseño de la PCB • Arduino IDE. Entorno de desarrollo que permite programar y compilar programas para el microcontrolador Arduino. • Matlab. Programa de cómputo numérico con lenguaje de programación propio. Su uso en este trabajo ha sido para el cálculo del vector de realimentación del control LQR y para la representación gráfica de los resultados obtenidos ante los experimentos desarrollados. 12 Capítulo 2. Componentes • Necesidad de estar conectados a la misma red mediante otro ordenador, bien por Wifi o cable Ethernet. Para realizar una conexión remota desde nuestro PC con Windows instalado, necesitamos descargar previamente una serie de programas en el pc: • PuTTy: Es un cliente SSH, sirve para la conexión remota a servidores. En resumidas cuentas, PuTTy nos permite tener acceso al terminal de comandos de un servidor remoto. • VNC Viewer: Con este programa podremos controlar remotamente la Raspberry Pi desde nuestro ordenador, teniendo acceso a todos los documentos, mostrándonos el escritorio, actuando como si conectásemos directamente la Raspberry a un monitor. A continuación vamos a explicar cómo conectarnos remotamente mediante un cable Ethernet. Lo primero que tenemos que hacer es conectar el cable entre los puertos Ethernet del PC y de la RPi. Nuestro PC cuenta con el sistema operativo Windows 10, tenemos que seguir la ruta: panel de control> Red e internet>Centro de redes y recursos compartidos> Cambiar configuración del adaptador. Figura 2.4 Pantalla de configuración Windows 10. Hacemos click derecho en el icono Ethernet y accedemos a sus propiedades, Figura 2.5 Pantalla de configuración Windows 10. Cliqueamos en Protocolo de internet versión 4 (TCP/IPv4) 2.1 Raspberry Pi 13 Figura 2.6 Propiedades de Ethernet. Aparecerá la siguiente pantalla, donde pulsaremos Usar la siguiente dirección IP y le proporcionaremos una, en nuestro caso 192.168.148.1 y la máscara de red 255.255.255.0. Figura 2.7 Propiedades Protocolo de Internet versión 4. Realizada la configuración en el PC, tenemos que proceder a configurar la IP estática en la RPi. Para ello, en la tarjeta micro SD buscaremos el archivo cmdline.txt. Al final del texto del archivo tenemos que añadir la siguiente información: 14 Capítulo 2. Componentes Tabla 2.4 Configuración IP estática RPi. IP cliente 192.168.148.2 IP servidor Puerta de enlace predeterminada 192.168.148.1 Mascara de Red 255.255.255.0 Nombre del cliente rpi Dispositivo al que nos conectamos eth0 Configuración automática off Todos los datos anteriores deberán estar separado con dos puntos, quedando en nuestro caso la siguiente línea: ip=192.168.148.2::192.168.1:255.255.255.0:rpi:eth0:off Modificado el archivo en la tarjeta micro SD, volvemos a introducirla en la RPi y la conectamos por cable al PC. Ahora vamos a comprobar que la instalación ha tenido éxito. Desde el terminal de comandos de Windows tecleamos el comando ipconfig, obteniendo la siguiente lista, donde vemos que el puerto Ethernet está conectado a la dirección IP definida estáticamente. Figura 2.8 Terminal de comandos Windows 10. Realizada la comprobación, podemos proceder a conectarnos a la Raspberry. Lo primero que tenemos que hacer es abrir el programa PuTTy e introducir la IP de la Raspberry que hemos definido estáticamente Figura 2.9 Servidor Putty. 2.1 Raspberry Pi 15 Acto seguido se abrirá el terminal de comandos de la RPi, donde se nos pedirá el usuario, por defecto pi y la contraseña que le pusimos cuando instalamos el software. Figura 2.10 Terminal de comandos RPi. 2.1.6 Comunicación Una de las grandes ventajas del uso de la Raspberry en robótica es la comunicación y control que podemos realizar sobre los circuitos y módulos electrónicos, que pueden ser de diversa manera: • Mediante el uso de buses, como I2C o SPI que permiten la comunicación con dispositivos complejos como sensores. • Mediante el uso de los pines GPIO que proporcionan versatilidad y su uso es similar a la ya más que conocida placa Arduino. • Mediante el uso de los módulos USB (nuestro modelo cuenta con 4), al que le podemos conectar un monitor, un teclado, un ratón... • Mediante el uso del módulo Wifi, Ethernet o Bluetooth, con los que podemos establecer contacto en tiempo real con la red. 2.1.7 I2C Por su importancia en este proyecto, se va hacer especial mención al protocolo I2C. A través de dicho protocolo se va conectar la RPi con el microcontrolador Arduino y con el sensor MPU6050. El protocolo I2C (Inter-Integrate Circuit) es un protocolo de transferencia síncrona de datos en serie maestro-esclavo, utilizado para comunicar datos entre dos dispositivos. Dicho protocolo apareció al principio de la década de los ochenta de la mano del fabricante Philips, destinado a periféricos de baja velocidad. Hoy en día sigue gozando de una gran popularidad y aceptación debido a su relativa simplicidad, así como a la capacidad de establecer un gran número de conexiones. El funcionamiento para la comunicación es simple, se requieren dos líneas de señal. Por un lado, se encuentra la línea Serial Data o SDA y por otro la línea Serial Clock o SCL. La línea SDA se utiliza para la transmisión de datos bidireccional, es decir, la misma línea SDA se utiliza para enviar y recibir datos en paquetes de 8 bits y la línea SCL se utiliza para la sincronización de la transmisión de datos. Dado que se requiere de una señal de reloj para la sincronización, la transferencia será síncrona. En la figura 2.11 podemos apreciar un esquema del bus I2C. 16 Capítulo 2. Componentes Figura 2.11 Esquema comunicación I2C. Los dispositivos conectados al bus I2C disponen de una dirección única para cada dispositivo y pueden ser maestros o esclavos. Cada uno de los esclavos recibe de forma predeterminada una dirección en formato de 7 bits, por ejemplo en el caso que se verá más adelante, el módulo GY-521 que integra el sensor MPU-6050, tendrá asignada la dirección 0x68 o 0x69. Como hemos indicado anteriormente, el proceso de transferencia de datos es del tipo maestroesclavo. El maestro será el encargado de comenzar la comunicación con el resto de esclavos, pudiendo mandar o recibir datos de estos. Por otro lado, los dispositivos que se han definido como esclavos no pueden iniciar comunicación con el dispositivo maestro, aunque si pueden transferir datos al maestro. En la RPi, el bus I2C admite direccionamiento de 7 y 10 bits y frecuencias como máximo hasta 400 kHz en modo rápido y 100 kHz en modo estándar. Protocolo de transferencia Para la transferencia de datos, el protocolo I2C seguirá los siguientes pasos: 1. El maestro será quien inicie la comunicación. Mientras la señal de reloj (SCL) se encuentre en alto, el maestro enviará un flanco descendente en la linea SDA para el comienzo de la comunicación. Se define así la secuencia de arranque, donde el bus pasa a considerarse ocupado. 2. Como hemos indicado, el bus envía paquetes de 8 bits. Primero enviará los 7 bits que corresponden a la dirección del dispositivo esclavo junto con el bit que indica si queremos realizar una operación de lectura o escritura. Los bits en la linea SDA estarán colocados de manera que comenzará enviándose el bit más significativo. 3. Si existe un dispositivo esclavo ocupando la dirección de los primeros 7 bits que envía el maestro, el esclavo contestará con un bit en bajo denominado bit de reconocimiento (ACK) que indicará que reconoce la solicitud y está listo para la comunicación. 4. Se envía el paquete de datos de 8 bits. En cada envío, la linea SCL produce un flanco de subida y posteriormente de bajada. 5. Una vez enviado el paquete de 8 bits, el dispositivo que recibe los datos envía de vuelta al un bit de reconocimiento que indica que se ha recibido los datos. Por lo tanto, en cada transferencia de 8 bits de datos se producen 9 pulsos en la señal de reloj(SCL), en concreto 8 pulsos para el envío del dato y 1 pulso para el envío del bit de recepción. 2.1 Raspberry Pi 17 6. La trasmisión finalizará cuando el maestro envíe un flanco ascendente en la linea SDA, mientras la línea SCL se encuentre en alto. Se define así la secuencia de parada, el bus pasa a considerarse libre. En el presente trabajo actuará como maestro la RPi, siendo la que inicia la comunicación con los dispositivos esclavos y el que lleve la iniciativa y genere la señal de reloj. Uno de los esclavos será el sensor MPU-6050 que ocupará la dirección 0x68 y del que únicamente se leerán datos. Por otro lado, tendremos el dispositivo esclavo Arduino al que hemos asignado la dirección 0x05. La interacción con este último esclavo será doble, pues el maestro le enviará dato y también leerá datos de él. Aunque se explicará más adelante, tenemos que recalcar que el maestro no puede comunicarse con ambos esclavos en el mismo instante, pues se produciría una interferencia y daría datos erróneos. Es decir, mientras se esta produciendo una transferencia de datos, no podemos iniciar otra sin que esta última haya finalizado. Instalación en Raspberry Por defecto, cuando instalamos el sistema operativo Raspbian no se encuentra habilitada la comunicación I2C, por ello tendremos que seguir una serie de pasos para habilitar dicha comunicación. Lo primero que tenemos que hacer es entrar en el menú de configuración de la RPi y activar la interfaz I2C como vemos en la figura 2.2. Raspbian, al ser una distribución de Linux, nos proporciona una serie de herramientas que facilitan la comunicación con los dispositivos del bus I2C. Para instalar dichas herramientas tenemos que teclear desde el terminal de comandos la siguiente línea: sudo apt-get install i2c-tools. Entre las herramientas que nos proporcionan, destacamos i2cdetect, que teclearemos en el terminal de comandos como sudo i2cdetect –y 1. Dicho comando se encarga de comprobar la dirección del dispositivo que se encuentra conectado a nuestro terminal por el bus I2C, si no se encuentra conectado ningún dispositivo a la dirección en cuestión, aparcera el símbolo "–" que significará que se ha sondeado dicha dirección pero no se ha obtenido respuesta de ningún dispositivo. En el sondeo de direcciones puede aparecer UU en el lugar de la dirección que debería ocupar un dispositivo, esto indica que se ha obviado el sondeo de la dirección, debido a que dicha dirección está siendo utilizada por un controlador. 2.1.8 GPIO Los pines GPIO (General Purpose Input/Output) permiten conectar dispositivos externos a nuestra Raspberry y pueden funcionar como de entrada o salida. Cuenta con 40 pines en dos hileras de 20, y la distribución es la que se muestra en la figura 2.12: 18 Capítulo 2. Componentes Figura 2.12 Distribución GPIO. • La mayoría de los pines, un total de 26, están configurados como de entrada/salida de propósito general. •Los pines 8 y 10 se pueden configurar como interfaz UART. • Los pines 3 y 5 se utilizan para la interfaz I2C, permitiendo interactuar con dispositivos que se rigen según este protocolo. • El pin 12, 13, 18 y 19 se configuran como salida PWM. Constará de dos canales PWM independientes que operan a la misma frecuencia, pudiendo alterar su ciclo de trabajo. •Los pines 6, 9 ,14, 20, 25, 30, 34 y 39 se configuran como GND. • Los pines 27 y 28 están configurados inicialmente como salida y se utilizan para conectar una memoria serie en las placas de expansión. • Los pines 1 y 17 se utilizan para alimentar a 3.3 V, pueden suministrar juntos un máximo entorno a los 50 mA. • Los pines 2 y 4 se utilizan para alimentar a 5V, pueden suministrar juntos un máximo entorno a los 200-300 mA. Estos pines soportan una tensión de entrada comprendida entre 0-3.3V. Por lo tanto, tenemos que prestar sumo cuidado a la hora de conectar directamente a ellos dispositivos que trabajen a 5V, como el caso de Arduino. Sin embargo, la conexión de la RPi con Arduino mediante los pines I2C no presentan este inconveniente. Esto es debido a que los pines SCL y SDA están dotados de una resistencia pull-up. 2.2 Arduino Nano Para el control de los motores se va hacer uso de la placa Arduino en su versión Nano. Arduino Nano es una placa que contiene el microcontrolador Atmel ATmega328p. De características similares a Arduino Uno, cuenta con 3 Timers para la gestión de interrupciones, como se 2.3 Motores paso a paso 19 explicará en el apartado de programación, se usará uno de ellos para generar el tren de pulsos que dote a los motores de movimiento. A continuación se detallan las características consideradas más importantes. No se entra en gran detalle, para más información se puede acudir al foro oficial de Arduino donde se resuelven todo tipo de dudas. Tabla 2.5 Características Arduino Nano. Microcontrolador Atmel ATmega328p Frecuencia de operación 16 MHz Voltaje de operación 5 V Tensión de entrada 5-12 V Entradas/Salidas Analógicas 8 Entradas/Salidas Digitales 14 PWM 6 SRAM 2kB Flash 32kB Puerto de programación Mini USB UART 1 EEPROM 1 kB Dimensiones 45x18 mm En el apartado anterior se indica que la RPi cuenta con dos canales PWM a la misma frecuencia. Al comienzo del proyecto se intentó controlar los motores paso a paso mediante ambos canales. Sin embargo, un requerimiento de nuestro proyecto es poder dotar a las ruedas de velocidades diferentes, es decir dotar a cada rueda de un tren de pulsos independientes a diferente frecuencia. Otra opción era generar la señal PWM por software pero este caso era lento y no se da garantía que los pulsos se produzcan cuando se desean. En esta explicación radica la necesidad de incorporar Arduino Nano a nuestro proyecto para el control de los motores paso a paso. 2.3 Motores paso a paso Como actuadores del sistema se harán uso de dos motores paso a paso. En concreto, usaremos dos motores Nema 17. Figura 2.13 Motor paso a paso Nema 17. 20 Capítulo 2. Componentes El motor nema 17 es un dispositivo electrónico que se encarga de realizar movimientos precisos, potentes y controlados a partir de impulsos eléctricos. La velocidad de rotación será proporcional a la frecuencia con la que recibe los impulsos eléctricos. Recibe su nombre debido a las dimensiones de su base, 1.7 x 1.7 pulgadas. Mejora las prestaciones de precisión y control en comparación con los motores de corriente continua o los servomotores. Los motores paso a paso están compuestos por dos elementos, por un lado se encuentra una parte fija denominada estator compuestas por dos bobinas agrupadas y por otra parte se compone de unos imanes dentados fijados a un eje giratorio denominado rotor. Las bobinas del estator son imanes que cuando se excitan energéticamente, atraen a los ejes del rotor en sentido horario o anti horario. Un motor paso a paso puede definirse como un motor de corriente continua en los que los desplazamientos angulares en el rotor son discretos. El funcionamiento del motor se produce en las dos bobinas que lo componen debido al cambio en la dirección del flujo de corriente que las atraviesa, es decir, al proporcionar energía en ambas bobinas de forma secuencial, se generan movimientos angulares en el rotor. El mínimo movimiento angular que se produce en el rotor se denomina paso, el movimiento de un paso se produce en cada pulso que se le envía al motor. La elección de este motor es debido, entre otros aspectos, a su bajo coste, alta precisión sin errores acumulados, alta potencia , rápido posicionamiento, así como facilidad de control desde un microcontrolador a través del envió de trenes de pulsos. Podemos encontrar dos tipos de motores paso a paso, los motores unipolares y los motores bipolares. En nuestro caso, usaremos este segundo tipo. La elección del motor bipolar se ha debido a que ese tipo de motores tiene mayor par motor que los unipolares, a costa de una mayor complejidad en el control. Este escollo es salvable con la adición del driver de control que se explica en el siguiente apartado. Nuestro motor bipolar cuenta con cuatro cables, dos por cada bobina, por ello para energizarlas necesitamos del uso de un controlador o driver. Con dicho controlador, generaremos de forma sencilla los pulsos de señal para el funcionamiento del motor. Para alimentar el motor, necesitaremos una fuente de alimentación externa que proporcione 12 V, ya que la RPi no es capaz de proporcionarnos dicha tensión por sus pines, como máximo proporciona 5V. 2.3.1 Características específicas • Cuenta con 200 pasos completos por vuelta lo que equivale a 1.8 grados cada paso ( 360o 200pasos = 1.8o/paso ). Cada paso puede dividirse en hasta 8 micropasos, con lo que tendríamos 1600 micropasos por vuelta, aumentando con ello la precisión y la suavidad del giro. Para poder producir esta división del paso, necesitaremos el uso de un microstepping motor drive, tal y como veremos en el apartado siguiente. A continuación podemos apreciar los pasos necesarios para barrer una vuelta de 360 grados. Tabla 2.6 División de pasos. Micropasos Pulsos por vuelta Ángulo de avance Paso 1 200 1.8º Medio paso 2 400 0.9º Cuarto de paso 4 800 0.45º Octavo de paso 8 1600 0.225º •Presenta un eje circular de 5 mm de diámetro y 21 mm de largo. 2.4 Driver de control Easydriver HW135 v4.4 21 •Presenta una base de 42x42 mm y una altura de 34 mm con un peso que ronda los 280 g. • Cuenta con dos cables por cada bobina que se conectará con el controlador como indicaremos más adelante. •Corriente nominal de 1.5 A DC. •Tensión de alimentación de 12-36 V. •Resistencia en fase de 8 ohm (+-10 %). •Fase de inductancia de 10 mH (+-20 %). •Inercia del rotor de 54 G.cm2. •Par de frenado: 1.6 N.cm. •Par de fricción: 12 mN.m. •Máxima frecuencia de arranque sin carga >= 1500pps. •Máxima frecuencia de ejecución sin carga >=8000pps. 2.4 Driver de control Easydriver HW135 v4.4 Para el control de los motores paso a paso necesitaremos indispensablemente dos controladores, uno para cada motor. Hemos elegido el Easydriver HW135 en su versión 4.4. Consiste en un circuito integrado con un chip A3967SLB de Allegro. En el mercado encontramos diferentes tipos de driver, la elección de este en concreto se debe a su utilización en proyectos anteriores del departamento donde se ha comprobado su alta fiabilidad, gozando de una gran popularidad debido, entre otros aspectos, a su bajo coste y tamaño reducido, ideal para sistemas embebidos. Como hemos indicado, el Easydriver HW135 v4.4 es un controlador para un motor paso a paso bipolar con consumos entre 150mA y 750mA por fase y una salida entre 7 y 30V. Capaz de proporcionar modos de paso completo, medio, cuarto u octavo de paso. El dispositivo cuenta con un potenciómetro que permite ajustar la corriente máxima que se suministra al motor por fase. Como se ha indicado, este ajuste se encuentra comprendido entre 150mA y 750 mA. Nuestro motor tiene un consumo de 1.5 A, por lo tanto tendremos que ajustar dicho pontenciómetro a su valor máximo. Figura 2.14 Easydriver HW135 v4.4. Entre las funciones del driver de control podemos destacar: • Proporcionar, a través de una fuente de alimentación externa, la energía necesaria para el funcionamiento del motor que será de 12 V. Tanto la RPi como Arduino poseen como máximo salidas de 5 V, la cual no harían funcionar los motores. 28 Capítulo 2. Componentes La siguiente ecuación muestra la forma de obtener el ángulo mediante el uso del giroscopio. Angulo =Anguloanterior +(Giroscopio ∗4t)(2.3) Por lo general, las medidas obtenidas por el giroscopio suelen ser más precisas que las calculadas por el acelerómetro, en un corto periodo de tiempo. Sin embargo, en un largo periodo de tiempo, al calcularse el dato como una suma de incrementos, hará que se aleje de la realidad debido a una suma de los errores acumulados en la medida. Las características principales del giroscopio serán: Tabla 2.11 Características Giroscopio. Características Medidas 3 convertidores ADC 16 bits Rango de salida programable +-250,+-500,+-1000 y +-2000 º/s Intensidad nominal 3.6 mA La finalidad de giroscopio en nuestro proyecto no solo radica en la posibilidad de calcular el ángulo de inclinación del vehículo. También nos proporciona la velocidad de caída del vehículo, que como se verá en el capítulo de control, será una variable de estado para el desarrollo de controladores lineales. 2.5.7 Filtro complementario Como se ha indicado, no podemos obtener el ángulo de inclinación de nuestro vehículo directamente haciendo uso de las medidas del acelerómetro o del giroscopio. Dichos datos presentarán un gran ruido en el caso del acelerómetro y una gran deriva en el caso del giroscopio. Para salvar estas dificultades, vamos aplicar un filtrado de las medidas con el objetivo de obtener unos datos limpios, carentes de ruido y deriva que puedan ser interpretados por nuestro controlador. Para el filtrado de los datos obtenidos directamente del IMU podemos contar con varias estrategias. Entre ellas se encuentran aplicar un filtro de Kalman o un filtro complementario. Por su sencillez vamos a hacer uso del filtro complementario. El filtro complementario puede entenderse como un modelo simplificado del filtro de Kalman, eliminando el análisis estadístico lo cual reduce el número de cálculos y dota de mayor sencillez su implementación, igual de válida para el caso que se nos presenta. La ecuación del filtro complementario adquiere la siguiente expresión, donde vemos que aparece el término τ , el cual pondera el peso de las medidas del acelerómetro y del giroscopio. Normalmente adquiere un valor cercano a 0.99 dando una mayor importancia a la medida del giroscopio, que como hemos indicado son más precisas. La medida del acelerómetro será la encarga de corregir la deriva que va produciendo el giroscopio. Angulo =τ(Anguloprevio +Angulogiroscopio)+(1−τ)Anguloacelerometro (2.4) 2.6 Batería Para dotar al vehículo de la autonomía necesaria se prescindirá de toda alimentación por cable conectados a la corriente. Nos centraremos en el uso de una batería que alimente a todos los dispositivos involucrados en el funcionamiento del sistema. 2.7 Regulador de tensión KA78T05 29 La batería elegida será de polímero de litio, popularmente conocida como batería Li-Po. En el mercado podemos encontrar baterías Li-Po con diferentes especificaciones. Se ha optado por el uso de la batería X-COPTER POWER que consta de 3 celdas. Nos proporciona una capacidad de 1.500 mAh y una tensión nominal de 11.1 V. Figura 2.18 Batería LIPO. En el esquema del sistema electrónico, podemos apreciar más detalladamente la forma de alimentar cada dispositivo dependiente de la batería, aquí se citará brevemente. Arduino y la RPi se alimentarán a una tensión constante de 5V, tensión que se obtendrá a partir del regulador de tensión explicado en el apartado siguiente. Ambos motores se alimentarán a través de los controladores Easydriver con la tensión que proporciona directamente la batería. El resto de componentes se alimentarán a partir de la tensión que proporciona tanto RPi como Arduino en las salida de sus pines. 2.7 Regulador de tensión KA78T05 En el apartado anterior se indica el uso de una batería LIPO para la alimentación de la RPi, de Arduino y de ambos motores. La alimentación de ambos motores será a 12 V, no teniendo problemas para alimentarlo directamente desde la batería a través del controlador EasyDriver. Sin embargo, la Rpi no tolera valores de 12 V por su pin de alimentación, siendo la tensión máxima soportada de 5V. Arduino se podría alimentar por el pin Vin que tolera voltajes comprendidos entre 6-12 V o por el pin de 5V. Se opta por esta segunda opción. Para solventar el problema anterior con respecto al voltaje de alimentación de la Rpi, se va hacer uso del regulador de tensión lineal KA78T05. Figura 2.19 Regulador de tensión KA78T05. El regulador anterior nos permite obtener una tensión de salida continua de 5V y 3A a partir de 30 Capítulo 2. Componentes la entrada de 12 V. Dicha salida será estable y suficiente para la alimentación de la Rpi y Arduino. Ambas placas se alimentarán por su pin correspondiente de 5V. En el esquema eléctrico podemos apreciar las conexiones. Si analizamos con detalle las conexiones del esquema eléctrico, se puede apreciar la incorporación de dos condensadores. Uno de 0.33 µ F a la entrada del regulador y otro de 0.1 µ F a la salida. Ambos condensadores se emplean, según detalla el Datasheet del regulador, para evitar picos de tensión que dañe nuestros dispositivos. Figura 2.20 Esquema de conexión del Regulador de Tensión. Ha sido necesario la adición de un disipador de calor debido a las altas temperatura que alcanza el regulador. Sin dicho disipador, el regulador corre riesgo de quemar la PCB. A parte del disipador, se ha incorporado un ventilador axial que ayuda a disipar el calor que genera tanto el regulador como la RPi y el resto de componentes en su funcionamiento. 2.8 Chasis El chasis del vehículo es la estructura que engloba en su interior todos los componentes electrónicos. Para su diseño, hemos hecho uso del programa de diseño CAD 3D Autodesk Fusion 360 y el formato utilizado ha sido STL, el cual nos permite obtener el vehículo mediante impresión 3D. El material utilizado para la impresión ha sido PLA (ácido poliláctico). El vehículo consta de una estructura lisa, que será la base, dividida en 3 compartimentos conectados donde alojaremos la electrónica, la batería y los motores. La parte exterior de la base se ha diseñado con paredes de 3 mm de grosor, en su parte trasera se ha dotado de respiradores con los que se pretende evacuar el calor que generan los circuitos electrónicos. La distribución de los compartimentos será la siguiente: •El compartimento superior está destinado a alojar la batería. • El compartimento medio se ha destinado a alojar los componentes electrónicos. Se ha perforado la parte trasera para atornillar la RPi. En uno de sus laterales se han realizado una serie de perforaciones para tener acceso a la alimentación por cable y al puerto HDMI de la RPi, así como al puerto microUSB de Arduino Nano. • El compartimento inferior alojará los motores atornillados a su pared lateral. Se ha realizado un orificio que permite la salida del eje del motor para poder acoplarle las ruedas. Las ruedas utilizadas también se han impreso en 3D. Su diseño no se ha realizado en el presente trabajo, ya se disponía de él y ha sido proporcionado por el tutor del mismo. Para dotar de adherencia a la rueda, se ha recubierto de una junta tórica. 2.8 Chasis 31 (a) (b) Figura 2.21 (a) Vista frontal del vehículo. (b) Vista trasera del vehículo. . 32 Capítulo 2. Componentes 2.9 PCB Para las conexiones de los diferentes componentes electrónicos, se ha optado por el diseño y fabricación de una placa de circuito impreso de tipo shield. La PCB (Printed Circuit Board) se ha diseñado como escudo para la RPi. En ella agrupamos todos los componente prescindiendo así del uso de cables que puedan darnos problemas en un futuro, consiguiendo así mejores contactos y más fiables. Las conexiones se realizan por caminos o pistas de cobre. El grosor de las pistas dependerá de la corriente que circule por ellas, se ha establecido en 1 mm excepto las pistas de alimentación que, debido a la intensidad que circula por ellas, se ha diseñado de 1.34 mm. Para la elección del grosor anterior hacemos uso de la siguiente gráfica donde se muestra el aumento de temperatura que se produce en una pista a partir de la corriente que circula por ella y de la anchura de la misma, todo ello para placas con un recubrimiento estándar de 35 micras. Se puede apreciar que para una intensidad de 3.1 A, el incremento mínimo de temperatura que soporta la placa (10◦C). exige que la pista tenga un grosor de 1.35 mm. Figura 2.22 Elección del ancho de pista. Para el diseño se ha hecho uso del programa web EasyEda que nos permite crear un esquema de conexiones para posteriormente crear la placa. En la sección conexiones se muestra el esquema que compone la placa. Como hemos indicado, se ha diseñado como escudo para la RPi, sobre ella irán los siguientes elementos: •Arduino nano. •2 controladores de motores paso a paso EasyDriver. •MPU-6050. 2.10 Módulo bluetooth 33 •LED y resistencia que se encenderá cuando Arduino reciba alimentación. •Convertidor de voltaje lineal KA78T05 y 2 condensadores. Figura 2.23 Vista 3D de la PCB. (a) (b) Figura 2.24 (a) Vista capa superior PCB. (b) Vista capa inferior PCB . 2.10 Módulo bluetooth Con el fin de poder llevar a cabo un control remoto del vehículo, se ha optado por la incorporación de un módulo Bluetooth directamente conectado a Arduino, que nos permita el movimiento del vehículo por medio de una aplicación en nuestros teléfono móvil. Se ha hecho uso del módulo Bluetooth HC-06, en el apartado de programación se detallará su configuración y funcionamiento. Para la comunicación vía Bluetooth se hace uso de la App para Android Joystick BT commander, la cual nos permite controlar fácilmente el vehículo mediante el envío de comandos que serán proporciones a las coordenadas del joystick. Pudiendo realizar movimientos hacia delante y atrás y a ambos lados. El joystick disponible en la aplicación sigue una circunferencia de valores enteros que comprenden desde el 0 al 100. La conversión dependerá de los límites de nuestro sistema. Para el movimiento 34 Capítulo 2. Componentes hacia atrás o delante, el dato enviado será la referencia en velocidad, mientras que para los giros, el dato enviado será el de la variable que aumente/disminuya la velocidad de las ruedas, recordemos que ambas velocidades pueden ser diferentes. 2.11 Esquema de conexiones Una vez explicado todos los componentes que forman parte de nuestro proyecto, vamos a proceder a mostrar su esquema de conexiones. Figura 2.25 Esquema de conexiones. Los dispositivos que podemos apreciar en el esquema anterior, son: •Microcontrolador Arduino Uno. •Raspberry Pi 3 modelo B. •Dos EasyDriver v4.4 basado en el chip A3967. •Un módulo GY-521 (unidad de medición inercial MPU-6050). •Batería Li-Po de 11.1V •Dos Motores paso a paso NEMA-17. •Un regulador de tensión lineal KA75T05. •Un led. •Una resistencia de 220 ohmios. 2.12 Presupuesto 35 •Un condensador de 0.33 µF y otro de 0.1 µF. Nótese en el anterior esquema que no aparece el módulo bluetooth, ni el ventilador axial que ya se han comentado, esto es debido a que se añadieron posteriormente al diseño de la PCB. 2.12 Presupuesto Tabla 2.12 Presupuesto. Descripción Cantidad Precio unitario (€) Precio (€) Raspberry Pi 3B 1 37.43 37.43 Arduino Nano 1 4.66 4.66 Motor Nema 17 2 8.60 17.2 Controlador EasyDriver 2 3.90 7.80 GY-521 (MPU 6050) 1 2 2 PCB 1 1 1 Batería LIPO 1 14.50 14.50 Bluetooth HC 06 1 8 8 El precio total de fabricación del vehículo alcanza los 92.59 € . El resto de componentes mencionados en la memoria y que no aparecen en el presupuesto anterior, es debido a que su precio es irrelevante en comparación con el resto de componentes o bien porque se disponía de ellos antes del comienzo del proyecto. 3 Modelo La clave para obtener un control eficiente de nuestro sistema, radica en la la obtención de un modelo que refleje el comportamiento real del mismo. Reflejar el comportamiento del sistema real es una tarea ardua y complicada que puede llevar a fallos, nuestro objetivo no será ese. Buscaremos la obtención de un modelo simplificado pero igualmente válido. Por ello, en el presenta capítulo vamos a proceder al estudio del modelo físico-matemático que rige el comportamiento del vehículo. Para realizar el estudio vamos hacer uso de las ecuaciones energéticas, es decir, de la mecánica Lagrangiana. Para el estudio del modelo se ha tomado como referencia el trabajo de Fin de Grado de D º Nicolás Cortés Fernández, titulado Diseño, fabricación, montaje, estudio dinámico, control y teleoperación de un vehículo tipo péndulo invertido sobre dos ruedas [ 6 ]. En el citado trabajo, se puede encontrar una deducción de las ecuaciones utilizando la dinámica vectorial de Newton, llegando a la misma conclusión que la aquí presente. 3.1 Modelado del sistema Antes de establecer el modelo de control, tenemos que realizar un estudio del sistema. Como ya hemos mencionado, el sistema estará compuesto por un vehículo de dos ruedas basado en la configuración del péndulo invertido, cuyo eje de giro es coincidente con el eje de giro de ambas ruedas. Cada rueda se ha dotado de un motor independiente para dotar de capacidad de giro al vehículo. El sistema de control tratará de mantener en todo momento el vehículo en su posición vertical, que será su posición de equilibrio. Notese en el montaje final del vehículo, como su posición de equilibrio no estará completamente en su vertical, sino unos pocos grados desplazado, en concreto 3.5◦ . Esto es debido a la distribución de los componentes y su peso dispar en el interior del vehículo. Por todo ello, cuando hagamos referencia al ángulo de inclinación con respecto a la vertical ( φ ), estaremos haciendo referencia al ángulo de inclinación corregido(φc). φc=φ+φ0(3.1) Siendo φ0el ángulo de desviación con respecto a la vertical. 3.2 Suposiciones y parámetros del sistema A continuación mostraremos los parámetros del sistema que determinan su modelo. •mr: masa de cada rueda. •R: Radio de cada rueda. 37 44 Capítulo 3. Modelo constante viendo hasta donde llega el motor, el valor alcanzado ronda los 60 rad/s, estableciendo este dato como limite de velocidad.   −90 2π 360 −4 −60  ≤  φ ˙ φ ˙ θ ≤  90 2π 360 4 60    rad rad/s rad/s (3.36) El cálculo de la aceleración máxima que puede proporcionarse a los motores, se obtendrá experimentalmente, siendo un buen candidato ±90rad/s2 . La elección de este valor puede apreciarse en las gráficas de los controladores LQR y LQRI, donde se vio que estos valores eran suficientes y alcanzables. −90 ≤u≤90rad s2(3.37) 4 Control El sistema basado en el péndulo invertido es un sistema aparentemente inestable lo cual supone un gran reto para el control automático del mismo. A lo largo de los años, muchos han sido los métodos y variantes de control que se han utilizado para dotar de estabilidad a este tipo de experimentos. En el presente capítulo, se calcularán y explicarán las diferentes estrategias de control que, posteriormente, van a aplicarse al modelo real. Empezaremos el estudio obteniendo el modelo matemático en el espacio de estados, para seguir con un control LQR y un control LQR con adición de un término integral así como la implementación de un control MPC. Finalizaremos explicando el marco teórico de los sistema a implementar. Por lo tanto, los objetivos de este apartado serán: •Verificación de las ecuaciones del modelo calculadas en el apartado 3. •Análisis de las diferentes estrategias de control, evaluando la validez de cada una de ellas. • Implementación del control de estabilidad, resolviendo los problemas de orientación del mismo. 4.1 Variables de estado Una vez obtenida la ecuación que modela el sistema y su linealizado, podemos proceder al desarrollo de controladores lineales en variables de estado. Podemos definir las variables de estado como el conjunto de variables de un sistema cuyo conocimiento, en cualquier momento, permite conocer la evolución del sistema en un futuro. Las variables que determinan el estado en el que se encuentra nuestro sistema en todo momento, serán las siguientes: •φ: Ángulo de inclinación del vehículo en [rad]. •˙ φ: Velocidad angular del vehículo [rad/s]. •˙ θ: Velocidad angular de las ruedas[rad/s]. Las dos primeras variables serán fácilmente obtenibles, pues su dato puede tomarse directamente del dispositivo IMU, en el capítulo 5 "Software" veremos la forma de calcular la tercera variable. 4.1.1 Espacio de estados Conocidas las variables de estado y el modelo matemático del sistema, se va a proceder a obtener el modelo en espacio de estados. 45 46 Capítulo 4. Control El espacio de estados puede definirse como la representación, en ecuaciones diferenciales de primer orden, del modelo matemático mediante el conjunto de entradas, salidas y variables internas. Estas ecuaciones diferenciales de primer orden se combinan en una ecuación diferencial matricial. A tenor de la definición anterior, el objetivo será convertir la ecuación (3.35) en una ecuación diferencial en términos matriciales. El sistema ante el que estamos presente, es un sistema dinámico y continuo, en el que las variables evolucionan con el tiempo. Por ello, será necesario la adición de una serie de mecanismos que nos den información, en cada momento, del estado y la evolución del sistema. A modo de ejemplo, si en un determinado instante el vehículo se encuentra inclinado, para enderezarlo necesitaremos el ángulo de inclinación con la vertical medido por el sensor. Este dato será insuficiente para el correcto control del vehículo y aplicación de la acción de control, que como es sabido será la aceleración de las ruedas. Necesitaremos pues, conocer otros parámetros como la velocidad de las ruedas. En la necesidad de conocer este dato radica el concepto de "estado del sistema". Es decir, necesitamos conocer en que condiciones se encuentra el vehículo en el instante de estudio. Es esencial, representar la salida del sistema en función del estado y del valor de las entradas. ¨ θ=f(x(t),u(t)) (4.1) La ecuación matricial que representa el modelo en espacio de estados en forma continua adquiere la siguiente forma: ˙x(t) = Ax(t)+Bu(t)(4.2) y(t) = Cx(t)+Du(t)(4.3) Donde: •x: Vector de estados (Vector de dimensión n). •u: Vector de entradas (Vector de dimensión m). •y: Vector de salidas (Vector de dimensión n). •A: Matriz del sistema (Dimensión nxn). •B: Matriz de entrada (Dimensión nxm). •C: Matriz de salida. La ecuación (4.2) es la encargada de obtener el estado siguiente a partir del estado actual y de la variable de control. La ecuación (4.3) obtiene la salida deseada. En dicha ecuación, el término matricial D se puede obviar ya que si asumimos que la entrada no afecta a la salida al mismo tiempo, su valor se hace nulo. 4.1.2 Espacio de estados particularizado a nuestro sistema Particularizando la ecuación anterior para nuestro sistema, se tiene:   ˙ φ ¨ φ ¨ θ =A  φ ˙ φ ˙ θ +B¨ θ(4.4) El objetivo radica ahora en el cálculo de las matrices A y B, para ello vamos a agrupar la ecuación (3.35) para que adquiera la siguiente forma: a¨ φ+b¨ θ−c˙ φ=0(4.5) 4.1 Variables de estado 47 Siendo los valores de las constantes anteriores a,b y c: a=2ML2+MRL b= (3mr+M)R2+MRL c=MgL (4.6) Con los valores anteriores, obtenemos las matrices A y B que serán sustituidas en la ecuación (4.4) que junto a la ecuación (4.3) obtendremos la descripción del sistema en el espacio de estados, cabe recordar que la expresión siguiente se encuentra en forma continua:   ˙ φ ¨ φ ¨ θ =  0 1 0 c/a0 0 0 0 0   φ ˙ φ ˙ θ +  0 −b/a 1 ¨ θ(4.7) Como necesitamos trabajar en tiempo discreto, vamos a hacer uso del siguiente código en MATLAB. El código nos proporciona las matrices A y B del sistema en tiempo discreto, partiendo de los parámetros del sistema y sus matrices en modo continuo. Para el cálculo de los valores siguientes, se tiene que hacer un cambio del sistema en tiempo continuo al sistema en tiempo discreto. ˙x(t) = Ax(t)+Bu(t)→xk+1=Adxk+Bduk(4.8) t=Tm·k(4.9)   1clear ; 2clc ; 3 4%Parámetros del modelo en el SI 5 M = 0.975 ; 6 L = 0.05; 7 R = 0.05; 8 mr = 0.064; 9 Iyy = 2∗M∗L^2; 10 g = 9.81; 11 12 A = [0 1 0; (M∗g∗L/(Iyy + M∗R∗L)) 0 0; 0 0 0]; 13 B = [0; −(R^2∗(3∗mr+M)+M∗R∗L)/(Iyy + M∗R∗L); 1]; 14 C = eye(3) ; 15 D = zeros (3, 1); 16 17 %Creación del modelo continuo en el espacio de estados 18 Modelo_continuo=ss(A,B,C,D); 19 20 %Discretización del modelo anterior con un Tm de 20 ms 21 Modelo_discreto=c2d(Modelo_continuo,0.02); 22 23 %Representación de las matrices discretas 24 [Ad,Bd,Cd,Dd]=ssdata(Modelo_discreto);    48 Capítulo 4. Control Los valores obtenidos, en tiempo continuo, serán: A=  0 1 0 65.400 0 0 0  B=  0 −0.7283 1  (4.10) Mientras que en tiempo discreto adquieren: A=  1.0131 0.0201 0 1.3137 1.0131 0 0 0 1  B=  −0.0001 −0.0146 0.02   (4.11) 4.2 Formulación general del control LQR El controlador Linear Quadratic Regulator (por sus siglas en inglés, LQR) pretende conseguir un control óptimo a un mínimo coste. Por ello, esta estrategia de control busca la obtención de una matriz de realimentación K que minimice la función energética o de coste, la cual adquiere la siguiente forma: J=1 2xT(tf)PT x+1 2Ztf t0 (xTQx +uTRu)dt (4.12) La ecuación anterior, puede simplificarse si asumimos un tiempo final infinito ( tf7→∞ ), lo cual hace que el estado x(tf)se anule y la función de coste J quede como tal: J=Z∞ 0 (xTQx +uTRu)dt (4.13) La función J de la ecuación (4.12) está expresada en su término general y está asociada con la energía del sistema. El término Q de la ecuación anterior es la matriz de ponderación sobre el error en el estado, mientras que R es la ponderación sobre el error en la señal de control. Tanto Q como R son matrices semidefinidas positivas, de dimensión nxn la primera y mxm la segunda, donde n es el número de variables de estado y m el número de entradas. La ley de control seleccionada consistirá en una realimentación negativa del vector de estados. u(t) = −Kx(t)(4.14) El vector K es el vector de ganancias de ralimentación de estados. Sustituyendo el termino anterior en la ecuación (4.2) ˙x(t) = Ax(t)−BKx(t) = (A−BK)x(t)(4.15) En el resultado obtenido, observamos que el estado futuro del sistema depende tanto de la entrada como del estado del sistema en el momento actual. 4.2 Formulación general del control LQR 49 Como se ha indicado, el objetivo del control LQR será obtener la matriz de realimentación K que minimice el funcional J. Para ello, tenemos que resolver la ecuación: K=R−1B0P(4.16) Para el cálculo de la matriz P, que debe ser definida positiva, se tiene que hacer uso de la ecuación de Riccati. A0P+PA −PBR−1B0P+Q=0(4.17) . Una vez obtenida la matriz P, se debe sustituir en la ecuación (4.16) , consiguiendo así la obtención de la matriz de realimentación. Observando la ecuación de Riccatti, se aprecia que la obtención de la matriz P es sumamente complicada, dando lugar a cálculos largos y tediosos, propenso a fallos, donde aparecen matrices inversas. Para evitar los cálculos anteriores, se puede hacer uso de la función de Matlab "lqr" en tiempo continuo o "dlqr" si la representación del sistema es en tiempo discreto . La ley de control LQR debe aplicarse sobre modelos linealizados. Recordemos pues la ecuación (4.7), la cual mostraba el modelo del sistema linealizado entorno al punto de equilibrio en forma de espacio de estados. El vector x de dimensiones 3x1 hace referencia a las variables de estado. Por ello, la matriz K será de dimensiones 1x3, siendo el objetivo ahora, el cálculo de dichas 3 componentes. Se debe cumplir que la matriz de realimentación K minimice el valor del funcional J, es decir, se asegure que la energía del sistema se minimice al máximo haciendo que el sistema sea estable. Obtenida la matriz K por Matlab, y sustituida en la ecuación (4.14) obtenemos el vector de entrada que realimenta el sistema. u=−Kx =−K1K2K3  φ ˙ φ ˙ θ (4.18) 4.2.1 Formulación LQR en sistemas discretos Dado que tenemos que trabajar en un espacio discreto, tendremos que partir de la ecuación (4.13), la cual se encuentra representada en tiempo continuo y horizonte infinito, para obtener funcional J en tiempo discreto y horizonte finito: J= N ∑ k=0 [XT nQXn+UT nRUn](4.19) siendo, •Xn=xk−xe •Un=uk−ue Como es sabido, el control optimo busca minimizar la energía aportada por cada estado ( XT nQXn ). Siendo el objetivo, el cálculo de la ley de control discreta, uk=−K(Xn)+ue(4.20) El término xehace referencia al punto de equilibrio siendo, xe=Axe+Bue(4.21) 50 Capítulo 4. Control Aplicando el cambio de variable Zk+1=xk+1−xellegamos a las ecuaciones, Zk+1= (A−BK)Zk(4.22) J= N ∑ k=0 ZT k(A−BK)Zk(4.23) 4.2.2 Calculo del Vector de realimentación K Para el cálculo del vector de realimentación K se usará la función dlqr implementada en el programa Matlab. La función dlqr, recibe como argumento de entrada las matrices A, B y C, de la descripción del sistema en modo discreto, así como las matrices de ponderación Q y R. Para el cálculo de las matrices A, B y C se usarán los parámetros del modelo que se han obtenido en apartados anteriores. Posteriormente, se usará la función de matlab ss para crear el modelo en el espacio de estados, finalmente con la función c2d se discretizará el modelo de estado anterior. Q y R son matrices que han de definirse heurísticamente de acuerdo a una ponderación. Como ya sabemos, la matriz Q busca minimizar la energía empleada por las variables de estado, mientras que la matriz R busca minimizar la energía empleada por su entrada.   1clear ; 2clc ; 3 4%Parámetros del modelo en el SI 5 M2 = 0.975 + 0.064; 6 L = 0.05; 7 R = 0.05; 8 mr = 0.064; 9 Iyy = 2∗M2∗L^2; 10 g = 9.81; 11 12 %Matrices del sistema en modo continuo 13 A = [0 1 0; (M2∗g∗L/(Iyy + M2∗R∗L)) 0 0; 0 0 0]; 14 B = [0; −(R^2∗(3∗mr+M2)+M2∗R∗L)/(Iyy + M2∗R∗L); 1]; 15 C = eye(3) ; 16 D = zeros (3, 1); 17 18 %Creación del modelo continuo en el espacio de estados 19 Modelo_continuo=ss(A,B,C,D); 20 21 %Discretización del modelo anterior con un Tm de 20 ms 22 Modelo_discreto=c2d(Modelo_continuo,0.02); 23 24 %Representación de las matrices discretas 25 [Ad,Bd,Cd,Dd]=ssdata(Modelo_discreto); 26 27 %%obtención del vector de realimentaci ón K 28 Q=[10 0 0 ; 0 1 0 ; 0 0 10 ]; 29 R=0.1; 30 [k]=dlqr(Ad,Bd,Q,R)    Una vez calculado el vector K, podemos proceder a calcular los autovalores del sistema en bucle cerrado (A-BK), obteniendo: K=−337.7394 −41.8230 −7.6894(4.24) 4.2 Formulación general del control LQR 51 Autovalores =  −8.4018 −7.1035 +2.8725i −7.1035 −2.8725i (4.25) De los autovalores anteriores, obtenemos que el sistema cuenta con un polo rápido en, τ=1 8.4018 =0.1190 (4.26) y con dos polos lentos en , τ=1 7.1835 =0.1392 (4.27) Siendo menor que el tiempo de muestreo que hemos establecido en 20 ms, por lo que deducimos que el planteamiento es correcto. 4.2.3 Formulación LQR con efecto integral El control LQRI consiste en adicionar un término integral que corrija la perturbación causada cuando el centro de gravedad del robot no se encuentre en su centro geométrico. La explicación del porqué se adiciona este término, podemos encontrarla en [3] Para evitar la influencia de la perturbación, se introduce la siguiente ecuación que se encuentra representada en variables incrementales en tiempo discreto ∆xk+1=A∆xk+B∆uk+E∆wk(4.28) Al ser la perturbación constante, su variación es cero y por ello se anula el último término de la ecuación anterior, quedando: ∆xk+1=A∆xk+B∆uk(4.29) El error en tiempo discreto puede representarse como: ∆ek+1=A∆rk−∆xk+1(4.30) Si consideramos una referencia constante, el primer término de la ecuación anterior puede considerarse nulo y por tanto, el sistema adquiere la siguiente forma: ∆xk+1 ek=A0 −I I∆xk ek−1+B 0∆uk(4.31) Suponiendo que la acción de control es una ganancia, se tiene: ∆uk=−K∆xk ek−1=Kx∆xk+Ke∆ek−1(4.32) Y sustituyendo en (4.31): ∆xk+1 ek=A0 −I I+B 0K∆xk ek−1(4.33) Entonces obtenemos que: un=u0+Kx(xn−x0)+Ke n−1 ∑ i=0 ei(4.34) El nuevo vector K estará compuesto por 4 componentes, las tres primeras serán las calculas en el apartado anterior, la cuarta se obtendrá experimentalmente. La componente primera penaliza el 52 Capítulo 4. Control error de inclinación, la segunda la velocidad de inclinación, la tercera la velocidad de las ruedas y la cuarta el error acumulado en la velocidad de las ruedas. 4.3 Control Predictivo basado en el Modelo MPC El control predictivo basado en el modelo (MPC), es una técnica de control avanzado, en el que se establece el estado del sistema a partir de las referencias que se desean alcanzar en el instante actual más las referencias futuras. Es decir, es necesario disponer del conocimiento de la situación actual para la predicción. Se establece un intervalo de tiempo que abarca desde el tiempo actual hasta el tiempo actual más la suma de un horizonte de predicción. 4.3.1 Estructura general del modelo La formulación general del modelo asume que el sistema cuenta con m entradas, q salidas y n estados. Las ecuaciones del modelo en espacio de estados discreto es lineal, finito e invariable en el tiempo y adquieren la siguiente forma. x(k+1) = Ax(k+1)+Buk(4.35) y(k) = Cx(k)+Du(k)(4.36) En la ecuación anterior, se asume que la entrada no afecta a la salida al mismo tiempo y por ello, el término D=0. Por ello, la ecuación 4.36 queda: y(k) = Cx(k)(4.37) En las ecuaciones (4.35) y (4.36), A , B y C representan las matrices del espacio de estados en tiempo discreto, siendo x el vector de estados de dimensión n, y representa la salida del sistema y u la variable de entrada. Para tener información tanto del estado actual como del estado pasado, debemos trabajar con variables incrementales, de tal manera: ∆x=x(k)−x(k−1)(4.38) ∆u=u(k)−u(k−1)(4.39) Si relacionamos la salida y(k)con el vector de estados ∆xm(k)se tiene. ∆y(k+1) = C∆x(k+1) = CA∆x(k+1) +CB∆u(k+1)(4.40) Donde: ∆y(k+1) = y(k+1)−y(k)(4.41) Con todo ello, la nueva representación del vector de estado será; ∆x(k+1) y(k+1)=A0T CA I ∆x(k) y(k)+B CB∆u(k)(4.42) y(k) = 0I∆x(k) y(k)(4.43) 4.3 Control Predictivo basado en el Modelo MPC 53 Donde I es la matriz identidad de orden q, O es la matriz de ceros de dimensione qxn , A es una matriz cuadrada de orden n, Bes de dimensiones nxm y Cde q x n. 4.3.2 Predicción de las variables de estado y las salidas La trayectoria futura vendrá determinada por la ecuación ∆U= [∆u(ki),∆u(ki+1),...,∆u(ki+Nc−1)]T(4.44) Siendo las variables de estado futuras : X= [∆x(ki+1|ki),∆x(ki+2|ki),...,∆x(ki+Np|ki)]T(4.45) En las ecuaciones anteriores aparece el término Nc que hace referencia al horizonte de control, mientras que Np es el horizonte de predicción. Debe cumplirse siempre que el horizonte de control sea menor que el horizonte de predicción. Las variables de salida predichas serán: Y= [y(ki+1|ki),y(ki+2|ki),...,y(ki+Np|ki)]T(4.46) La variable de salida anterior estará relacionada con la trayectoria futura por la siguiente ecuación: Y=Fx(ki)+Φ∆U(4.47) Donde F es la matriz de observalidad: F=       CA CA2 CA3 . . . CANP        ;Φ=       CB 0 0 ··· 0 CAB CB 0··· 0 CA2B CAB CB ··· 0 . . .. . .. . .··· . . . CANp−1B CANp−2B CANp−3B··· CANp−NCB        (4.48) 4.3.3 Breve descripción del MPC para formulación de seguimiento En los apartados anteriores se presenta la formulación general del MPC para tener una idea de dicho control. En el presente apartado se va a describir el control MPC para seguimiento que se usará en el trabajo. Para más información se puede acudir a [7] Consideramos el sistema descrito por el modelo de espacio de estado lineal, discreto e invariante en el tiempo que se muestra en la ecuación (4.35). En dicha ecuación, xk∈Rn y uk∈Rm son el estado y la entrada del sistema de control en el tiempo de muestreo k respectivamente. Además consideramos que el sistema está sujeto a las restricciones: x≤xk≤x(4.49a) u≤uk≤u.(4.49b) El objetivo del control es dirigir el sistema a la referencia dada (xr,ur) mientras se satisfacen las restricciones del sistema. Esto solo es posible si la referencia dada es una referencia admisible del sistema, que definimos formalmente en la siguiente definición. Definición: La pareja (xa,ua)∈Rn∈Rm se dice que es admisible para el sistema (4.35) sujeto a (4.49) si (i) xa=Axa+Bua , es un estado estable del sistema (4.35), (ii) x≤xa≤x ,y (iii) u≤ua≤u . Si la referencia dada no es un estado admisible del sistema (4.35) sujeto a (4.49), entonces se dirigirá el sistema al estado estable admisible más próximo. 60 Capítulo 5. Software 15 TCCR1A=0; //Iniciamos en modo normal 16 TCCR1B=0; //Iniciamos en modo normal 17 switch (PREESCALA_1){ 18 case 1: // 4.095ms 19 SET(TCCR1B,CS10); 20 break; 21 case 8: // 32.7675ms 22 SET(TCCR1B,CS11); 23 break; 24 case 64: // 0.2621s 25 SETT(TCCR1B,CS11,CS10); 26 break; 27 case 256: // 1.048s 28 SETT(TCCR1B,WGM12,CS12); 29 break; 30 case 1024: // 4.1942S 31 SETT(TCCR1B,CS12,CS10); 32 break; 33 dir_M1=0; 34 dir_M2=0; 35 }; 36 OCR1A =65535; //Iniciamos en modo normal 37 OCR1B =65535; //Iniciamos en modo normal 38 TIMSK1 =0; // Limpia la máscara del timer 39 SETT(TIMSK1,OCIE1A,OCIE1B); //Activas las interrupciones A y B 40 // Configuración de las interrupciones del timer 2 41 TCCR2A=0; // Iniciamos en modo normal 42 TCCR2B=0; // Iniciamos en modo normal 43 TCNT2=0; 44 switch (PREESCALA_2){ 45 case 1: // 19.94us 46 SET(TCCR2B,CS20); 47 break; 48 case 8: // 127us 49 SET(TCCR2B,CS21); 50 break; 51 case 32: // 510us 52 SETT(TCCR2B,CS21,CS20); 53 break; 54 case 64: // 1.02ms 55 SET(TCCR2B,CS22); 56 break; 57 case 128: // 2.04ms 58 SETT(TCCR2B,CS20,CS22); 59 break; 60 case 256: // 4.08ms 61 SETT(TCCR2B,CS21,CS22); 62 break; 63 case 1024: // 16.32ms 64 SETTT(TCCR2B,CS20,CS21,CS22); 65 break; 66 }; 67 OCR2A=250; 68 SET(TCCR2A,WGM21); 69 SET(TIMSK2,OCIE2A); //Activa la interrupción A del timer 70 // // CONTINUA//// 71 } 5.1.3 Obtención de la aceleración Para la obtención de la aceleración se parte del código desarrollado en [ 4 ] con ligeras modificaciones. 5.1 Programación Arduino 61 Implementación de la velocidad Para implementar la velocidad, se hará uso de las interrupciones del Timer 1: 1 // // CONTINUA//// 2 // /////////////// INTERRUPCIÓN A DEL TIMER 1 PARA MOVER EL MOTOR 1/////////////////////// 3ISR(TIMER1_COMPA_vect) 4{ 5 // Activa la interrupci ón A del Timer para dar velocidad al motor 1 6if (dir_M1==0) // si el motor no se encuentra girando en ningún sentido (condición inicial ) , el motor no se moverá 7return ; 8SET(PORTD,STEP_MOTOR_1); //activa el pin STEEP 9OCR1A+=npuls_M1; // mantiene el pin en alto durante el tiempo que le inidica npuls_M1 10 CLR(PORTD,STEP_MOTOR_1); //desactiva el pin STEEP 11 } 12 // /////////////// INTERRUPCION B DEL TIMER 1 PARA MOVER EL MOTOR 2/////////////////////// 13 ISR(TIMER1_COMPB_vect) 14 { 15 if (dir_M2==0) 16 return ; 17 SET(PORTD,STEP_MOTOR_2); 18 OCR1B+=npuls_M2; 19 CLR(PORTD,STEP_MOTOR_2); 20 } 21 // // CONTINUA//// Actualización de la aceleración Para actualizar la velocidad e implementar la nueva aceleración se hará uso de las interrupciones del Timer2. El código siguiente actualiza la velocidad cada 4 ms, que es el tiempo con el que se ha programado las interrupciones para que salte el Timer 2, e implementa la nueva aceleración cada 20 ms. Arduino será el encargado de actualizar la velocidad en cada salto del timer 2, con el dato de la aceleración que vendrá determinado por la Rpi que actualizará dicho dato cada 20ms, es decir cada 5 saltos del Timer 2 se actualiza el valor de la aceleración. Aparece una bandera flag que se pone a cero cuando se recibe una nueva aceleración de forma que este dato recibido se procese una única vez. Es necesario introducir esta bandera porque recordemos que la velocidad se implementa cada 4 ms y la aceleración se actualiza cada 20 ms. Además, en la práctica se comprobó que de vez en cuando el dato que recibía de la RPi se desbordaba, dando un valor demasiado alto o un número no real. Por ello, en el caso de que el dato de la aceleración llegue corrupto, se tomará el dato anterior evitando así que el vehículo se descontrole. 1 // // CONTINUA//// 2ISR(TIMER2_COMPA_vect) 3{ 4interrupts () ; // Función que evita que el programa se colapse 5 6if ( flag==0) 7{ 8flag=1; // Se pone la bandera a 1 para trabajar con la aceleraci ón recibida y no volver a cambiarla hasta que se vuelve a recibir un nuevo dato. 9comprueba=isnan(alfa); // Comprueba que el número que se recibe es real , en caso contrario devuelve 1. 10 if ( alfa >150 || alfa <−150 || comprueba==1) // Filtro para obviar los valores de aceleraci ón recibidos muy grandes o no reales 11 { 12 alfa_M1=alfa_M1; //En caso de recibir un dato corrupto , mantengo la aceleraci ón anterior . 13 } 14 else 62 Capítulo 5. Software 15 { 16 alfa_M1=alfa; // En caso de recibir un dato dentro del rango, actualizo con la aceleraci ón recibida . 17 } 18 } 19 20 vel_M1=vel_M1+alfa_M1∗T_PULS_2+giro_M1∗0.05; //Actualizo la velocidad del motor 1 cada T_PULS_2 MS 21 vel_M2=vel_M2−alfa_M1∗T_PULS_2+giro_M1∗0.05; //Actualizo la velocidad del motor 1 cada T_PULS_2 MS 22 vel_M1=constrain(vel_M1 ,−60,60); // Saturo la velocidad entre los l ímites alcanzables 23 vel_M2=constrain(vel_M2 ,−60,60); // Saturo la velocidad entre los l ímites alcanzables 24 comprueba_vel_1(vel_M1); //Compruebo la direcci ón de giro del motor 1 con la nueva velocidad 25 comprueba_vel_2(vel_M2); //Compruebo la direcci ón de giro del motor 2 con la nueva velocidad 26 vel_M1−=giro_M1∗0.05; 27 vel_M2−=giro_M1∗0.05; 28 vec[0]=(vel_M1−vel_M2)/2.0; //Calculo el valor de la velocidad media de la rueda que se envía a la RPi 29 } 30 // // CONTINUA//// Comprobación de la dirección de giro Se muestra a continuación la función que comprueba el giro del motor 1 y actualiza el valor que debe estar en alto el pin STEP para implementar la velocidad. La función que comprueba la dirección del motor 2 es análoga a la siguiente y por ello no se muestra. 1 // // CONTINUA//// 2void comprueba_vel_1(float vel) 3{ 4if (vel >0) 5{ 6if (dir_M1 !=1) 7{ 8SET(PORTD,DIR_MOTOR_1); 9dir_M1=1; 10 } 11 if (vel<=omega_motor_max[3] && vel>=omega_motor_min[3]) 12 { 13 SETT(PORTB, MS1_MOTOR_1, MS2_MOTOR_1); 14 npuls_M1=omega2npuls[3]/vel; 15 } 16 else if (vel<=omega_motor_max[2]&& vel>=omega_motor_min[2]) 17 { 18 CLR(PORTB,MS1_MOTOR_1); 19 SET(PORTB,MS2_MOTOR_1); 20 npuls_M1=omega2npuls[2]/vel; 21 } 22 else if (vel<=omega_motor_max[1]&& vel>=omega_motor_min[1]) 23 { 24 CLR(PORTB,MS2_MOTOR_1); 25 SET(PORTB,MS1_MOTOR_1); 26 npuls_M1=omega2npuls[1]/vel; 27 } 28 else if (vel<=omega_motor_max[0]&& vel>=omega_motor_min[0]) 29 { 30 CLRR(PORTB,MS1_MOTOR_1,MS2_MOTOR_1); 31 npuls_M1=omega2npuls[0]/vel; 32 } 33 else 34 { 35 SETT(PORTB,MS1_MOTOR_1,MS2_MOTOR_1); 36 npuls_M1=npuls_max[3]; 37 } 5.2 Obtención del Ángulo 63 38 } 39 if (vel <0) 40 { 41 if (dir_M1 !=−1) 42 { 43 CLR(PORTD,DIR_MOTOR_1); 44 dir_M1=−1; 45 } 46 if (−vel<=omega_motor_max[3] && −vel>=omega_motor_min[3]) 47 { 48 SETT(PORTB, MS1_MOTOR_1, MS2_MOTOR_1); 49 npuls_M1=−omega2npuls[3]/vel; 50 } 51 else if (−vel<=omega_motor_max[2]&& −vel>=omega_motor_min[2]) 52 { 53 CLR(PORTB,MS1_MOTOR_1); 54 SET(PORTB,MS2_MOTOR_1); 55 npuls_M1=−omega2npuls[2]/vel; 56 } 57 else if (−vel<=omega_motor_max[1]&& −vel>=omega_motor_min[1]) 58 { 59 CLR(PORTB,MS2_MOTOR_1); 60 SET(PORTB,MS1_MOTOR_1); 61 npuls_M1=−omega2npuls[1]/vel; 62 } 63 else if (−vel<=omega_motor_max[0]&& −vel>=omega_motor_min[0]) 64 { 65 CLRR(PORTB,MS1_MOTOR_1,MS2_MOTOR_1); 66 npuls_M1=−omega2npuls[0]/vel; 67 } 68 else 69 { 70 SETT(PORTB,MS1_MOTOR_1,MS2_MOTOR_1); 71 npuls_M1=npuls_max[3]; 72 } 73 } 74 } 75 // // CONTINUA//// 5.2 Obtención del Ángulo Sin duda uno de los puntos claves del proyecto radica en la obtención del ángulo del vehículo con su vertical. Se pretende en todo momento que este ángulo de inclinación con respecto al eje vertical, que será su eje de equilibrio, sea cero. Ante cualquier perturbación el vehículo debe buscar esa posición. Como vimos en el apartado "2.5 MPU6050", el dispositivo encargado de controlar en todo momento el ángulo de inclinación será el dispositivo GY-521, el cual cuenta con un giroscopio y un acelerómetro ambos de 3 ejes. Para el cálculo de la acción de control será necesario el conocimiento del vector de estados. Dos de los parámetros que conforman el vector de estados del sistema van a ser obtenidos con este dispositivo. Seremos capaces de conocer tanto el ángulo de inclinación (componente φ del vector de estados) como la velocidad de caída del vehículo (componente ˙ φdel vector de estados). El mencionado dispositivo, irá conectado a la RPi y su comunicación con esta será a través del protocolo de comunicación I2C. El dispositivo cuenta con dos direcciones, la 0x68 y la 0x69. Por defecto la comunicación será a través de la primera dirección, sin embargo, si queremos comunicarnos por la segunda de ellas será necesario conectar el pin ADO a 5V. 64 Capítulo 5. Software Del funcionamiento del dispositivo, que se detalló en su apartado correspondiente, resumiremos los conocimientos que debemos de aplicar al código para la obtención del ángulo. Aunque el dispositivo cuenta con 6 grados de libertad, cabe recordar que en nuestro proyec- to solamente será necesario el uso de 3 de ellos. Dos componentes del acelerómetro y una del giroscopio. El dispositivo se encuentra acoplado a la PCB y girado 90 º con respecto al eje x. Por tanto, la orientación que siguen sus ejes se ve en el siguiente esquema, siendo el eje z el que se encuentra paralelo al suelo. Figura 5.2 Orientación de la IMU en la PCB. 5.2.1 Programación La RPi desde su lanzamiento en 2012 ha gozado de una gran popularidad y aceptación para el desarrollo de actividades robóticas. Presenta la posibilidad de programar en diferentes lenguajes como pueden ser, Python, C, C++, java, ADA... La multitud de lenguajes de programación disponibles, así como el poco tiempo en el mercado, hacen difícil encontrar bibliotecas específicas para controlar dispositivos externos en un lenguaje de programación específico. En el presente capítulo, se esta indagando en la necesidad de adicionar un dispositivo capaz de medir la inclinación del robot en cada momento. El lenguaje de programación por el que nos hemos decantado es C, ante la falta de una biblioteca en C para dicho dispositivo, estudiando su funcionamiento, se ha procedido a su creación. A continuación, se va a proceder a la explicación de las partes más importantes del código, el cual se encuentra incluido en el archivo adjunto MPU6050.h. •void inicio (int addr): Función que recibe como argumento de entrada la dirección I2C del dispositivo, que por defecto será 0x68. 1void inicio (int addr) 2{ 3int status; 4 5MPU6050_addr = addr; //guarda la dirección recibida en la variable MPU6050_addr 5.2 Obtención del Ángulo 65 6 7//abre la comuicacion i2c 8f_dev = open("/dev/i2c-1", O_RDWR); 9if (f_dev < 0) { 10 printf("error abriendo la conexión"); 11 } 12 status = ioctl(f_dev, I2C_SLAVE, MPU6050_addr); 13 if (status < 0) { 14 printf("error"); 15 } 16 17 i2c_smbus_write_byte_data(f_dev, 0x6b, 0b00000000); // desabilita el modo sleep del dispositivo 18 19 i2c_smbus_write_byte_data(f_dev, 0x1a, 0b00000011); //ajusta un filtro paso bajo a 44Hz 20 21 i2c_smbus_write_byte_data(f_dev, 0x19, 0b00000100); //Establece el divisor de frecuencia de muestreo 22 23 i2c_smbus_write_byte_data(f_dev, 0x1b, GYRO_CONFIG); // configura los agustes del giroscopio 24 25 i2c_smbus_write_byte_data(f_dev, 0x1c, ACCEL_CONFIG); // configura los ajustes del acelerómetro 26 27 28 i2c_smbus_write_byte_data(f_dev, 0x06, 0b00000000), i2c_smbus_write_byte_data(f_dev, 0x07, 0b00000000), i2c_smbus_write_byte_data(f_dev, 0x08, 0b00000000), i2c_smbus_write_byte_data(f_dev, 0x09, 0b00000000), i2c_smbus_write_byte_data(f_dev, 0x0A, 0b00000000), i2c_smbus_write_byte_data(f_dev, 0x0B, 0b00000000), i2c_smbus_write_byte_data(f_dev, 0x00, 0b10000001), i2c_smbus_write_byte_data(f_dev, 0x01, 0b00000001), i2c_smbus_write_byte_data(f_dev, 0x02, 0b10000001); 29 30 } •Void getAccelRaw(float *x, float y*, float *z) : Función que devuelve los valores en bruto del acelerómetro. Recibe como argumento de entrada un puntero a los valores que devuelve. Tiene un rango de medición comprendido entre -32768 y +32767. 1//obtiene los valores en bruto del acelerómetro 2void getAccelRaw(float *x, float *y, float *z) 3{ 4int16_t X = i2c_smbus_read_byte_data(f_dev, 0x3b) << 8 | i2c_smbus_read_byte_data(f_dev, 0x3c); //Lee el registro X de la aceleración 5int16_t Y = i2c_smbus_read_byte_data(f_dev, 0x3d) << 8 | i2c_smbus_read_byte_data(f_dev, 0x3e); //Lee el registro Y de la aceleración 6int16_t Z = i2c_smbus_read_byte_data(f_dev, 0x3f) << 8 | i2c_smbus_read_byte_data(f_dev, 0x40); //Lee el registro Z de la aceleración 7*x=(float)X; 66 Capítulo 5. Software 8*y=(float)Y; 9*z=(float)Z; 10 } •Void getGyroRaw (float *roll, float *pitch, float *yaw): Función que devuelve los valores en bruto del giroscopio. Recibe como argumento de entrada un puntero a los valores que devuelve. Tiene un rango de medición comprendido entre -32768 y +32767. 1//obtiene los valores en bruto del giroscopio 2void getGyroRaw(float *roll, float *pitch, float *yaw) 3{ 4int16_t X = i2c_smbus_read_byte_data(f_dev, 0x43) << 8 | i2c_smbus_read_byte_data(f_dev, 0x44); //Lee el registro X del giroscopio 5int16_t Y = i2c_smbus_read_byte_data(f_dev, 0x45) << 8 | i2c_smbus_read_byte_data(f_dev, 0x46); //Lee el registro Y del giroscopio 6int16_t Z = i2c_smbus_read_byte_data(f_dev, 0x47) << 8 | i2c_smbus_read_byte_data(f_dev, 0x48); //Lee el registro Z del giroscopio 7*roll = (float)X; 8*pitch = (float)Y; 9*yaw = (float)Z; 10 11 } •Void getAccel(float *x, float y*, float *z) : Función que devuelve los valores en g del acelerómetro con 3 cifras decimales. Recibe como argumento de entrada un puntero a los valores que devuelve. Dicha función primero calcula los valores brutos con getAccelRaw y posteriormente aplica el factor de sensibilidad. 1//obtiene los valores del acelerómetro en g con 3 cifras decimales 2void getAccel(float *x, float *y, float *z) 3{ 4getAccelRaw(x, y, z); 5*x = round((*x - A_OFF_X) *1000.0 / ACCEL_SENS) / 1000.0; 6*y = round((*y - A_OFF_Y) *1000.0 / ACCEL_SENS) / 1000.0; 7*z = round((*z - A_OFF_Z) *1000.0 / ACCEL_SENS) / 1000.0; 8} •Void getGyro (float *roll, float *pitch, float *yaw) : Función que devuelve los valores en g/s del giroscopio con 3 cifras decimales. Recibe como argumento de entrada un puntero a los valores que devuelve. Dicha función primero calcula los valores brutos con getGyroRaw y posteriormente aplica el factor de sensibilidad. 1//obtiene los valores del giroscopio en g/s con 3 cifras decimales 2void getGyro(float *roll, float *pitch, float *yaw) 3{ 4getGyroRaw(roll, pitch, yaw); 5*roll = round((*roll - G_OFF_X) *1000.0 / GYRO_SENS) / 1000.0; 6*pitch = round((*pitch - G_OFF_Y) *1000.0 / GYRO_SENS) / 1000.0; 5.2 Obtención del Ángulo 67 7*yaw = round((*yaw - G_OFF_Z) *1000.0 / GYRO_SENS) / 1000.0; 8} •void getOffsets(float *ax_off, float *ay_off, float *az_off, float *gr_off, float *gp_off, float *gy_off) : Función que calcula los valores del offsets que posteriormente tendremos que añadir al mismo fichero “MPU6050.h”. Para calcular el offset se realiza un bucle for de 10.000 iteraciones calculando los datos en bruto. Finalmente realiza la media de los datos leídos. 1//calcula el offset 2void getOffsets(float *ax_off, float *ay_off, float *az_off, float *gr_off, float *gp_off, float *gy_off) 3{ 4float gyro_off[3]; 5float accel_off[3]; 6float suma_x=0, suma_y=0, suma_z=0, suma_gx=0, suma_gy=0, suma_gz=0; 7 8 9for (int i = 0; i < 10000; i++) { 10 getGyroRaw(&gyro_off[0], &gyro_off[1], &gyro_off[2]); // 11 suma_gx=suma_gx+ gyro_off[0], suma_gy=suma_gy+ gyro_off[1], suma_gz=suma_gz+ gyro_off[2]; 12 13 getAccelRaw(&accel_off[0], &accel_off[1], &accel_off[2]); 14 suma_x=suma_x+accel_off[0], suma_y=suma_y+accel_off[1], suma_z=suma_z+ accel_off[2]; 15 } 16 17 *gr_off = suma_gx / 10000, *gp_off = suma_gy / 10000, *gy_off = suma_gz / 10000; 18 *ax_off = suma_x / 10000, *ay_off = suma_y / 10000, *az_off = suma_z / 10000; 19 20 *az_off = *az_off - ACCEL_SENS; / 21 } Para el cálculo del offset, se programó una función a parte que, tras su ejecución, nos dio los siguientes valores. •A_OFF_X =8977.75 •A_OFF_Y =1870.6 •A_OFF_Z =-16697.05 •G_OFF_X =-3.55 •G_OFF_Y =0.65 •G_OFF_Z =3.7 68 Capítulo 5. Software 5.2.2 Cálculo del ángulo Para la obtención del ángulo por medio del acelerómetro, tendremos que utilizar la siguiente ecuación trigonométrica, donde ax, ay y az son los valores de la aceleración medidos en dichos ejes: Angulo =atan az pay2+ax2!(5.1) Como se vio en su apartado correspondiente, el valor que se obtiene no es del todo fiable pues introduce un exceso de ruido en la medida lo cual hace inviable la utilización de este dato para el cálculo del ángulo. El cálculo del ángulo mediante el uso del giroscopio puede calcularse como: Angulo =Anguloanterior +(Giroscopio ∗4t)(5.2) Por lo general, las medidas obtenidas por el giroscopio suelen ser más precisas que las calculadas por el acelerómetro, en un corto periodo de tiempo. Sin embargo, en un largo periodo de tiempo, al calcularse el dato como una suma de incrementos, hará que se aleje de la realidad debido a una suma de los errores acumulados en la medida. Por todo ello, es necesario la adición de un filtro complementario, cuya explicación puede verse en el apartado "2.5.7 Filtro Complementario", que compense los errores citados y nos proporcione un valor fiable. Por tanto, la idea básica consiste en combinar ambas salidas ponderando el peso de cada una ellas. Se ha elegido un valor de τ=0.99 . La función que se encarga de calcular dichos valores será: 1 2void calculo_angulo(void) 3{ 4getGyro(&gx_escalado, &gy_escalado, &gz_escalado); 5getAccel(&ax_escalado, &ay_escalado, &az_escalado); //lee valores en el sistema internacional g 6accel_ang = atan2(az_escalado , (sqrt((ax_escalado*ax_escalado) + (ay_escalado*ay_escalado))))*(180/3.1415); //Cálculo del á ngulo por el acelerómetro. Datos en grados. 7giros_ang = filtro (accel_ang, gy_escalado); 8 9} En el código anterior se puede apreciar que aparece la función filtro() , la cuál se encarga de calcular el filtro complementario y el tiempo que transcurre entre cada medida, medido en ms. Como argumento de entrada recibirá el ángulo medido por el acelerómetro así como la velocidad angular escalada del eje y. 1 2float filtro(float accel, float gxy) 3{ 4dtt=(double)(clock()-iniciall)/CLOCKS_PER_SEC; 5giros_ang_y_prev=TAU*(giros_ang_y_prev+dtt*gxy)+(1-TAU)*accel; 6iniciall=clock(); 7return (giros_ang_y_prev); 8} 5.3 Programación RPi 69 En la siguiente gráficas podemos apreciar la necesidad del uso del filtro complementario. Se realiza una comparativa del ángulo obtenido con el acelerómetro (gráfica azul) donde se aprecia el excesivo ruido, y el ángulo calculado aplicando el filtro (gráfica roja) donde se aprecia que las medidas son más suaves y por lo tanto de mayor validez. 012345678910 Tiempo (s) -100 -80 -60 -40 -20 0 20 40 60 80 100 Ángulo (º) Comparativa acelerómetro-filtro complementario acelerómetro filtro complementario Figura 5.3 Comparativa del ángulo calculado mediante el acelerómetro y el filtro complementario. 5.3 Programación RPi En este punto de capítulo, ya conocemos como obtener las tres componentes que forman el vector de estados. Mediante Arduino conocemos la velocidad de las ruedas ( ˙ θ ), y mediante la IMU conocemos el ángulo de inclinación ( φ ) y la velocidad del vehículo ( ˙ φ ). Es necesario conocer en este instante como calcular la señal de control, que será la aceleración de las ruedas, y como comunicarnos con Arduino para el envío de dicha señal de control. La adición de la RPi a este proyecto abre un nuevo mundo de posibilidades, quedando atrás las limitaciones temporales de Arduino en cuanto a cálculos complejos. La frecuencia del procesador que incorpora la RPi es de 1.2 GHz, siendo sumamente superior a Arduino (16MHz), dando la posibilidad de programar todo tipo de controladores, abriendo numerosas líneas de investigación para trabajas futuros. Dentro de la cantidad de lenguajes con los que podemos trabajar en la RPi, se ha optado por C. La RPi se encuentra especialmente diseñada para trabajar con python. Sin embargo, phyton es un lenguaje interpretado a diferencia de C que es compilado. Los costes computacionales de python son muy superiores a los alcanzados con C y como nuestro sistema requiere de una respuesta rápida, especialmente en el control MPC, no queda garantizado que se puedan llevar a cabo las acciones. A continuación se va a proceder a explicar la parte común de código que comparten todos los controladores, para posteriormente centrarnos en la parte que difiere. La parte que difiere se 76 Capítulo 5. Software envía, de tal manera que cada vez que se recibe un dato, en el siguiente envío Arduino mande el componente cero del vector de envío. El vector de envío está formado por dos componentes, el primero es la velocidad de la ruedas mientras que el segundo es la referencia de velocidad de las ruedas que proporciona el joystick. La segunda variable entera f lag se pone a cero para que cuando salte la interrupción del timer 2, se actualice el valor de la aceleración que se aplica a las ruedas. 1void dato_recibido ( int howMany) { 2interrupts () ; 3volatile float fnum; 4volatile byte ∗p = (byte∗) &fnum; // CONVIERTE EL VALOR DE UN FLOTANTE EN BYTES 5while(Wire. available () ) // BUCLE PARA LEER MIENTRAS QUEDEN DATOS DISPONIBLES 6{ 7for (int i = 0; i < 4; i++) 8∗p++=Wire.read(); // GUARDA EL VALOR LEÍDO EN EL PUNTERO A BYTE 9} 10 alfa =( float )fnum; // GUARDA EN ALFA EL VALOR RECIBIDO COMO FLOTANTE 11 j=0; // PONE A 0 EL VALOR DEL VECTOR DE ENVÍO PARA QUE CUANDO LA RPI PIDA UN VALOR SE LE ENVÍE PRIMERO LA VELOCIDAD DE LOS MOTORES 12 flag=0; // PONE LA BANDERA A CERO PARA QUE SE PROCESE EL DATO RECIBIDO EN LAS INTERRUPCIONES DEL TIMER 2, ACTUALIZÁNDOSE EL VALOR DE LA ACELERACI ÓN A APLICAR. 13 } • Función dato_enviado() , envía el dato que contiene el vector de envío cuando la Rpi realiza una petición de lectura. El envío se realiza mediante la función Wire.write() . Observese que se envía un dato en bytes con el tamaño de un flotante. El primer dato enviado será la velocidad de las ruedas, una vez enviado el dato, se actualiza el índice del vector de envío, para que en la siguiente petición se envíe el siguiente componente que será la referencia de velocidad. La sincronización entre todos los códigos hace que el entero j adquiera únicamente el valor 0 ó 1. 1void dato_enviado() { 2 // VECTOR ENVÍO, PRIMERO SE ENVÍA LA COMPONENTE 0 QUE EQUIVALE A LA VELOCIDAD DE LOS MOTORES 3Wire.write (( byte∗) &vel, sizeof (float )) ; 4j++;//INCREMENTA EL VALOR DE J PARA QUE EN LA SIGUIENTE PETICIÓN DE DATO SE ENVÍE LA COMPONENTE 1 QUE EQUIVALE A LA REFERENCIA EN VELOCIDAD. 5} 5.5 Comunicación Bluetooth Para la comunicación Bluetooth se hace uso del módulo HC-06 que irá conectado a Arduino. Los datos que recibe serán enviados desde la App Android Joystick BT Commander. Lo primero que tendremos que hacer es configurar el módulo Bluetooth mediante los comandos AT de forma manual. 1#include <SoftwareSerial .h> 2SoftwareSerial Bluetooth (10,11) ; 3 4void setup () 5{ 6BT.begin(9600); // Velocidad del puerto del módulo Bluetooth 5.5 Comunicación Bluetooth 77 7Serial .begin(9600); // Abrimos la comunicación serie con el PC y establecemos velocidad 8} 9 10 void loop() 11 { 12 if (Bluetooth . available () ) 13 { 14 Serial . write(Bluetooth .read () ) ; 15 } 16 if (Serial .available () ) 17 { 18 Bluetooth. write(Serial .read () ) ; 19 } 20 } • Tecleando "AT+NAMEXX", cambiaremos el nombre del módulo que pasará a llamarse XX. El nombre estará compuesto como máximo por 20 caracteres. •Tecleando "AT+PINXXXX", cambiaremos el pin del módulo que pasará a ser XXXX. •Tecleando "AT+BAUDX", cambiaremos la velocidad del módulo qué pueder ser: Tabla 5.1 Velocidades módulo HC-06. X velocidad (baudios) 1 1200 2 2400 3 4800 4 9600 5 19200 6 38400 7 57600 8 115200 9 230400 A 460800 B 921600 C 1382400 El código que interpreta los datos que envía la APP será el siguiente: 1void blue() 2{ 3if ( bluetooth . available () ) { 4delay(2) ; 5cmd[0] = mySerial.read () ; 6if (cmd[0] == STX) { 7int i=1; 8while( bluetooth . available () ) { 9delay(1) ; 10 cmd[i] = bluetooth .read() ; 11 if (cmd[i]>127 || i>7) break; 12 if ((cmd[i]==ETX) && (i==2 || i==7)) break; 13 i++; 14 } 78 Capítulo 5. Software 15 if (i==7) getJoystickState (cmd); 16 } 17 } 18 } 19 20 21 void getJoystickState (byte data [8]) { 22 int joyX = (data[1]−48)∗100 + (data[2]−48)∗10 + (data[3]−48); 23 int joyY = (data[4]−48)∗100 + (data[5]−48)∗10 + (data[6]−48); 24 joyX = joyX −200; 25 joyY = joyY −200; 26 if (joyX<−100 || joyX>100 || joyY<−100 || joyY>100) return ; 27 giro_M1=joyX; 28 vec[1]=joyY∗0.4; 29 } Se aprecia que los valores del eje X son los que determinarán el giro, mientras que los valores del eje Y serán los que nos varíen la referencia de velocidad. El joystick puede considerarse una circunferencia de radio 100, de tal manera que se escala la velocidad multiplicando el valor recibido por 0.4. Así, la referencia de velocidad máxima que vamos a obtener en el control remoto será de +-40 rad/s. 6 Resultados Llegados a este punto de la memoria, ya tenemos diseñado los controladores a aplicar así como la explicación de los códigos involucrados. Se mostrarán a continuación los resultados obtenidos mediante los diferentes métodos así como las condiciones en las que se han llevado a cabo los experimentos. Como nota, indicaremos que se mostrarán 3 gráficas por experimento. La primera de ella contendrá conjuntamente las 3 variables de estado (ángulo de inclinación del vehículo con respecto al plano vertical, velocidad de caída del vehículo y velocidad angular de las ruedas) así como la acción de control (aceleración angular de las ruedas). La segunda gráfica mostrará el ángulo de inclinación del vehículo de la gráfica primera por separado para apreciar con mayor claridad sus valores. La tercera gráfica será análoga a la segunda pero mostrando la velocidad angular de las ruedas. 6.1 Control LQR 6.1.1 Seguimiento de referencia nula en velocidad de las ruedas Para este experimento, se ha dejado al vehículo quieto sobre un plano horizontal durante 50 segundos. Se aprecia como aparecen pequeñas variaciones del ángulo no superiores a ningún momento a 2◦ . Este pequeño ruido que puede apreciarse es debido a pequeños errores en la medida del MPU-6050. La amplitud de las pequeñas ondas es similar a las obtenidad con el control LQRI, pero mayores a las que se obtienen con el control MPC que estabiliza mejor en referencias nulas de velocidad. 79 80 Capítulo 6. Resultados 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -20 -10 0 10 20 Ángulo (º) Ángulo 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -20 -10 0 10 20 omega (rad/s) Velocidad Angular Rueda A 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -2 -1 0 1 2 dphi(rad/s) velocidad caída del vehículo 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -50 0 50 alfa (rad/s2) alfa Figura 6.1 LQR: Variables ante ensayo de seguimiento de referencia nula en velocidad. 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -20 -15 -10 -5 0 5 10 15 20 Ángulo (º) Ángulo Figura 6.2 LQR: Ángulo ante ensayo de seguimiento de referencia nula en velocidad. 6.1 Control LQR 81 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -20 -15 -10 -5 0 5 10 15 20 omega (rad/s) Velocidad Angular Ruedas Figura 6.3 LQR: Velocidad angular de las ruedas ante ensayo de seguimiento de referencia nula en velocidad. 6.1.2 Perturbaciones tipo pulso En este experimento, colocamos el vehículo sobre un plano horizontal con un seguimiento en referencias de velocidad nula y cada cierto tiempo se dieron pequeños pulsos. El experimento dura 40 segundos, el primer pulso se da poco antes de los 5s , el segundo pulso pasados los 10s y el último pulso pasados los 25s. Se aprecia que el vehículo se inclina sobre −15◦ y vuelve a recuperar su posición de equilibrio. El funcionamiento es simple, al darle el pequeño golpe seco, el vehículo se inclina y tiende a acelerar en la dirección hacia donde se esta cayendo, aumentando la velocidad de las ruedas, momento en el que empieza a actuar el sistema de control que se opondrá a esta caída, cambiando el sentido de la velocidad y volviendo a su posición de equilibrio. 82 Capítulo 6. Resultados 0 5 10 15 20 25 30 35 40 Tiempo (s) -20 0 20 Ángulo (º) Ángulo 0 5 10 15 20 25 30 35 40 Tiempo (s) -20 0 20 omega (rad/s) Velocidad Angular Rueda A 0 5 10 15 20 25 30 35 40 Tiempo (s) -2 0 2 dphi(rad/s) velocidad caída del vehículo 0 5 10 15 20 25 30 35 40 Tiempo (s) -100 0 100 alfa (rad/s2) alfa Figura 6.4 LQR: Variables ante ensayo de perturbaciones tipo pulso. 0 5 10 15 20 25 30 35 40 Tiempo (s) -30 -20 -10 0 10 20 30 Ángulo (º) Ángulo Figura 6.5 LQR: Ángulo ante ensayo de perturbaciones tipo pulso. 6.1 Control LQR 83 0 5 10 15 20 25 30 35 40 Tiempo (s) -30 -20 -10 0 10 20 30 omega (rad/s) Velocidad Angular Ruedas Figura 6.6 LQR: Velocidad angular de las ruedas ante ensayo de perturbaciones tipo pulso. 6.1.3 Seguimiento ante cambios de referencia de velocidad de las ruedas En este ensayo se ha producido un cambio en la referencia de velocidad de las ruedas. Este cambio en las referencias de velocidad se produce cada 2s y es de -10 rad/s descendente hasta llegar a -20 rad/s, y de -5 rad/s desde -20 rad/s hasta llegar a -30 rad/s. Se puede apreciar un cambio brusco en el ángulo al cambiar la referencia de velocidad, en cada cambio de referencia el vehículo llega a inclinarse en torno a 18◦ , es por ello que se decide que el cambio de referencia sea el indicado, pues para cambios mayores el vehículo alcanzaba tal ángulo que no era capaz de compensar el ángulo que alcanzaba y por ello caía. Este problema que se verá solucionado en los dos siguientes controladores. Se puede apreciar que se produce un error en régimen permanente, pues la velocidad a la que llegarán las ruedas no alcanza la referencia deseada. Problema que será solucionado con los controladores venideros. 84 Capítulo 6. Resultados 012345678910 Tiempo (s) -20 0 20 Ángulo (º) Ángulo 012345678910 Tiempo (s) -40 -20 0 20 40 omega (rad/s) Velocidad Angular Rueda A 012345678910 Tiempo (s) -4 -2 0 2 4 dphi(rad/s) velocidad caída del vehículo 012345678910 Tiempo (s) -100 0 100 alfa (rad/s2) alfa Figura 6.7 LQR: Variables ante ensayo de seguimiento de referencia cambiante en velocidad. 012345678910 Tiempo (s) -30 -20 -10 0 10 20 30 Ángulo (º) Ángulo Figura 6.8 LQR: Ángulo ante ensayo de seguimiento de referencia cambiante en velocidad. 6.1 Control LQR 85 012345678910 Tiempo (s) -40 -30 -20 -10 0 10 20 30 40 omega (rad/s) Velocidad Angular Ruedas Figura 6.9 LQR: Velocidad angular de las ruedas ante ensayo de seguimiento de referencia cambiante en velocidad. 6.1.4 Ascenso y Descenso en plano inclinado Este ensayo consistió en forzar al vehículo, aplicándole una fuerza con la mano, para subir un plano inclinado de 20◦ , una vez suba la rampa se estabilizara durante unos cuantos segundos y se lanzará por ella. En la subida, el vehículo se inclina hacia donde se le aplica la fuerza, es decir, hacia abajo de la cuesta, para compensar esta perturbación que será superior a la fuerza peso que aplica la gravedad. Al caer, el vehículo se inclinará hacia atrás, hacia el alto de la cuesta, detectando una variación en su ángulo de inclinación que será corregido por el sistema de control para volver a la referencia nula de sus variables. El funcionamiento es similar a la perturbación tipo pulso explicada en la sección anterior. Nótese como en la caída, el ángulo inclinación alcanza valores cercano a los 30◦ y aceleraciones de hasta 100 rad/s2 . Este ensayo dura 60 segundos y se realizan 5 descensos por el plano inclinado. Se parte del vehículo en lo alto de la rampa y a los 6 segundos se deja caer viéndose como el vehículo se inclina ( picos hacia arriba en figura 6.11) y la velocidad de las ruedas aumentan hasta que el sistema de control lo estabiliza. Sobre el segundo 12 se empieza aplicar durante 2 segundos un empuje por nuestra parte (mesetas hacia abajo en figura 6.11) para que el vehículo suba la rampa. 92 Capítulo 6. Resultados 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -20 0 20 Ángulo (º) Ángulo 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -40 -20 0 20 40 omega (rad/s) Velocidad Angular Rueda A 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -2 0 2 dphi(rad/s) velocidad caída del vehículo 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -100 0 100 alfa (rad/s2) alfa Figura 6.19 LQRI: Variables ante ensayo de perturbaciones tipo pulso. 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -30 -20 -10 0 10 20 30 Ángulo (º) Ángulo Figura 6.20 LQRI: Ángulo ante ensayo de perturbaciones tipo pulso. 6.2 Control LQR con efecto integral 93 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -40 -30 -20 -10 0 10 20 30 40 omega (rad/s) Velocidad Angular Ruedas Figura 6.21 LQRI: Velocidad angular de las ruedas ante ensayo de perturbaciones tipo pulso. 6.2.3 Seguimiento ante cambios de referencia de velocidad de las ruedas En este ensayo se ha producido un cambio en la referencia de velocidad de las ruedas. Este cambio en las referencias de velocidad se produce cada 4s y es de 10 rad/s ascendente y -10rad/s descendente. Se parte de una velocidad nula hasta llegar a 20 rad/s seguidamente se cambia el sentido de movimiento hasta llegar a los -20 rad/s y volver a 0 rad/s, así repetidas veces. El vehículo es capaz de alcanza mayores velocidades de las que aquí se muestran, sin embargo, se decide incluir esta gráfica para que se aprecie el movimiento de ida y vuelta del vehículo. Aunque se aprecia un cambio algo más suave en la variación del ángulo al cambiar la referencia de velocidad, sigue siendo más brusco que con el control MPC. En cada cambio de referencia el vehículo llega a inclinarse en torno a 15◦ , se decide mantener el cambio de referencia en +-10 rad/s, aunque en este caso nos permitía cambios mayores sin llegar a caerse. Se corrige el error en régimen permanente del control anterior, pues la velocidad a la que llegan las ruedas alcanza la referencia indicada. 94 Capítulo 6. Resultados 0 5 10 15 20 25 30 35 Tiempo (s) -20 0 20 Ángulo (º) Ángulo 0 5 10 15 20 25 30 35 Tiempo (s) -40 -20 0 20 40 omega (rad/s) Velocidad Angular Rueda A 0 5 10 15 20 25 30 35 Tiempo (s) -2 0 2 dphi(rad/s) velocidad caída del vehículo 0 5 10 15 20 25 30 35 Tiempo (s) -100 0 100 alfa (rad/s2) alfa Figura 6.22 LQRI: Variables ante ensayo de seguimiento de referencia cambiante en velocidad. 0 5 10 15 20 25 30 35 Tiempo (s) -30 -20 -10 0 10 20 30 Ángulo (º) Ángulo Figura 6.23 LQRI: Ángulo ante ensayo de seguimiento de referencia cambiante en velocidad. 6.2 Control LQR con efecto integral 95 0 5 10 15 20 25 30 35 Tiempo (s) -40 -30 -20 -10 0 10 20 30 40 omega (rad/s) Velocidad Angular Ruedas Figura 6.24 LQRI: Velocidad angular de las ruedas ante ensayo de seguimiento de referencia cambiante en velocidad. 6.2.4 Fuerza creciente en parte superior del vehículo Este ensayo consiste en aplicar una fuerza creciente, en forma de empuje, sobre una cara del vehículo durante 4 segundos. Puede apreciarse que el vehículo se inclina hasta llegar a los 25◦ , momento en el cuál dejamos de aplicar la fuerza. Una vez que se deja de aplicar la fuerza, el sistema de control detecta que el vehículo no se encuentra en la referencia deseada y actuará la señal de control (aceleración angular de las ruedas) en sentido contrario a la fuerza aplicada para variar esta perturbación y volver a su posición de equilibrio. El control LQRI presenta la adición del término integral que se encargará de integrar el error en velocidad. Por ello, una vez deje de aplicarse la fuerza, el vehículo volverá a la posición donde se empezó a aplicar dicha fuerza. 96 Capítulo 6. Resultados 0 2 4 6 8 10 12 Tiempo (s) -20 0 20 Ángulo (º) Ángulo 0 2 4 6 8 10 12 Tiempo (s) -40 -20 0 20 40 omega (rad/s) Velocidad Angular Rueda A 0 2 4 6 8 10 12 Tiempo (s) -2 0 2 dphi(rad/s) velocidad caída del vehículo 0 2 4 6 8 10 12 Tiempo (s) -100 0 100 alfa (rad/s2) alfa Figura 6.25 LQRI: Variables ante ensayo de fuerza creciente en parte superior del vehículo. 0 2 4 6 8 10 12 Tiempo (s) -30 -20 -10 0 10 20 30 Ángulo (º) Ángulo Figura 6.26 LQRI: Ángulo ante ensayo de fuerza creciente en parte superior del vehículo. 6.2 Control LQR con efecto integral 97 0 2 4 6 8 10 12 Tiempo (s) -40 -30 -20 -10 0 10 20 30 40 omega (rad/s) Velocidad Angular Ruedas Figura 6.27 LQRI: Velocidad angular de las ruedas ante ensayo de fuerza creciente en parte superior del vehículo. 6.2.5 Ascenso y Descenso en plano inclinado control remoto En este ensayo, se hizo subir al vehículo por un plano inclinado de 15◦ , mediante el uso del joystick, una vez suba la rampa se estabilizara durante unos cuantos segundos y se le hará bajar. Su resultados son similares a los explicados, para el mismo experimento, en el apartado anterior. Este ensayo dura 50 segundos y se realizan 3 descensos por el plano inclinado. Se parte del vehículo al inicio de rampa y en el segundo 2 se varía la referencia de velocidad para hacer subir al vehículo por la rampa. Se aprecia que la subida sigue siendo lenta, pues dura cerca de 4 segundos y el ángulo que alcanza en este caso, es superior a 20◦ , para compensar la fuerza de la gravedad. Se estabiliza entorno a 0◦ en lo alto de la rampa durante 6 segundos y en el segundo 11 se dirige el vehículo para que descienda la cuesta. Se repite la operación de subida en los segundos 20 y 33. 98 Capítulo 6. Resultados 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -20 0 20 Ángulo (º) Ángulo 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -40 -20 0 20 40 omega (rad/s) Velocidad Angular Rueda A 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -2 0 2 dphi(rad/s) velocidad caída del vehículo 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -100 0 100 alfa (rad/s2) alfa Figura 6.28 LQRI: Variables ante ensayo en plano inclinado. Control remoto. 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -30 -20 -10 0 10 20 30 Ángulo (º) Ángulo Figura 6.29 LQRI: Ángulo ante ensayo en plano inclinado. Control remoto. 6.3 Control MPC 99 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -40 -30 -20 -10 0 10 20 30 40 omega (rad/s) Velocidad Angular Ruedas Figura 6.30 LQRI: Velocidad angular de las ruedas ante ensayo en plano inclinado. Control remoto. 6.3 Control MPC El control MPC que se muestra en las siguientes gráficas, mejora sustancialmente el control del vehículo, haciéndolo más suave, corrigiendo los errores en régimen permanente, alcanzando mayores velocidades. 6.3.1 Seguimiento de referencia nula en velocidad de las ruedas Como se ha visto, este experimento consiste en dejar al vehículo quieto sobre un plano horizontal durante 50 segundos. En los controladores anteriores, se apreciaba como aparecían pequeñas variaciones del ángulo no superiores a ningún momento a 2◦ . En el MPC el vehículo queda prácticamente parado, siendo la amplitud de las pequeñas oscilaciones entorno a 0.5◦ y no superando en ningún momento 1◦ . Por todo ello, este controlador supera con creces a los dos anteriores en el ensayo de referencia de velocidad nula. 100 Capítulo 6. Resultados 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -5 0 5 Ángulo (º) Ángulo 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -10 -5 0 5 10 omega (rad/s) Velocidad Angular Rueda A 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -2 -1 0 1 2 dphi(rad/s) velocidad caída del vehículo 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -40 -20 0 20 40 alfa (rad/s2) alfa Figura 6.31 MPC: Variables ante ensayo de seguimiento de referencia nula en velocidad. 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -5 -4 -3 -2 -1 0 1 2 3 4 5 Ángulo (º) Ángulo Figura 6.32 MPC: Ángulo ante ensayo de seguimiento de referencia nula en velocidad. 6.3 Control MPC 101 0 5 10 15 20 25 30 35 40 45 50 Tiempo (s) -10 -8 -6 -4 -2 0 2 4 6 8 10 omega (rad/s) Velocidad Angular Ruedas Figura 6.33 MPC: Velocidad angular de las ruedas ante ensayo de seguimiento de referencia nula en velocidad. 6.3.2 Perturbaciones tipo pulso Este ensayo consiste en dejar al vehículo quieto que estabilice entorno a su posición de equilibrio, con una referencia nula en sus variables. Se aprecia que el vehículo alcanza menor inclinación que la alcanzada en los dos controladores anteriores, que recordemos superaba los 20◦ . . El vehículo presente una gran resistencia a la variación de su posición. Teniendo que dar golpes secos de mayor fuerza, para alcanzar menor inclinación. Puede apreciarse que la aceleración llega a saturar al valor que se ha indicado como restricción (80 rad/s2 ). El control MPC es más robusto que los controladores anteriores. 108 Capítulo 6. Resultados 0 10 20 30 40 50 60 70 Tiempo (s) -20 0 20 Ángulo (º) Ángulo 0 10 20 30 40 50 60 70 Tiempo (s) -40 -20 0 20 40 omega (rad/s) Velocidad Angular Rueda A 0 10 20 30 40 50 60 70 Tiempo (s) -2 0 2 dphi(rad/s) velocidad caída del vehículo 0 10 20 30 40 50 60 70 Tiempo (s) -100 -50 0 50 100 alfa (rad/s2) alfa Figura 6.43 MPC: Variables ante ensayo en plano inclinado. Control remoto. 0 10 20 30 40 50 60 70 Tiempo (s) -30 -20 -10 0 10 20 30 Ángulo (º) Ángulo Figura 6.44 MPC: Ángulo ante ensayo en plano inclinado. Control remoto. 6.3 Control MPC 109 0 10 20 30 40 50 60 70 Tiempo (s) -40 -30 -20 -10 0 10 20 30 40 omega (rad/s) Velocidad Angular Ruedas Figura 6.45 MPC: Velocidad angular de las ruedas ante ensayo en plano inclinado. Control remoto. 7 Conclusiones El objetivo de este trabajo ha sido el diseño, fabricación, montaje y control remoto de un vehículo balancín sobre dos ruedas basado en la configuración del péndulo invertido. Dicho trabajo parte de una línea de trabajos iniciada hace algunos años en el departamento de Ingeniería Automática de la Escuela Técnica Superior de Ingeniería de Sevilla. Pretende ser una ampliación y mejora de los trabajos anteriores. Con la adición de la microconputadora Rasberry Pi 3, se pretende dar un giro a las técnicas de control. La adición de este dispositivo nos abre un amplio horizonte de posibilidades. La potencia de cálculo nos permite implementar técnicas de control avanzadas como el control MPC, imposible de implementar en un controlador Arduino debido a los costes computacionales del mismo. Sin embargo ha sido necesario seguir contando con el controlador Arduino, pues la RaspberryPi, aunque cada día está más enfocada a aplicaciones robóticas, aún dista mucho de Arduino en este sentido. La necesidad de dotar de giro al vehículo y por tanto, de dos canales PWM a diferente frecuencia para implementar la velocidad de las ruedas, hicieron necesario el uso de Arduino. Si la RPi incorporase en sus modelos venideros un segundo canal PWM podría prescindirse del uso de Arduino. Aún así, la Rpi ha dado muy buenos resultados en este trabajo para la aplicación de controladores, dada su gran velocidad de cálculo y facilidad de comunicación con Arduino. Los resultados obtenidos ante los diferentes ensayos y con los diferentes controladores que se han implementados, han sido bastante satisfactorios. A modo de resumen: • Para el ensayo ante referencias nulas, los mejores resultados han sido los obtenidos con el control LQRI y MPC consiguiendo que el vehículo se mantenga en su posición de equilibrio prácticamente quieto sin apenas oscilaciones. • Para el ensayos ante perturbaciones tipo pulso, el control LQRI adquiría mayor inclinación volviendo rápidamente a su posición de equilibrio. En este experimento el control MPC presentaba una gran resistencia a variar su posición, teniendo que dar golpes más fuertes pero aún así adquiría un menor ángulo. • Para el ensayo ante variaciones en la referencia de velocidad, el control MPC nos permite dar saltos mayores en velocidad sin que se produzcan oscilaciones que tumben el vehículo. En las diferentes gráficas, se puede apreciar que con el control MPC se hacen variaciones de ±25(rad/s) , inclinándose el vehículo entorno a 15◦ , mientras que con el control LQR, para variaciones de ±10(rad/s)el vehículo se inclinaba entorno a 18◦. • Para el ensayo en un plano inclinado, se llevaron a cabo dos tipo experimentos. El primero, consistía en forzar con la mano a que el vehículo subiese la cuesta, mientras que en el segundo, la subida se realizaba vía remota con un joystick. En el movimiento forzado, el vehículo se oponía a la fuerza, empujando hacia ella (hacia la base de la rampa), mientras que el segundo caso, la oposición era a la fuerza de la gravedad (Hacia lo alto de la rampa). En el 111 112 Capítulo 7. Conclusiones movimiento de bajada, el control LQR adquiere una mayor inclinación que el control MPC que, al igual que el ensayo ante perturbaciones tipo pulso, se opone fuertemente a la variación de su posición. A pesar de los buenos resultados, el trabajo queda lejos de ser perfecto, pues admite numerosas mejoras y líneas de investigación futuras. Como hemos dicho, la Raspberry Pi nos permite abrir un horizonte de nuevas posibilidades como puede ser la incorporación de la PiCam. La PiCam es una pequeña cámara que nos daría la posibilidad de convertir nuestro vehículo Segway en un robot completamente autónomo que fuera procesando, en tiempo real, las imágenes que recibe de su entorno y actuase en consecuencia, pudiendo transmitir estas imágenes vía stremming. Por ejemplo, permitir que el vehículo se desplazara hacía un objetivo, mediante el reconocimiento de formas o colores. En trabajos futuros, deberá modificarse la PCB diseñada para añadir dispositivos que no se tuvieron en cuenta en su diseño y fueron necesarios añadir a lo largo del trabajo, como puede ser el módulo bluetooth de Arduino o un pequeño ventilador que disipe todo el calor que generan las placas electrónicas. La Rpi y el regulador de tensión adquirían tal temperatura que se reiniciaba cada pocos segundos, con la adición de un ventilador axial y de aletas, desaparecieron los citados problemas de temperatura. El diseño del vehículo debe mejorarse, haciéndolo algo más ligero, pues el elevado peso del mismo (1115 gr) impide que adquiera una mayor inclinación. Al utilizar unos motores de mayor par, los controladores EasyDriver quedan algo cortos, investigar la existencia de otro tipo de controladores para motores paso a paso que sean capaces de proporcionar mayor par a los motores, lo que haría aumentar la fuerza de los mismos y por tanto permitiría una mayor inclinación y una mayor velocidad. Bibliografía [1] Sistema operativo en tiempo real, https://xenomai.org/documentation/xenomai- 2.4/html/xenomai/. [2] Brian Schmalz, Easydriver stepper motor driver, https://www.schmalzhaus.com/EasyDriver/. [3] C.Gonzalez, I.Alvarado, and D.Muñoz La Peña., Low cost two-wheels self-balancing robot for control education., IFAC PapersOnLine (2017.). [4] José Antonio Borja Conde, Desarrollo de un robot autoequilibrado basado en arduino con motores paso a paso, 2018. [5] FAIRCHILD, Datasheet ka78txx, https://pdf1.alldatasheet.es/datasheetpdf/view/527328/FAIRCHILD/KA78T05.html. [6] Nicolás Cortés Fernández, Diseño, fabricación, montaje, estudio dinámico, control y teleoperación de un vehículo tipo péndulo invertido sobre dos ruedas, 2019. [7] A. Ferramosca, D. Limon, I. Alvarado, and E. Camacho, MPC for tracking with optimal closed-loop perdormance, 2009. [8] Cecilia González González, Mejora del software de un robot autoequilibrado de tipo péndulo invertido de bajo coste, 2016. [9] Guillermo Lehmann, Instructivo para la distribución de componentes y diseño de pistas en placas de circuito impreso (pcb). [10] InvenSense Inc, Mpu-600 and mpu-6050 product specification revision 3.4, https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-6000-Datasheet1.pdf. [11] Rodríguez L. Arroyo S Jaramillo D, Loya.H, Estabilización de un péndulo invertido aplicando MPC y LQR, 2014. [12] Pablo Kupra, Daniel Limon, and Teodoro Alamo, Implementation of model predictive control in programmable logic controllers, IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY (2020.). [13] Victor Manuel Villalar Lara, Control predictivo de un vehículo tipo segway, 2017. [14] Luis Llamas, Determianr la orientación con arduino y el imu mpu-6050, https://www.luisllamas.es/arduino-orientacion-imu-mpu-6050/. [15] , El bus i2c en arduino, https://www.luisllamas.es/arduino-i2c/. 113 114 Apéndice 7. Bibliografía [16] ,Medir la inclinación con imu, arduino y filtro complementario, https://www.luisllamas.es/arduino-i2c/. [17] ,Qué son y cómo usar interrupciones en arduino, https://www.luisllamas.es/que-son- y-como-usar-interrupciones-en-arduino/. [18] Antonio J. Croche Navarro, Vehículo autoequilibrado de tipo péndulo invertido de bajo coste, 2014. Códigos 1 Código Arduino 1#include <Wire.h> 2#include <math.h> 3#include <SoftwareSerial .h> 4#define DIRECCION_RPI 0x05 //ESTABLE LA DIRECCIÓN I2C 5 6 // Bluetooth 7char comando; 8int dtheta_r ; 9char NOMBRE[21] = "JOSE"; 10 char BPS = ’8’;// 1=1200 , 2=2400, 3=4800, 4=9600, 5=19200, 6=38400, 7=57600, 8=115200 11 char PASS[5] = "1234";// PIN O CLAVE de 4 caracteres numericos 12 SoftwareSerial mySerial(0, 1); 13 #define STX 0x02 14 #define ETX 0x03 15 #define ledPin 13 16 #define SLOW 750 17 #define FAST 250 18 byte cmd[8] = {0, 0, 0, 0, 0, 0, 0, 0}; 19 byte buttonStatus = 0; 20 long previousMillis = 0; 21 long sendInterval = SLOW; 22 String displayStatus = "xxxx"; 23 24 // DEFINO LA PREESCALA DE LOS TIMER 1 Y 2 25 #define PREESCALA_1 8 26 #define PREESCALA_2 256 27 // TIEMPO DE PULSO DE CADA TIMER 28 float T_PULS_1=0.0000000625∗PREESCALA_1; //T=(1/16000000)∗65535∗Escala 29 float T_PULS_2=0.0000159375∗PREESCALA_2; //T=(1/16000000)∗255∗Escala 30 31 // DEFINO FUNCIONES PARA ACTIVAR O DESACTIVAR PINES 32 #define CLR(x, y) (x&=(~(1<<y))) 33 #define CLRR(x, y, z) (x&=(~((1<<y)|(1<<z)))) 34 #define SET(x, y) (x|=(1<<y)) 35 #define SETT(x, y, z) (x|=(1<<y)|(1<<z)) 36 #define SETTT(x, y, z, v) (x|=(1<<y)|(1<<z)|(1<<v)) 37 38 // VARIABLES GLOBALES 39 float alfa =0; // VARIABLE QUE ALMACENA LA ACELERACIÓN QUE RECIBE DE LA RPI 40 float vec[2]={0,0}; // VECTOR QUE ALMACENA LOS VALORES DE LA VELOCIDAD 115 116 Capítulo . Códigos 41 float alfa_M1=0; // VARIABLE QUE ALMACENA LA ACELERACIÓN A APLICAR, DESPUÉS DE FILTRAR SI ESTA ENTRE LOS LIMITES ESTABLECIDOS EN +−xx RAD/S^2. EVITANDO ASI EL OVF AL ENVIARSE UN DATO CORRUPTO 42 float flag=0; // BANDERA QUE SE PONE A CERO CUANDO SE RECIBE UN DATO alfa DESDE LA RPI, UNA VEZ PROCESADO ESTE DATO SE PONE A UNO A LA ESPERA DE RECIBIR OTRO DATO 43 int comprueba; 44 int j=0; 45 float vel ; // VALOR DE LA VELOCIDAD MEDIA QUE LE ENVÍO A LA PI 46 float vel_M1, vel_M2; 47 48 // DECLARACION PINES MOTOR 1 49 #define STEP_MOTOR_1 2 // PORTD 4 50 #define DIR_MOTOR_1 6 // PORTD 5 51 int MS1_MOTOR_1 = 8 −8 ; //PORTB 1 52 int MS2_MOTOR_1 = 9 −8; // PORTB 2 53 54 // DECLARACION PINES MOTOR 2 55 #define STEP_MOTOR_2 4 // PORTD 7 56 #define DIR_MOTOR_2 5 // PORTD 6 57 int MS1_MOTOR_2 = 11 −8; // PORTB 3 58 int MS2_MOTOR_2 = 10 −8; // PORTB 2 59 60 // PARÁMETROS DEL LED 61 #define PIN 3 62 63 // CONEXIÓN BLUETOOTH 64 float giro_M1=0, giro_M2; 65 66 // DECLARACIÓN DE LA CANTIDAD DE PASOS POR REVOLUCIÓN DEPENDIENDO DE LA CONFIGURACIÓN 67 uint16_t npuls_min[]={700, 600, 600, 600}; 68 uint16_t npuls_max[]={65535, 65535, 65535, 65535}; 69 // NUMERO MAX Y MIN DE PASOS/SEGUNDO 70 float f_timer_min[]={1/(npuls_max[0]∗T_PULS_1),1/(npuls_max[1]∗T_PULS_1),1/(npuls_max[2]∗T_PULS_1 ),1/(npuls_max[3]∗T_PULS_1)}; 71 float f_timer_max[]={1/(npuls_min[0]∗T_PULS_1),1/(npuls_min[1]∗T_PULS_1),1/(npuls_min[2]∗T_PULS_1) ,1/(npuls_min[3]∗T_PULS_1)}; 72 // NUMERO DE PASOS PARA DAR UNA VUELTA Y AVANCE DE CADA PASO 73 float avance_grados [4]={1.8,0.9,0.145,0.225}; 74 float pasos[]={200,400,800,1600}; 75 // FRECUENCIAS MAX Y MINIMAS, No ¯MAX Y MINIMOS DE VUELTAS POR SEGUNDO. EXPERIMENTALMENTE LA VEL MAX 3000o ¯/S 76 float f_motor_max[]={f_timer_max[0]/pasos[0],f_timer_max[1]/pasos [1], f_timer_max[2]/pasos [2], f_timer_max[3]/pasos [3]}; 77 float f_motor_min[]={f_timer_min[0]/pasos [0], f_timer_min[1]/ pasos [1], f_timer_min[2]/pasos [2], f_timer_min [3]/ pasos [3]}; 78 // VELOCIDAD MAX Y MIN DEL MOTOR EN RAD/S.. W=2∗PI∗f 79 float omega_motor_max[]={2∗PI∗f_motor_max[0],2∗PI∗f_motor_max[1],2∗PI∗f_motor_max[2],2∗PI∗ f_motor_max[3]}; 80 float omega_motor_min[]={2∗PI∗f_motor_min[0],2∗PI∗f_motor_min[1],2∗PI∗f_motor_min[2],2∗PI∗ f_motor_min[3]}; 81 // NUMERO DE PULSOS DE CADA INTERRUPCION DEL TIMER 1 82 uint16_t npuls_M1=65535, npuls_M2=65535; 83 // DIRECCIÓN DE CADA MOTOR 84 int dir_M1=0, dir_M2=0; 85 // TRADUCE DE o ¯/S A PULSOS/S 86 float omega2npuls[]={omega_motor_max[0]/(T_PULS_1∗f_motor_max[0]∗pasos[0]),omega_motor_max[1]/( T_PULS_1∗f_motor_max[1]∗pasos[1]),omega_motor_max[2]/(T_PULS_1∗f_motor_max[2]∗pasos[2]), omega_motor_max[3]/(T_PULS_1∗f_motor_max[3]∗pasos[3])}; 87 88 // FUNCIÓN SETUP PARA INICIAR VALORES INICIALES// 89 void setup () { 1 Código Arduino 117 90 // COMUNICACIÓN I2C CON RPi 91 Wire.begin(DIRECCION_RPI); //ESTABLECE COMUNICACIÓN I2C CON LA RPI EN LA DIRECCIÓN DIRECCION_RPI QUE SERÁ LA 0X05 92 Wire.onReceive( dato_recibido ); // FUNCIÓN QUE ACTÚA COMO UN MANEJADOR SALTANDO CUANDO RECIBE UN ENVÍO DE DATO DE LA RPI 93 Wire.onRequest(dato_enviado); // FUNCIÓN QUE ACTÚA COMO UN MANEJADOR SALTANDO CUANDO RECIBE UNA PETICIÓN DE DATO DE LA RPI 94 95 // CONFIGURACIÓN DE LA VELOCIDAD DE TRANSMISIÓN I2C 96 TWBR=158; 97 TWBR|−bit(TWPS0); 98 99 // CONFIGURACIÓN DEL MONITOR SERIE Y VELOCIDAD BUETOOTH // 1=1200 , 2=2400, 3=4800, 4=9600, 5=19200, 6=38400, 7=57600, 8=115200 100 mySerial.begin(38400); 101 Serial .begin(38400); 102 103 // CONFIGURACIÓN MOTOR 1 104 pinMode(MS1_MOTOR_1 + 8 , OUTPUT); //MS1 105 pinMode(MS2_MOTOR_1 + 8, OUTPUT); //MS2 106 pinMode(STEP_MOTOR_1 , OUTPUT); //STEP 107 pinMode(DIR_MOTOR_1, OUTPUT); //DIR 108 109 // CONFIGURACIÓN DEL MOTOR 2 110 pinMode(MS1_MOTOR_2 + 8 , OUTPUT); //MS1 111 pinMode(MS2_MOTOR_2 + 8 , OUTPUT); //MS2 112 pinMode(STEP_MOTOR_2, OUTPUT); //STEP 113 pinMode(DIR_MOTOR_2, OUTPUT); //DIR 114 115 // CONFIGURACIÓN DEL PIN 116 pinMode(PIN, OUTPUT); 117 digitalWrite (PIN, HIGH); 118 119 // CONFIGURACIÓN DE LAS INTERRUPCIONES DEL TIMER 1 PARA EL CONTROL DE LA VELOCIDAD 120 TCCR1A=0; //INICIAMOS EN MODO NORMAL 121 TCCR1B=0; //INICIAMOS EN MODO NORMAL 122 switch (PREESCALA_1){ 123 case 1: // 4.095ms 124 SET(TCCR1B,CS10); 125 break; 126 case 8: // 32.7675ms 127 SET(TCCR1B,CS11); 128 break; 129 case 64: // 0.2621s 130 SETT(TCCR1B,CS11,CS10); 131 break; 132 case 256: // 1.048s 133 SETT(TCCR1B,WGM12,CS12); 134 break; 135 case 1024: // 4.1942 136 SETT(TCCR1B,CS12,CS10); 137 break; 138 dir_M1=0; 139 dir_M2=0; 140 }; 141 OCR1A =65535; //ACTIVA INTERRUPCIÓN DEL TIMER 1A 142 OCR1B =65535; //ACTIVA INTERRUPCION DEL TIMER 1B 143 TIMSK1 =0; // LIMPIA LA MASCARA DEL TIMER 144 SETT(TIMSK1,OCIE1A,OCIE1B); //ACTIVA LAS INTERRUPCIONES A Y B DEL TIMER 1 145 124 Capítulo . Códigos 37 #include <sys/fcntl.h> 38 #include <sys/stat.h> 39 #include <i2c/smbus.h> 40 #include <math.h> 41 #include <pthread.h> 42 43 //normas posix para calcular el tiempo 44 #define _POSIX_C_SOURCE 200809L 45 46 //cambio de radianes a grados 47 #define RAD_T_DEG 57.29577951308 48 49 //posibilidades de rango giroscopio, por defecto usaremos 0 50 #if GYRO_RANGE == 1 51 #define GYRO_SENS 65.5 52 #define GYRO_CONFIG 0b00001000 53 #elif GYRO_RANGE == 2 54 #define GYRO_SENS 32.8 55 #define GYRO_CONFIG 0b00010000 56 #elif GYRO_RANGE == 3 57 #define GYRO_SENS 16.4 58 #define GYRO_CONFIG 0b00011000 59 #else 60 #define GYRO_SENS 131.0 61 #define GYRO_CONFIG 0b00000000 62 #endif 63 #undef GYRO_RANGE 64 65 //posibilidades de rango giroscopio, por defecto usaremos 0 66 #if GYRO_RANGE == 1 67 #define ACCEL_SENS 8192.0 68 #define ACCEL_CONFIG 0b00001000 69 #elif GYRO_RANGE == 2 70 #define ACCEL_SENS 4096.0 71 #define ACCEL_CONFIG 0b00010000 72 #elif GYRO_RANGE == 3 73 #define ACCEL_SENS 2048.0 74 #define ACCEL_CONFIG 0b00011000 75 #else 76 #define ACCEL_SENS 16384.0 77 #define ACCEL_CONFIG 0b00000000 78 #endif 79 #undef GYRO_RANGE 80 81 //variables globales 82 float _accel_angle[3]; 83 float _gyro_angle[3]; 84 int MPU6050_addr; 85 int f_dev; 86 87 88 89 //función para iniciar el sensor, recibe como argumento la dirección i2c 90 void inicio_mpu (int addr) 91 { 2 Biblioteca MPU6050.h 125 92 int status; 93 MPU6050_addr = addr; //guarda la dirección recibida en la variable MPU6050_addr 94 //abre la comuicacion i2c 95 f_dev = open("/dev/i2c-1", O_RDWR); 96 if (f_dev < 0) { 97 printf("error abriendo la conexión"); 98 } 99 status = ioctl(f_dev, I2C_SLAVE, MPU6050_addr); 100 if (status < 0) { 101 printf("error"); 102 103 104 } 105 i2c_smbus_write_byte_data(f_dev, 0x6b, 0b00000000); //desabilita el modo sleep del dispositivo 106 107 i2c_smbus_write_byte_data(f_dev, 0x1a, 0b00000011); //ajusta un filtro paso bajo a 44Hz 108 109 i2c_smbus_write_byte_data(f_dev, 0x19, 0b00000100); //Establece el divisor de frecuencia de muestreo 110 111 i2c_smbus_write_byte_data(f_dev, 0x1b, GYRO_CONFIG); //configura los agustes del giroscopio 112 113 i2c_smbus_write_byte_data(f_dev, 0x1c, ACCEL_CONFIG); //configura los ajustes del acelerómetro 114 115 //pone el offset a ceto 116 i2c_smbus_write_byte_data(f_dev, 0x06, 0b00000000), i2c_smbus_write_byte_data(f_dev, 0x07, 0b00000000), i2c_smbus_write_byte_data(f_dev, 0x08, 0b00000000), i2c_smbus_write_byte_data(f_dev, 0x09, 0b00000000), i2c_smbus_write_byte_data(f_dev, 0x0A, 0b00000000), i2c_smbus_write_byte_data(f_dev, 0x0B, 0b00000000), i2c_smbus_write_byte_data(f_dev, 0x00, 0b10000001), i2c_smbus_write_byte_data(f_dev, 0x01, 0b00000001), i2c_smbus_write_byte_data(f_dev, 0x02, 0b10000001); 117 118 } 119 120 //obtiene los valores en bruto del acelerómetro 121 void getAccelRaw(float *x, float *y, float *z) 122 { 123 int16_t X = i2c_smbus_read_byte_data(f_dev, 0x3b) << 8 | i2c_smbus_read_byte_data(f_dev, 0x3c); 124 int16_t Y = i2c_smbus_read_byte_data(f_dev, 0x3d) << 8 | i2c_smbus_read_byte_data(f_dev, 0x3e); 125 int16_t Z = i2c_smbus_read_byte_data(f_dev, 0x3f) << 8 | i2c_smbus_read_byte_data(f_dev, 0x40); 126 *x=(float)X; 127 *y=(float)Y; 128 *z=(float)Z; 129 } 130 126 Capítulo . Códigos 131 //obtiene los valores en bruto del giroscopio 132 void getGyroRaw(float *roll, float *pitch, float *yaw) 133 { 134 int16_t X = i2c_smbus_read_byte_data(f_dev, 0x43) << 8 | i2c_smbus_read_byte_data(f_dev, 0x44); 135 int16_t Y = i2c_smbus_read_byte_data(f_dev, 0x45) << 8 | i2c_smbus_read_byte_data(f_dev, 0x46); 136 int16_t Z = i2c_smbus_read_byte_data(f_dev, 0x47) << 8 | i2c_smbus_read_byte_data(f_dev, 0x48); 137 *roll = (float)X; //giro en el eje x 138 *pitch = (float)Y; //giro en el eje y 139 *yaw = (float)Z; //giro en el eje z 140 141 } 142 143 //obtiene los valores del acelerómetro en g con 3 cifras decimales 144 void getAccel(float *x, float *y, float *z) 145 { 146 getAccelRaw(x, y, z); 147 *x =round((*x - A_OFF_X)*1000.0 / ACCEL_SENS)/1000.0 ; 148 *y =round((*y - A_OFF_Y)*1000.0 /ACCEL_SENS)/1000.0; 149 *z = round((*z - A_OFF_Z)*1000.0 / ACCEL_SENS)/1000.0 ; 150 } 151 152 153 //obtiene los valores del giroscopio en g/s con 3 cifras decimales 154 void getGyro(float *roll, float *pitch, float *yaw) 155 { 156 getGyroRaw(roll, pitch, yaw); 157 *roll = round((*roll - G_OFF_X) *1000.0 / GYRO_SENS) / 1000.0; 158 *pitch = round((*pitch - G_OFF_Y) *1000.0 / GYRO_SENS) / 1000.0; 159 *yaw = round((*yaw - G_OFF_Z) *1000.0 / GYRO_SENS) / 1000.0; 160 } 161 162 //calcula el offset 163 void getOffsets(float *ax_off, float *ay_off, float *az_off, float * gr_off, float *gp_off, float *gy_off) 164 { 165 float gyro_off[3]; 166 float accel_off[3]; 167 float suma_x=0, suma_y=0, suma_z=0, suma_gx=0, suma_gy=0, suma_gz=0; 168 //bucle para calcular los ofset como sumatorio y luego hacer el valor medio 169 for (int i = 0; i < 10000; i++) { 170 getGyroRaw(&gyro_off[0], &gyro_off[1], &gyro_off[2]); // 171 suma_gx=suma_gx+ gyro_off[0], suma_gy=suma_gy+ gyro_off[1], suma_gz= suma_gz+ gyro_off[2]; 172 getAccelRaw(&accel_off[0], &accel_off[1], &accel_off[2]); 173 suma_x=suma_x+accel_off[0], suma_y=suma_y+accel_off[1], suma_z=suma_z + accel_off[2]; 174 } 175 176 *gr_off = suma_gx / 10000, *gp_off = suma_gy / 10000, *gy_off = suma_gz / 10000; 177 *ax_off = suma_x / 10000, *ay_off = suma_y / 10000, *az_off = suma_z / 10000; 3 Biblioteca ARDUINO.h 127 178 *az_off = *az_off - ACCEL_SENS ; 179 } 3 Biblioteca ARDUINO.h 1#include <stdio.h> 2#include <unistd.h> 3#include <fcntl.h> 4#include <sys/ioctl.h> 5#include <sys/types.h> 6#include <time.h> 7#include <sys/time.h> 8#include <linux/i2c-dev.h> 9#include <sys/fcntl.h> 10 #include <sys/stat.h> 11 #include <i2c/smbus.h> 12 #include <math.h> 13 #include <pthread.h> 14 15 //VARIABLES PARA LA COMUNICACIÓN CON ARDUINO 16 int ADRESS_ARDUINO; 17 int file; 18 19 void inicio_arduino(int ddr) 20 { 21 ADRESS_ARDUINO=ddr; 22 file = open("/dev/i2c-1", O_RDWR); 23 if (file < 0) { 24 printf("error abriendo la conexión"); 25 } 26 if (ioctl (file, I2C_SLAVE, ADRESS_ARDUINO) < 0) { 27 printf("I2C: error al acceder al esclavo en 0x%x", ADRESS_ARDUINO); 28 } 29 } 4 Código LQR y LQRI 1 2//DECLARACIÓN DE CABECERAS 3#include "MPU6050.h" //dentro de dicha libreria va declarada todas las cabeceras necesarias 4#include <signal.h> 5#include <string.h> 6#include <stdlib.h> 7#include "ARDUINO.h" 8 9 10 //Para implementar el control LQRI se varía la cuarta componente del vector K a 0.3 11 float K[]={-350.22,-47.23,7.42,0}; 12 float offset=3.5; 128 Capítulo . Códigos 13 14 //FUNCIONES 15 float calculo_lqr(float A, float B, float gir); 16 float filtro(float accel, float gxy); 17 18 //CAMBIO DE RADIANES A GRADOS 19 #define DEG_TO_RAD 0.01745329252 20 float PI=3.1415; 21 int cont; 22 23 //VARIABLES PARA EL MPU 24 float ax,ay,az, ax_escalado, ay_escalado, az_escalado; 25 float accel_ang_y; 26 float gx,gy,gz, gx_escalado, gy_escalado, gz_escalado; 27 float giros_ang_y, giros_ang_y_prev=3; 28 float TAU=0.99; 29 30 //VARIABLES PARA COMUNICAR CON ARDUINO 31 float omega, omega_r; 32 float alpha; 33 34 //VARIABLES PARA CONTROL LQR 35 36 float ivr=0, u; 37 float dtheta_r, phi,dphi,dtheta; 38 39 //VARIABLES PARA TEMPORIZADOR Y SENAL 40 timer_t temporizador;//declaración del nombre del temporizador 41 struct itimerspec periodo;//estructura para indicar cada cuanto tiempo se lanza el temporizador 42 struct timespec inicial; 43 struct timespec tiempo; //estructura para definirla duración del temporizador 44 struct sigevent aviso; //establezco una senal que avise que se ha cumplido el temporizador 45 sigset_t sigset; //conjunto de senales 46 struct sigaction act; 47 48 //VARIABLE EJECUTIVO CICLICO 49 int ciclo=0; 50 double dtt; 51 clock_t iniciall; 52 int flag=0; 53 54 //FUNCIÓN MANEJADOR 55 void Manejador(int signo, siginfo_t *info, void*context) 56 { 57 ciclo++; 58 //CADA VEZ QUE SE ACTIVA LA SENAL, CICLO AUMENTA EN UNA UNIDAD Y HACE LO QUE LE CORRESPONDA EN EL SWITCH-CASE 59 //printf("ciclo %d \n", ciclo); 60 switch(ciclo) 61 { 62 case 1 : 63 getGyro(&gx_escalado, &gy_escalado, &gz_escalado); 4 Código LQR y LQRI 129 64 getAccel(&ax_escalado, &ay_escalado, &az_escalado); //lee valores en el sistema internacional g 65 accel_ang_y = atan2(az_escalado , (sqrt((ay_escalado*ay_escalado) + ( ax_escalado*ax_escalado))))*(180/PI); //da el angulo en grados 66 giros_ang_y =filtro(accel_ang_y,gy_escalado); 67 68 //printf("gx %f \n", gx_escalado); 69 //printf("gy %f \n", gy_escalado); 70 //printf("gz %f \n", gz_escalado); 71 //printf("ay %f \n", ay_escalado); 72 //printf("az %f \n", accel_ang_y); 73 //printf("dphi %f \n", gy_escalado); 74 //printf ("angulo %f \n", (giros_ang_y+offset)); 75 break; 76 77 case 2: 78 read(file, &omega, sizeof(omega)); 79 //printf("omega %f \n", omega_A); 80 break; 81 82 case 3: 83 read(file, &omega_r, sizeof(omega_r)); 84 //printf ("ref %f \n", omega_r); 85 break; 86 87 case 4: 88 alpha=calculo_lqr(omega,omega_r, giros_ang_y); 89 //printf ("alfa %f \n", alpha); 90 break; 91 92 case 5: 93 write(file, &alpha, sizeof(alpha)); 94 ciclo = 0; 95 break; 96 97 } 98 } 99 100 101 //FUNCIÓN PRINCIPAL MAIN 102 int main(void ) 103 { 104 inicio_mpu(0x68); //inicio la conexión del MPU6050 105 inicio_arduino(0x05);//inicio la conexión con ARDUINO 106 if (flag==0){ 107 flag=1; 108 iniciall=clock(); 109 } 110 //TRATAMIENTO DE SENALES 111 sigemptyset(&sigset); //crea una mascara vacía 112 sigaddset(&sigset, SIGUSR1); //anade la senal SIGALARM al conjunto 113 pthread_sigmask(SIG_UNBLOCK,&sigset, NULL); //bloqueo la senal para el proceso 114 /*programo la senar SIGVTALRM para que salte la funcion manejador cuando se cumpla el temporizador */ 115 act.sa_sigaction=Manejador; 130 Capítulo . Códigos 116 sigemptyset(&(act.sa_mask)); 117 act.sa_flags=SA_SIGINFO; 118 sigaction(SIGUSR1, &act, NULL); 119 //configuro el temporizador 120 inicial.tv_sec=1; 121 tiempo.tv_nsec=4000000; //cada 4 ms salta el manejador 122 periodo.it_value=inicial; //indico que el primer valor lo lance 1 segundo despues de llamarlo 123 periodo.it_interval=tiempo; //indico que los peridos sean cada 4 ms 124 aviso.sigev_notify=SIGEV_SIGNAL; 125 aviso.sigev_signo=SIGUSR1; 126 timer_create(CLOCK_REALTIME, &aviso, &temporizador);//creo el temporizador llamado tempo, de tiempo real y que avise con la senal configurada en el segundo argumento de entrada 127 //inicio el temporizador 128 timer_settime(&temporizador, TIMER_ABSTIME, &periodo , NULL); 129 while(1){} 130 exit(0); 131 } 132 133 //función que calcula la acción de control 134 float calculo_lqr(float A, float B,float gir) 135 { 136 phi=(gir+offset)*DEG_TO_RAD; 137 dphi=gy_escalado*DEG_TO_RAD; 138 dtheta=A; 139 ivr=ivr+B-A; 140 u=K[0]*(phi)+K[1]*(dphi)+K[2]*(B+A)+K[3]*ivr; 141 return (u); 142 } 143 144 //función que calcula el filtro complementario 145 float filtro(float accel, float gxy) 146 { 147 dtt=(double)(clock()-iniciall)/CLOCKS_PER_SEC; 148 giros_ang_y_prev=TAU*(giros_ang_y_prev+dtt*gxy)+(1-TAU)*accel; 149 iniciall=clock(); 150 return (giros_ang_y_prev); 151 } 5 Código MPC 1//DECLARACIÓN DE CABECERAS 2#include "MPU6050.h" //dentro de dicha libreria va declarada todas las cabeceras necesarias 3#include <signal.h> 4#include <string.h> 5#include <stdlib.h> 6#include "ARDUINO.h" 7#include <stdbool.h> 8 9//FUNCIONES 10 void MPCT_EADMM(float *pointer_u, float *pointer_x0, float * pointer_ref); 5 Código MPC 131 11 float filtro (float accel, float gxy); 12 13 //VARIABLES PARA EL MPC 14 #define n 3 15 #define m 1 16 #define nm 4 17 #define N 12 18 #define k_max 500 19 #define tol 0.0010000000 20 const float beta[4][13] = { {1000.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 1000.0000000000}, {1000.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 1000.0000000000}, {1000.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 1000.0000000000}, {5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 5.0000000000, 200.0000000000} }; 21 const float LB[4] = { -0.8707963268, -1000.0000000000, -3.0000000000, -4.50000000 }; 22 const float UB[4] = { 0.8707963268, 1000.0000000000, 3.0000000000,4.50000000 }; 23 const float AB[3][4] = { {1.0131085393, 0.0200873141, 0.0000000000, -0.0029194134}, {1.3137103440, 1.0131085393, 0.0000000000, -0.2925774981}, {0.0000000000, 0.0000000000, 1.0000000000, 0.0200000000} }; 24 const float TS[4] = { -1000.0000000000, -1000.0000000000, -1000.0000000000, -0.1250000000 }; 25 const float Alpha[11][3][3] = { {{-0.3187462091, -0.4133221424, 0.0000000000}, {-0.0014156360, -0.2870717845, 0.0000000000}, {-0.0000516370, -0.0030857846, -0.3145598227}}, {{-0.3186572629, -0.4132068048, 0.0000000000}, {0.0030727422, -0.2608568047, 0.0000000000}, {-0.0000324881, -0.0048119945, -0.3145044137}}, {{-0.3185796468, -0.4131061590, 0.0000000000}, {0.0062006954, -0.2440971015, 0.0000000000}, {0.0000645421, -0.0053381803, -0.3144719197}}, {{-0.3185474408, -0.4130643971, 0.0000000000}, {0.0079683437, -0.2351160162, 0.0000000000}, {0.0002045042, -0.0051923599, -0.3144451799}}, {{-0.3185546280, -0.4130737169, 0.0000000000}, {0.0088170295, -0.2308171255, 0.0000000000}, {0.0003564351, -0.0047721595, -0.3144147698}}, {{-0.3185844102, -0.4131123358, 0.0000000000}, {0.0091495440, -0.2289528124, 0.0000000000}, {0.0005037833, -0.0042795787, -0.3143777503}}, {{-0.3186239173, -0.4131635652, 0.0000000000}, {0.0092173022, -0.2282754245, 0.0000000000}, {0.0006398447, -0.0037983638, -0.3143341756}}, {{-0.3186658007, -0.4132178759, 0.0000000000}, {0.0091572370, -0.2281587293, 0.0000000000}, {0.0007627965, -0.0033572915, -0.3142850806}}, {{-0.3187064173, -0.4132705440, 0.0000000000}, {0.0090402573, -0.2282983200, 0.0000000000}, {0.0008728902, -0.0029626145, -0.3142316592}}, {{-0.3187441762, -0.4133195064, 0.0000000000}, {0.0089023348, -0.2285468779, 0.0000000000}, {0.0009711361, -0.0026122911, -0.3141749933}}, 132 Capítulo . Códigos {{-0.3187785344, -0.4133640591, 0.0000000000}, {0.0087613840, -0.2288329869, 0.0000000000}, {0.0010587384, -0.0023017687, -0.3141159920}} }; 26 const float Beta[12][3][3] = { {{3.1462197455, 0.0047650758, -0.0000365577}, {0.0000000000, 2.8897514064, -0.0033645762}, {0.0000000000, 0.0000000000, 3.1455982272}}, {{3.1453417925, 0.0098975925, -0.0000876370}, {0.0000000000, 2.6831294958, -0.0057265604}, {0.0000000000, 0.0000000000, 3.1450441366}}, {{3.1445756740, 0.0139248122, -0.0000686687}, {0.0000000000, 2.5544277560, -0.0068380111}, {0.0000000000, 0.0000000000, 3.1447191968}}, {{3.1442577812, 0.0163654304, 0.0000272831}, {0.0000000000, 2.4866613927, -0.0070711744}, {0.0000000000, 0.0000000000, 3.1444517987}}, {{3.1443287236, 0.0175834046, 0.0001656615}, {0.0000000000, 2.4542582365, -0.0068722004}, {0.0000000000, 0.0000000000, 3.1441476981}}, {{3.1446226917, 0.0180766713, 0.0003158739}, {0.0000000000, 2.4397389987, -0.0065156522}, {0.0000000000, 0.0000000000, 3.1437775033}}, {{3.1450126511, 0.0181906925, 0.0004615580}, {0.0000000000, 2.4337664715, -0.0061289247}, {0.0000000000, 0.0000000000, 3.1433417558}}, {{3.1454260658, 0.0181204255, 0.0005960755}, {0.0000000000, 2.4317951393, -0.0057615166}, {0.0000000000, 0.0000000000, 3.1428508060}}, {{3.1458269770, 0.0179677681, 0.0007176134}, {0.0000000000, 2.4316725701, -0.0054287773}, {0.0000000000, 0.0000000000, 3.1423165923}}, {{3.1461996797, 0.0177843589, 0.0008264128}, {0.0000000000, 2.4323788300, -0.0051326406}, {0.0000000000, 0.0000000000, 3.1417499335}}, {{3.1465388162, 0.0175958003, 0.0009234686}, {0.0000000000, 2.4334257379, -0.0048703879}, {0.0000000000, 0.0000000000, 3.1411599197}}, {{22.4828378165, 0.1244178314, 0.0072158015}, {0.0000000000, 4.2822303782, -0.0119272289}, {0.0000000000, 0.0000000000, 21.2648360907}} }; 27 int i = 0; 28 int j = 0; 29 int k = 0; 30 int l = 0; 31 float z1[4][13] = { {0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000}, {0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000}, {0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000}, {0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000} }; 32 float z2[4] = { 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000 }; 33 float z3[4][13] = { {0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000}, {0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 5 Código MPC 133 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000}, {0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000}, {0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000} }; 34 float lambda[4][15] = { {0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000}, {0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000}, {0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000}, {0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000} }; 35 float q2[4]; 36 float q3[4][13]; 37 float res[4][15]; 38 float mu3[3][12]; 39 float x0[4] = { 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000 }; 40 float res_1; 41 float res_2; 42 float ref[4] = { 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000 }; 43 bool e_flag; 44 const float H1i[4][13] = { {0.0005000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.0005000000}, {0.0005000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.0005000000}, {0.0005000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.0005000000}, {0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.2000000000, 0.0025000000} }; 45 const float W2[4][4] = { {0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000}, {0.0000000000, -0.0000000000, 0.0000000000, 0.0000000000}, {0.0000000000, 0.0000000000, -0.0002466091, 0.0000000000}, {-0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000} };