proyecto final de carrera simulador en matlab de robots...

Proyecto Final de CarreraSimulador en MATLAB de Robots Móviles

AUTOR: Santos Barquero CáceresDIRECTOR: Albert Oller i Pujol

FECHA: Junio / 2001

Dedicatoria

El trabajo realizado en este Proyecto, así como poder estudiar esta carrera, tiene su origenen el gran apoyo de toda mi familia, comenzando por mis padres, a quienes dedico TODA micarrera y este Proyecto, y siguiendo por mis hermanos; Juan Benito, Eugenio, Carlos, Cabezón (hayno Antonio) y a mi hermanito pequeñito de 1,95 m. El Orejón, es broma... Jordi.

Hosti !!! me olvidaba de mi hermana, la mayor, ¿ como se llamaba... ? Ah! Sí, Mª. Luisa.

Y continuando con la familia: mi sobrina Marina, y mi pequeño (de momento) sobrinoSergi.

Todo este “papel” va por y para vosotros.

Agradecimientos

Al director del Proyecto, Albert Oller, por su largiiiiiiiiisima paciencia.A mi hermano Juan Benito, que ha soportado dolores de cabeza corrigiendo las parrafadas

del Proyecto, y aún así no lo ha conseguido del todo.Y para acabar, agradecer a otro de mis muchos pelmazos hermanos, Eugenio, el último

viaje y lo que él quiera.

Otros agradecimientos

Al creador de los Simpsons por los buenos ratos que me ha hecho pasar delante de la cajatonta (“la tele”).

1. Índice

i

1. Índice

Página

2. Memoria Descriptiva ................................................................................................. 1

2.1 Introducción ...................................................................................................................................22.2 Objetivos del Proyecto .................................................................................................................. 22.3 Antecedentes .................................................................................................................................. 32.4 Titular .............................................................................................................................................32.5 Destinatario ....................................................................................................................................42.6 Posibles Soluciones y Solución Adoptada ....................................................................................42.7 Descripción General del Proyecto ................................................................................................5

2.7.1 Lenguaje de Programación (MATLAB V4.2.1.C) ................................................................52.7.2 Fases para el Desarrollo y el Planteamiento del Programa ............................................... 52.7.3 Inicio del Programa (comensa) ............................................................................................7

2.7.3.1 Declaración de Variables del Fichero Simulink. Función invarsim() ......................... 72.7.3.2 Declaración de Variables del Programa. Función invargen() ......................................72.7.3.3 Pantalla de Simulación. Función idsply() .................................................................... 8

2.7.4 Creación del Robot y Pantalla de Diseño del Robot ............................................................92.7.4.1 Representación Gráfica del Robot (función dibu_a(...)) ............................................. 9

Circulo del Espacio Vital .............................................................................................10Dibujo Sensores del Robot .......................................................................................... 10

2.7.5 Estructura de Datos del Robot (función strucrob(...)) ......................................................... 112.7.6 Funciones Asociadas al Tratamiento de Datos del Setpoint del Robot ............................... 14

2.7.6.1 Creación del Setpoint (función position(...)) ................................................................142.7.6.2 Función Set de Datos en el Setpoint (función setpos(...)) ............................................152.7.6.3 Función Get datos del Setpoint (función getpos(...)) ................................................... 16

2.7.7 Funciones Asociadas al Tratamiento de Datos del Robot ................................................... 162.7.7.1 Creación de un Nuevo Robot (función agent(...)) ........................................................162.7.7.2 Función Set Datos en la Estructura del Robot (función sagent(...)) ............................ 182.7.7.3 Función Get Datos de la Estructura del Robot (función gagent(...)) ........................... 19

2.7.8 Pantalla de Diseño del Robot ...............................................................................................202.7.8.1 Cambio de Tamaño del Robot ..................................................................................... 212.7.8.2 Diseño y Creación de Sensores de Proximidad ........................................................... 22

Crear un Sensor (función asensor()) ............................................................................22Seleccionar el Sensor (función chcolsen()) ................................................................. 26Actualizar Datos de un Sensor (función updasen()) ....................................................27Borrar un Sensor (función dsensor()) .......................................................................... 28

2.7.8.3 Fichero Simulink de Control ........................................................................................29Modelado del Sistema de Control ................................................................................29Modelado de los Motores CC ...................................................................................... 30

Principios de Operación Básicos de Motores de CC ............................................ 31Modelado Matemático Motores de CC de Imán Permanente ................................ 31Relación Ki y Kb .....................................................................................................35Relación Par-Velocidad de un Motor de CC ..........................................................35

Modelado del Controlador PID ....................................................................................37Edición y Aplicación del Fichero de Control Tipo Simulink ...................................... 37El fichero de Control de Tipo Simulink (Modelado de Motores) ................................38Edición del Fichero de Control ....................................................................................38

2.7.8.4 Guardar Configuración del Robot (función filecfg(...)) ...............................................412.7.8.5 Recuperar Configuración del Robot (función filecfg(...)) ............................................432.7.8.6 Salir de la Pantalla de Diseño del Robot (función quit(...)) .........................................45

1. Índice

ii

2.7.9 Herramientas de la Pantalla de Simulación ........................................................................ 462.7.9.1 Selección del Robot (función chrobot()) ..................................................................... 462.7.9.2 Edición del Robot ........................................................................................................ 472.7.9.3 Mover el Robot con el Mouse (función sagent()) ........................................................472.7.9.4 Actualizar Posición del Robot (función sagentk()) ......................................................492.7.9.5 Selección del Movimiento Programado .......................................................................50

Grasping (función fungras()) .......................................................................................50Funciones Asociadas al Movimiento Grasping ......................................................51

Tracking (función funtrack()) ...................................................................................... 53Forward (función funforw()) ........................................................................................54Other (función funother()) ........................................................................................... 54

2.7.9.6 Borrar un Robot ........................................................................................................... 55Borrar la Estructura de Datos y Gráfico del Robot (función dagent()) ...................... 55Borrar el Setpoint (función delpos(i)) ..........................................................................56

2.7.9.7 Opción Logger ............................................................................................................. 562.7.9.8 Opción Trail .................................................................................................................572.7.9.9 Clear (función limpiar()) ............................................................................................. 572.7.9.10 Reset Simulación (función reset()) ........................................................................... 582.7.9.11 Salir del Simulador ................................................................................................... 58

2.7.10 Pantalla de Diseño de Obstáculos ....................................................................................... 592.7.10.1 Editar los Obstáculos ................................................................................................ 592.7.10.2 Selección del Obstáculo ............................................................................................602.7.10.3 Crear un Obstáculo (función obsta()) ....................................................................... 612.7.10.4 Mover el Obstáculo con el Mouse (función sobsta()) ...............................................642.7.10.5 Actualizar Datos del Obstáculo (función sobstak()) .................................................652.7.10.6 Borrar un Obstáculo (función dobsta()) ....................................................................662.7.10.7 Forma del Obstáculo ................................................................................................. 672.7.10.8 Fichero de Datos de los Obstáculos .......................................................................... 68

Guardar Entorno de Obstáculos (función fileobst()) .................................................68Recuperar Entorno de Obstáculos (función fileobst()) ............................................. 70

2.7.10.9 Salir de la Pantalla de Edición de Obstáculos (función quit()) .................................732.7.11 Inicio, Ejecución de la Simulación .......................................................................................742.7.12 Muestreo ...............................................................................................................................75

2.7.12.1 Limitación del Grain Time (dt) .................................................................................762.7.12.2 Precisión de Movimientos y Tiempo de Refresco de la Pantalla ..............................77

2.7.13 Step a Step (función paso()) ................................................................................................. 782.7.13.1 Cálculo de la Distancia entre el Robot y la Diana (función simgoal(self,a)) ........... 802.7.13.2 Simulación del Fichero Simulink (función dynamics()) ...........................................80

Función de Cálculo de la Nueva Posición (función calxyth()) ..................................82Función de Choque Paredes (Límites de la Simulación) .......................................... 83

Factor de Choque Paredes ................................................................................... 83Función xoc_paret(‘car’,self) ............................................................................. 83Función [factor_xoc,self] = parets(self) ............................................................. 84

2.7.13.3 Función de Detección de Obstáculos (función sensor()) ..........................................85Función Sensor(self,graf_sen,dades_sen,x_obs,y_obs,ra_obs,ESTADO_SENSORES).................................................................................................................................... 85

2.7.13.4 Simulación del Choque entre Robots ........................................................................89Función xoc_robots(self1,j,self2) ..............................................................................89

2.7.13.5 Simulación del Choque Robot-obstáculo ..................................................................91Función xoc_obstacles(self,selfobs) ..........................................................................91

2.7.13.6 Control del Movimiento Asociado a las Acciones del Robot ...................................93Acción Grasping ....................................................................................................... 93

Función Object .................................................................................................... 93Función sgras() ................................................................................................... 94

1. Índice

iii

Función sagent() ..................................................................................................94Función assig_po() ..............................................................................................94

Acción Tracking ........................................................................................................95Función sagent() ..................................................................................................95Función assigpo2() ..............................................................................................95

Acción Forward ........................................................................................................ 96Función sagent() ..................................................................................................96Función assigpo3() ..............................................................................................97

Acción Other .............................................................................................................97Función sagent() ..................................................................................................98Función assigpo4() ..............................................................................................98

2.7.13.7 Función de Programación de la Máquina de Estados ............................................... 992.7.13.8 Función setpos(a,x,y,theta) ....................................................................................... 992.7.13.9 Datos para la Representación Gráfica .......................................................................1002.7.13.10 Control del Total Grain Time ................................................................................100

2.7.14 Run ....................................................................................................................................... 1012.7.14.1 Función de Simulación Continuada mainloop() ....................................................... 101

2.7.15 Tratamiento de los Resultados de la Simulación ................................................................. 1022.7.15.1 Guardar los Resultados de la Simulación (función filedata()) ..................................1022.7.15.2 Recuperar los Resultados de la Simulación (función filedata()) .............................. 1042.7.15.3 Representación Gráfica de los Resultados ................................................................105

2.7.16 Pantalla de Configuración de la Simulación ....................................................................... 1062.7.16.1 Cerrar la Pantalla de Configuración ..........................................................................107

2.7.17 Anexo de funciones auxiliares ..............................................................................................1082.7.17.1 Función de no Superposición noencima(selfobj, i, tipo) ...........................................1082.7.17.2 Función de Control de los Límites de la Simulación bounds.m .............................. 1102.7.17.3 Función [índice] = popup(vector, pop, opción, txt, mensa) ..................................... 1112.7.17.4 Función [distan] = discua(angulo,radio_obs,angu_obs) .........................................113

2.8 Ejemplos de Simulación ................................................................................................................1142.8.1 Modelado del Sistema de Control y de los Motores ............................................................ 114

2.8.1.1 Modelado del Motor .................................................................................................... 1152.8.1.2 Diseño del Sistema de Control .....................................................................................117

Configuración del Controlador (PID) ..........................................................................117Realimentación ............................................................................................................ 118Asignación de los Parámetros del PID ........................................................................ 118

2.8.1.3 Cálculo de la Consigna ................................................................................................ 1202.8.1.4 Estudio de los Resultados ............................................................................................ 1222.8.1.5 Conclusiones ................................................................................................................133

2.8.2 Estudio y Programación de la Máquina de Estados ............................................................1342.8.2.1 Ejemplo del Laberinto ................................................................................................. 135

Conclusiones ................................................................................................................1402.8.2.2 Ejemplo de las Columnas ............................................................................................ 141

Conclusiones ................................................................................................................1452.9 Conclusiones ...................................................................................................................................1462.10 Presupuesto ..................................................................................................................................150

3. Memoria de Cálculo .................................................................................................. 151

3.1 Distancia y Ángulo entre Dos Puntos ...........................................................................................1523.2 Cálculo de la Velocidad Lineal y Angular del Robot .................................................................1533.3 Cálculo del Espacio Lineal y Angular Recorrida por el Robot .................................................154

1. Índice

iv

4. Presupuesto ................................................................................................................ 155

4.1 Precios Unitarios ............................................................................................................................1564.2 Precios Descompuestos de las Subpartidas .................................................................................1574.3 Precios Compuestos de las Subpartidas ...................................................................................... 1634.4 Resumen del Presupuesto ............................................................................................................. 164

5. Pliego de Condiciones ................................................................................................ 165

5.1 Objetivos .........................................................................................................................................1665.2 Materiales .......................................................................................................................................1665.3 Condiciones Generales ..................................................................................................................1665.4 Condiciones Económicas ...............................................................................................................1665.5 Condiciones de Seguridad .............................................................................................................1675.6 Limitación de Responsabilidad .................................................................................................... 167

6. Anexos .........................................................................................................................168

Anexo A. Programa ..............................................................................................................................169

A.1 Funciones de Inicio del Programa ...........................................................................................170A.2 Funciones de Creación de las Pantallas del Programa ............................................................ 175A.3 Funciones Asociadas al Tratamiento de Datos del Robot .......................................................194A.4 Funciones Asociadas al Tratamiento de Datos del Setpoint ................................................... 208A.5 Funciones Asociadas al Diseño del Robot (sensores) .............................................................212A.6 Funciones Asociadas al Tratamiento de Ficheros ...................................................................222A.8 Funciones Asociadas al Tratamiento de Datos de los Obstáculos .......................................... 237A.9 Funciones Asociadas al Tipo de Movimiento en la Simulación ............................................. 247A.10 Funciones Asociadas a la Ejecución de la Simulación .........................................................257A.11 Funciones de Control de los Choques .................................................................................. 274A.12 Función de Tratamiento Gráfico de los Resultados ............................................................. 281A.13 Funciones Auxiliares ............................................................................................................284A.14 Funciones de Fin de la Simulación .......................................................................................288A.15 Función de Cierre y Salida de las Diferentes Pantallas ........................................................290

Anexo B. Presentación de las Pantallas del Simulador .....................................................................297

Anexo C. Anexo de Información Técnica ...........................................................................................302

Referencias ....................................................................................................................... 314

Manual de Usuario


Memoria Descriptiva

2. Memoria Descriptiva

2


2.1 Introducción

El nacimiento de los primeros sistemas en Robótica se produce durante eldesarrollo de la Industria, debido a la necesidad creada por los grandes sistemas deproducción. La utilización de la nueva maquinaria, mejora y aumenta la efectividad de laproducción, de ahí, la importante evolución que estos sistemas han provocado en laindustria. La gran expansión de los sistemas robotizados en aplicaciones para lamaquinaria industrial, ha provocado la necesidad de su estudio y desarrollo en el mundo dela Tecnología de los países más avanzados. El gran avance en la aplicación, y en el númerode los Robots en el mundo científico/Tecnológico se debe a la importancia que en lacultura actual tiene la investigación Tecnológica aplicada a la industria de la producción,donde se obtienen los principales beneficios.

La gran y rápida evolución de los sistemas robotizados, plantea la necesidad decrear simuladores, como el que se desarrolla en este Proyecto, que sirvan de herramienta, yde ayuda, para introducir de forma didáctica al estudiante e investigador en estos sistemas,obteniendo una previsión de resultados antes de realizar el diseño y la programación de lossistemas robotizados.

Realmente, en este Proyecto no se desarrolla un simulador para grandes yavanzados sistemas robotizados, sino que se pretende desarrollar un simulador didácticopara el estudio de pequeños Robots móviles, que sirva de introducción en el desarrollo deeste tipo de sistemas.

2.2 Objetivos del Proyecto

La finalidad principal de este Proyecto es, aportar una herramienta de trabajo en laSimulación de Robots móviles, en un entorno de trabajo interactivo entre el usuario y elprograma. Dentro de este entorno, el usuario debe disponer de diversas herramientas parael control y diseño de la simulación, como por ejemplo; el modelado de los motores (conSimulink), los sensores de proximidad, etc.

No se pretende crear un programa cerrado, en el cual el usuario tenga simplemente,la posibilidad de simular su sistema en unas condiciones estrictas, sino que gracias a lautilización de por ejemplo, ficheros de Simulink, tenemos un programa versátil donde elusuario modela su propio sistema de control.

El programa dispone de pequeñas herramientas para el usuario, y este, debeconstruir su modelo de Robot, para después realizar la simulación y obtener resultados quepuedan ser estudiados de forma didáctica.

Los objetivos del programa se resumen en las siguientes herramientas:- Modelado-simulación del sistema de control de velocidad-posición del Robot.- Simulación-creación de un entorno con obstáculos.- Simulación de un sistema de detección de obstáculos, a partir de sensores.- Programación de una Máquina de estados de los sensores para el control de la

trayectoria del Robot.


3

- Entorno de simulación dinámico para la obtención de resultados, con datos ygráficas para el posterior estudio del modelo realizado.

- Herramientas para: el diseño del robot (sensores, tamaño, etc.), el movimiento delRobot, el entorno de obstáculos, la programación de movimientos del Robot, etc.

SISTEMA DE CONTROL Movimiento coordenadas x, y, ángulo del Robot.Control de velocidad-posición (PID, ...).

PROGRAMACIÓN Posibilidad de realizar un sistema de control a partir del estado de los sensores(Máquina de estados).Programación del movimiento.

ENTORNO Simulación de un entorno con obstáculos para el Robot.RESULTADOS Obtención de la respuesta del Robot (motores, ...), en gráficas y datos, para su posterior

estudio.Tabla 1. Planteamiento de los objetivos del Proyecto

Además, este Proyecto establece un punto de partida para posteriores programas desimulación, en los que se pretenda desarrollar simuladores de otros tipos de sistemasrobotizados u otro tipo de programas en el entorno de MATLAB V4.2.1.c [1].

2.3 Antecedentes

Desde hace años, la creciente evolución y avance de los sistemas robotizados en laaplicación de la producción industrial, ha provocado un necesario estudio-desarrollo deéstos por parte de los diferentes investigadores de nuevas tecnologías (Robótica yAutomatización Industrial).

Hoy en día, esta investigación y estudio de sistemas robotizados, se desarrollatambién en los laboratorios de las universidades, como por ejemplo, en la UniversidadRovira i Virgili de Tarragona (URV), concretamente en el Departamento de IngenieríaElectrónica, Eléctrica y Automática. En la evolución de estos estudios, se ha creídonecesario crear un programa de simulación de robots móviles, en el entorno de MATLAB,que sirva de base, herramienta y estudio del desarrollo de este tipo de sistemas robotizados.

Se han utilizado como referencia y punto de partida para la creación de esteprograma de simulación los siguientes proyectos de la Universidad de Girona:

Peter Libbrecht, (programa Erasmus, Bélgica), Task-Planning in a Multi-robot Environment. Junio, 1997.

Johan Delfosse, (programa Erasmus, Bélgica), Development of PHAG-SIM: A framework to deal with multi-agent systems. Junio, 1998.

2.4 Titular

El titular de este proyecto es la Universidad Rovira i Virgili, Departamento deIngeniería Electrónica, Eléctrica y Automática, ubicada en Tarragona, Autovía Salou-Tarragona s/n la cual es la encargada del Proyecto, realizado por el Ingeniero TécnicoIndustrial Santos Barquero Cáceres.


4

2.5 Destinatario

El proyecto está destinado a la Escuela Técnica Superior de Ingeniería de laUniversidad Rovira i Virgili de Tarragona, concretamente al Departamento de IngenieríaElectrónica, Eléctrica y Automática.

2.6 Posibles Soluciones y Solución Adoptada

Cuando se planteó la idea de realizar un simulador de Robots móviles, además detener muy en cuenta las limitaciones que tendríamos en nuestra simulación, también setuvo muy claro el utilizar el entorno de MATLAB para realizar este Proyecto. Ahora bien,el dilema era qué versión utilizar, ya que disponemos de MATLAB V5, que pone a nuestroalcance potentes herramientas, útiles funciones en librería y un tratamiento de datos ygráficas que permiten una representación dinámica, con las mismas posibilidades e inclusomayores que la versión 4.2c.1. No obstante MATLAB V4.2c.1 [1], tiene unos factores muyimportantes a su favor; el tiempo que lleva en el mercado y la gran cantidad dedocumentación e información disponible.

Por otro lado, cuando planteamos el modo de realizar la simulación, la decisiónestaba en escoger una simulación continua, sin mostrar la evolución del Robot en tiemporeal, o por el contrario y tal como se programa finalmente, realizar pequeños intervalos desimulación, para que entre intervalo e intervalo se muestre la evolución del Robot en lapantalla observando de forma gráfica la respuesta del Robot en tiempo real.

Para que el simulador sea funcional y permita un gran dinamismo en el modeladodel sistema de control del Robot (cálculo de la consigna, motores, etc.), se utilizan losficheros Simulink, aunque en un principio se podía pensar en modelar el sistema de controla partir de lenguaje MATLAB [1]. El programa de simulación se completa con diferentesherramientas para dotar al Robot de un sistema de sensores, que controlando su estado apartir de una Máquina de Estados, corrige la dirección del Robot para evitar el choque conlos obstáculos diseñados por el usuario utilizando las herramientas preparadas a tal efecto.

Estas y otras cualidades (tratamiento gráfico de resultados, movimientosprogramados para el Robot, etc.) describen superficialmente las propiedades del programade simulación, que se presentan de forma más concreta en el siguiente apartado.


5

2.7 Descripción General del Proyecto

2.7.1 Lenguaje de Programación (MATLAB V4.2c.1)

En el inicio de este Proyecto de simulación, cuando se realizó el ante-Proyecto paraplantear y desarrollar los objetivos de este simulador, se hizo necesario el estudio dediferentes lenguajes de programación para la elección del entorno de programación másadecuado. En este caso, se ha decidido utilizar y el trabajar en el entorno de lenguajeMATLAB V4.2c.1 [1], el cual permite: un tratamiento de datos (matrices de datos,estructuras, etc.), cálculos técnicos, soporte de modelado de sistemas Simulink, gráficas,que hacen de este programa el idóneo en la simulación de los sistemas robotizados concomplicados sistemas de datos.

MATLAB V4.2c.1

- Programa de una gran base matemática y de entorno muy versátil.- Importantes y útiles funciones en librería de gran y potente aplicación (lenguaje dealto nivel).- Tratamiento de datos y gráficas que permiten una representación dinámica.- Simulink permite el modelado de los sistemas de control (representación en bloques).

Tabla 2. Cualidades del programa MATLAB [1]

El programa de Simulación de Robots móviles en el entorno de MATLAB V4.2c.1,no debe limitar al usuario en su simulación, además tiene que permitir la interacción através de la pantalla, con el mouse y el teclado. El desarrollo y funcionamiento delprograma necesita de un entorno de trabajo fácil de entender y útil para el usuario en elmodelado de su Robot móvil.

2.7.2 Fases para el Desarrollo y Planteamiento del Programa

En las fases que se presentan a continuación, se observa el desarrollo para lacreación y funcionamiento del programa de simulación.

1- Fase de inicialización del entorno de trabajo (inicialización de las variables, delas funciones y de la pantalla de simulación).

1ª FASEPantalla inicial de entorno de trabajoe interacción con el usuario (pantallacon buttons, espacio de simulacióngráfica, etc.). Inicializar variables yfunciones básicas en el programa.

- Inicializar el entorno de trabajo y Simulación para el usuario.- Establecer unas mínimas condiciones y pasos en la simulación.- Elementos necesarios para la comunicación entre el usuario y elsimulador.- Comunicación entre usuario y programa.

Tabla 3. Descripción de la 1ª fase para el desarrollo y ejecución del simulador


6

2- Una vez que el usuario tiene los medios de interacción con el programa y hainicializado el entorno de trabajo en la simulación, debe crear su sistema robotizado (Robotmóvil).

2ª FASECreación y entorno de edición delRobot móvil, para el modelado delsistema robotizado en la simulación.

- Pantalla de interacción para la edición del Robot.- Herramientas para el modelado del Robot.- Construcción del Robot, tamaño, sensores, etc.- Crear el fichero de control y el modelo para los motores del Robot.


3- Con la creación del modelo de Robot móvil, el usuario antes de iniciar laSimulación, debe establecer unas condiciones en la simulación, como por ejemplo; añadirobstáculos o dar al Robot un estado inicial (se utilizan parte de las herramientas, buttons,etc., creados en la pantalla de Simulación, 1ª fase).

3ª FASEHerramientas para crear el entornode simulación que se pretendaestudiar (obstáculos, posicionar elRobot en unas coordenadasconcretas, etc.).

- Pantalla de edición de obstáculos.- Herramientas de inicialización del Robot, como por ejemplo,setkeyboard, setmouse, delete, etc.


4- Definido el entorno de la Simulación, las herramientas de interacción con elusuario y el modelado del Robot móvil, se procede a dar inicio a la simulación.

4ª FASESimulación paso a paso.Simulación continuada Run.

- Simulación dinámica del sistema modelado ya sea paso a paso(step to step) o continuada (Run).- Observar resultados durante la simulación en el entorno creado.


5- A partir de la simulación, se da inicio a una nueva fase de obtención y estudio deresultados, o por el contrario, se vuelve a la tercera fase para corregir el sistema modeladosegún lo observado en la simulación.

5ª FASEObservar la evolución de la simula-ción.Herramientas de obtención de resul-tados; gráficas y datos.

- Observar evolución del Robot en pantalla.- Herramientas para guardar los datos de la simulación.- Representación gráfica de datos del sistema modelado.- Visualización de los datos y su estudio.


Con las fases descritas anteriormente, el organigrama de la Figura 1 muestra elplanteamiento en bloques que se ha realizado para la creación, desarrollo y funcionamientodel programa de Simulación.


7

1ª fase

2ª fase

3ª fase

4ª fase

5ª fase

Inicio

Repetir simulación

Estudio de resultados

Decisión

Salir de la

Repetir simulación

Finsimulación

Figura 1. Organigrama para el desarrollo y ejecución del simulador

2.7.3 Inicio del Programa (comensa.m)

En el arranque del programa, es necesario definir las condiciones iniciales defuncionamiento y el entorno de la pantalla de simulación para la interacción con el usuario.

La función de inicio de la simulación, comensa(), se divide en las funciones de:configuración del programa y creación del entorno de interfaz entre usuario y el programa(Pantalla de Simulación, Figura 2).

close all; % Cerrar todas las pantallas-gráficas actualesclear all; % Vacía el espacio de trabajo de MATLAB V2.1.cclc; % Limpiar pantalla de trabajo

invarsim; % Función de declaración variables de Simulinkinvargen; % Función de declaración variables globales generalesidsply; % Display-entorno de la pantalla para la Simulación

% (buttons, popups, etc...)

2.7.3.1 Declaración de Variables del Fichero Simulink. Función invarsim()

Función de declaración de las variables globales en el inicio del programa para laejecución de los ficheros tipo Simulink, ya que las variables utilizadas por estos ficherosdeben declararse como globales.

2.7.3.2 Declaración de Variables del Programa. Función invargen()

En esta función, se inicializan las variables globales para la configuración delprograma, leyendo los valores de configuración en el fichero varcfg.m. Este fichero dedatos, se modifica en la simulación cambiando los valores de configuración (barra de menúOptions de la Pantalla de Simulación “Physical Robot Simulator”), o bien editándolodesde el exterior de la simulación.


8

2.7.3.3 Pantalla de Simulación. Función idsply()

La pantalla inicial donde se realiza la simulación, se define en la función idsply().En esta función, primero se caracteriza la figura de “Physical Robot Simulator”, paradespués con la función de aplicación axes [1] establecer los límites de la Simulación. Lautilización de axes sirve para controlar el escalado y la apariencia de los ejes de referencia(definir las unidades de distancia). La simulación dinámica depende de estos ejes creadosal inicio de la función, siendo todos los gráficos posteriores “hijos” de estos ejes.

Después de definir-acotar el espacio de la simulación, se diseña la implementacióngráfica de los interfaces de usuario, con los objetos uicontrol [1], o también llamadoscontroles de interfaz de usuario (existen diferentes tipos de uicontrol que se utilizan a lolargo del programa [1]). Cuando se seleccionan los objetos uicontrol se realiza una acciónpredefinida, es decir, llamada a una función asociada, creada para la interacción con elusuario en la simulación.

Figura 2. Pantalla de simulación

Una vez definida la pantalla principal de Simulación (1ª fase), con los elementos deinterfaz entre usuario y simulador, se da inicio a la 2ª fase de creación-edición del Robotmóvil.

Push-button

Popup

Button radio

Button edit

Barra de menús Button label


9

2.7.4 Creación del Robot y Pantalla de Diseño del Robot

2.7.4.1 Representación Gráfica del Robot (función dibu_a(...))

En el programa, la representación gráfica del Robot se realiza con la función deaplicación patch [1], que es un tipo de “objeto parche”, escalado en los ejes definidos en lapantalla principal (ejes definidos con la variable WORLD_AXIS). El patch gráfico sedefine por cuatro coordenadas (figura cerrada) que se calculan en la función dibu_a(), apartir de la coordenada central del Robot (ver Figura 3).

90º

Radio Robotx,y

radius * cos(theta+(9/2)pi)

radius * sin(theta+(9/2)pi)

radius * cos(theta-(3/4)pi)

radius * sin(theta+(3/4)pi)

theta

y = m*x + n

0º

90º

180º

270º

0º

90º

180º

270º

Ángulo de rotación del Robot theta = 90º, en esta respresentación

80º

radius x,y

Figura 3. Representación gráfica del Robot

function [xydata, redon, graf_sen] =dibu_a(x, y, theta, radius, redon, graf_sen, dades_sen,a);... ... ...% Calculo de las coordenadas para dibujar la forma cónica del Robotxydata(:,1) = [x + radius*cos(theta + (1/8)*pi); ...x + radius*cos(theta - (1/8)*pi); ...x + radius*cos(theta - (3/4)*pi); ...x + radius*cos(theta - (5/4)*pi);];

xydata(:,2) = [y + radius*sin(theta + (1/8)*pi); ...y + radius*sin(theta - (1/8)*pi); ...y + radius*sin(theta - (3/4)*pi); ...y + radius*sin(theta - (5/4)*pi);];... ... ...

El patch gráfico del Robot, definido en ‘Xdata’-‘Ydata’, con cuatro coordenadas,calculadas en la función dibu_a() a partir de la coordenada central de posición del Robot,permite un gran dinamismo gráfico para la simulación, muy útil y de fácil tratamiento.

% Guardar datos modificados del Robot (“i” índice del Robot)set(AGENTS(i),'XData',xydata(:,1),'YData',xydata(:,2));


10

Círculo Espacio Vital

El gráfico del Robot, es una figura adscrita a un círculo, que define la superficieocupada por éste durante la representación en la simulación.

El espacio ocupado por el Robot se aproxima al diámetro de este círculo, llamadoespacio vital, y que puede verse durante la simulación, si no es que se elimina en laconfiguración del programa.

function [xydata, redon, graf_sen] =dibu_a(x, y, theta, radius, redon, graf_sen, dades_sen,a);... ... ...% Dibujar círculo del espacio vital del Robotz = linspace(0,2*pi,MAX_ELE);

s1 = x + cos(z) * radius; % Calculo nuevas coordenadas círculovital del Robot

s2 = y + sin(z) * radius;

set(redon,'XData', s1,'YData', s2); % Set redibujar círculo vitaldel Robot

... ... ...

Dibujo Sensores del Robot

En la función dibu_a(...), además del cálculo de las coordenadas de dibujo delpatch [1] gráfico del Robot, y de redibujar el círculo del espacio vital, también se dibujanlos sensores a partir de los datos almacenados en la matriz dades_sen y con el objeto tipoline [1] llamado graf_sen, creado a tal efecto. La operación de dibujo de los sensores,realizada en forma de bucle uno por uno, recalcula las coordenadas del objeto line [1]graf_sen con los datos de distancia y ángulo del sensor respecto a la coordenada central delRobot.

dades_sen(i,:) =[distan_detec, ang_detec, ang_rotate_sensor, position_dis, position_ang]

x,y 0º

90º

180º

270ºEn este caso,ang_positon = 0 + theta = 90ºang_rotate_sensor = 0º

distan_detec

position_dis

ang_detec

ang_rotate_sensorposition_ang

Figura 4. Representación de los datos caracterísiticos de los sensores


11

function [xydata, redon, graf_sen] = dibu_a(x, y, theta, radius, redon,graf_sen, dades_sen,a);... ... ...% Dibujar los sensores del Robotif nargin > 6

for index=1:MAX_SENSORS % Dibujar sensores

if dades_sen(index,1)~=0 % Existe sensor?

xini = x + dades_sen(index,4) * cos(dades_sen(index,5)+theta); yini = y + dades_sen(index,4) * sin(dades_sen(index,5)+theta);

% Puntos de la representación gráfica del sensor z = linspace((-dades_sen(index,2)/2),(dades_sen(index,2)/2),MAX_ELE);

s1 = xini + cos(z+theta+dades_sen(index,3)) * dades_sen(index,1); s2 = yini + sin(z+theta+dades_sen(index,3)) * dades_sen(index,1);

% Set de graf_sen 'Xdata' 'Ydata' (dibujar sensores) set(graf_sen(index),'XData',s1,'YData',s2,'color',col);

... ... ... end; % end dades_sen(index,1)~=0 end; % end for index=1:MAX_SENSORSend; % end if nargin>6

2.7.5 Estructura de Datos del Robot (función strucrob(...))

Para el tratamiento de datos del Robot, se define al patch [1] gráfico una estructurade datos compacta y dinámica que permita desde cualquier función del programa, obtenerla información referente al Robot de manera rápida y sencilla, tal y como se presenta eneste apartado (ver Figura 5).

Crear Robot 1

AGENTS(2)AGENTS(1)

VECTOR AGENTS

Índices-manejadoresde los nuevos patch UserData

UserData

UserDataMANEJADORES

goal

UserDatagraf_sen

self

Crear Robot 2

dades_senredon

confinamast

graf_sen

confinamastredon

selfgoal

dades_senUserData

Crear Robot 7 7

2

AGENTS(7) graf_sen

confinamastredon

selfgoal

dades_senUserData

1

Figura 5. Estructura de datos característicos del Robot


12

state() = [i self goal confina mast redon graf_sen] donde

i -- índice del Robot(n)

self = [x y theta radius velolineal veloangular pertur] -- datos del Robot, coordenadas, velocidad

goal = [r psi libre?] -- vector coordenadas polares entre diana y Robot

% goal = [r psi libre?] -- coordenadas polares Robot-objetivo (función agent())goal = [0 0 0]; % Vector de datos para la consigna de posición (coordenadas polares)

confina = [control_name_file tipo_control] -- fichero Simulink de control

% 'defaultc' es el fichero Simulink de control, por defecto (función agent())confina = patch('Visible','off','XData',typefile,'YData',0,'UserData', 'defaultc');

mast = [control_name_file tipo_control] -- fichero de programación de la máquina de estados

% 'maes' es el fichero de control máquina de estados, por defecto (función agent())mast = patch('Visible','off','UserData',’maes’);

redon = [gráfico tipo "line" que define el círculo vital del Robot] -- círculo del espacio vital del Robot

% Inicializar objeto tipo "line", círculo de espacio vital (función agent())redon = line('EraseMode','xor','XData',0,'YData',0,'Color','r','Visible',vital);

graf_sen(i) = [datos tipo line sensor "i"] -- datos tipo line, dibujo-gráfico de los sensores

% Inicializar gráfico "line" para dibujar sensores (función agent())graf_sen(j)=line('EraseMode','xor','Visible','on','XData',0,'YData',0,'color','g','UserData',dades_sen);

dades_sen(i,:) = [distancia_detección, ángulo_detección, ang_rotate_sen, position_dis, position_ang]-- dades_sen, es la matriz de datos propios de los sensoresdades_sen = [0 0 0 0 0]; % Inicializar vector de datos referentes a los sensores (función agent())

La estructura se encadena de forma que:

state = [i self goal confina mast redon graf_sen]; -- vector de datos del Robot.

a = patch(...) -- asignar características al manejador del patchgráfico del Robot.

AGENTS(n) = a; -- manejador patch en vector AGENTS(índice delRobot).

set(AGENTS(n), 'UserData', state); -- el vector state es declarado como valor de tipoUserData del manejador del patch guardado enAGENTS(índice del Robot).

Para recuperar los datos relativos al Robot, state tipo UserData, se realiza lasiguiente operación:

state = get(AGENTS(n), 'UserData'); % Vector de datos del Robot


13

El índice apuntador, o también llamado manejador, guardado en el vectorAGENTS(índice del Robot), recupera la estructura de tipo UserData, en forma de vector,para asignarla correlativamente.

i = state(1);self = state(2:8);goal = state(9:11);confina= state(12);mast = state(13);redon = state(14);graf_sen = state(15:(14+MAX_SENSORS-1));

for j = 1:MAX_SENSORS dades_sen(j,:) = get(graf_sen(j), 'UserData');end;

donde, recordar que:

i índice del Robot

self = [x y theta radius velolineal veloangular pertur], donde;self(1) = x coordenada de posición x del Robotself(2) = y coordenada de posición y del Robotself(3) = theta ángulo de posición del Robotself(4) = radius radio-tamaño del Robotself(5) = velolineal velocidad lineal (cm/s) del Robotself(6) = veloangular velocidad angular (rad/s) del Robotself(7) = pertur par de carga (perturbación) en N·m de los motores del Robot

goal = vector con coordenadas en forma polar entre Robot y dianagoal = [distancia ángulo libre?]

confina = patch(...), datos referentes al fichero Simulink, modelado sistema de controlcontrol_name_file = get(confina,’UserData’)tipo_control = get(confina,’Xdata’)

control_name_file nombre del fichero Simulink de control (tipo UserData).tipo_control 0 fichero Simulink modela sólo los motores.

1 fichero Simulink modela todos los cálculos.

mast = patch(...), datos referentes al fichero de programación de la máquina de estados (sensores)fichero_máquina_de_estados = get(mast,’UserData’)

redon = [círculo tipo line], gráfico que define el espacio vital del Robot

graf_sen(i) = [datos tipo line sensor "i"] gráfico tipo line, dibujo de los sensores

UserData de graf_sen(i), vector de datos dades_sen(i,:)set(graf_sen(i),’UserData’, dades_sen(i,:)), donde;

dades_sen(i,:) = [dist_detec, ang_detec, ang_rotate_sen, position_dis, position_ang]


14

La función strucrob(...) puede realizar dos operaciones, dependiendo del número deparámetros de entrada: a) guardar la estructura de datos (datos de entrada) del Robotdeclarándolos como UserData del manejador del patch que define este Robot, y b)devolver la estructura de datos, en los parámetros de salida.

function[state,i,self,goal,confina,mast,redon,graf_sen,dades_sen]=strucrob(a,i,self,goal,confina, mast,redon,graf_sen)... ... ...if nargin == 1 % Recuperar "UserData" state, patch del Robot state = get(AGENTS(a), 'UserData'); i = state(1); self = state(2:8); ... ... ... graf_sen = state(14:(14+MAX_SENSORS-1)); ... ... ...elseif nargin == 8 % Guardar estructura de datos del Robot state = [i self goal confina redon graf_sen]; set(AGENTS(a), 'UserData', state);end; % end if nargin == 1

2.7.6 Funciones Asociadas al Tratamiento de Datos del Setpoint del Robot

Para el tratamiento de datos de la diana es necesario crear unas funciones dinámicasque permitan un tratamiento sencillo y rápido. En los siguientes apartados, se presentan lasfunciones y el código utilizado para definir la estructura de datos del setpoint asociado alRobot.

2.7.6.1 Creación del Setpoint (función position(...))

Cada Robot, tiene asociado una diana (setpoint) para guardar y marcar lascoordenadas de la consigna de posición, es decir, el destino asignado a ese Robot. Estemarcador, no deja de ser un elemento de texto [1] definido con la letra “O”, y con unaestructura de datos de tipo “UserData”. El manejador del elemento de texto se guarda en laposición “i” del vector POSITIONS (“i” índice de la diana que corresponde al mismoíndice del Robot).

function [n] = position(n, x, y, libre)... ... ...%Vector de estado de la diana (coordenadas de posición, etc.)loc=[x y libre];

% Texto que define el gráfico de la diana "O"txt=text(x,y,'O','color',color(n),'visible',diana,'VerticalAlignmen

t','middle','EraseMode','xor','HorizontalAlignment','center','UserData',[n loc]);

% Guardar el manejador del texto (gráfico de la diana)POSITIONS(n) = txt;... ... ...

Las coordenadas marcadas por esta diana, servirán como destino y guía a latrayectoria del Robot (trayectoria controlada por una serie de funciones assig_poX:assig_po, assig_po2, assig_po3 y assig_po4), además, de utilizar el manejador paraguardar y recuperar estas coordenadas, que representan la consigna de posición. El Robot,en caso de estar bien programado y modelado, deberá ir a buscar y tener como objetivo laposición de la diana.


15

Crear Robot 1

AGENTS(2)AGENTS(1)

AGENTS(7)

VECTOR AGENTS

Índices-manejadoresde los nuevos patch UserData

UserData

MANEJADORES

goal

UserData

mast

graf_senredon

2self

Crear Robot 2

Crear Robot 7dades_sen

MANEJADORES

POSITIONS(2)

VECTOR POSITIONSPOSITIONS(1)

POSITIONS(7)

Crear texto setpoint 1



1

2

7

Estructura de datos de los Robots

confina mastredon

confina

graf_sen

goal

1self

dades_senUserData

x, y, libre

x, y, libre

x, y, libre

Figura 6. Estructura de datos del setpoint (diana) asociada al Robot

2.7.6.2 Función Set de Datos en el Setpoint (función setpos(...))

La función, setpos(...), realiza el set de las coordenadas de la diana obtenidas comoparámetro de entrada para guardar la consigna de posición del Robot asociado a esta diana.

1- Recuperar la estructura de datos del setpoint (state = get(POSITIONS(i),'UserData')).2- Comprobar que las coordenadas de entrada no superan el MAX/MIN permitidos.3- Set parámetros de entrada, nueva posición del setpoint.

set(POSITIONS(i),'Position',loc(1:2))4- Guardar parámetros de entrada en la estructura de datos del setpoint.

% Set de las coordenadas de la diana (consigna de posición delRobot "i") xdia = coordenada x ydia = coordenada y

setpos(i, xdia, ydia, libre);

Las coordenadas resultantes, se almacenan en la estructura de texto de la diana

como vector tipo “UserData”.

function [x,y,libre] = setpos(i,x,y,libre)... ... ...% Control límites del terreno de simulación de la diana "i"rk45('bounds',temps,[],[1e-6 temps temps 0 0 2]);... ... ...loc = [xdia ydia libre]; % Vector de estado de la diana(coordenadas de posición, etc.)% Set de la nueva posición x-y de la diana (loc(1) = x loc(2) = y)set(POSITIONS(i),'Position',loc(1:2));

% Guardar datos "UserData" de la diana "i"set(POSITIONS(i),'UserData',[i loc]);


16

2.7.6.3 Función get Datos del Setpoint (función getpos(...))

Los datos de la estructura de la diana, que guarda la consigna de posición delRobot, se pueden recuperar con la función getpos(i,’propertyname’). Esta funcióndevuelve como parámetro de salida, el dato solicitado en el parámetro de entrada“propertyname”, como por ejemplo, el vector de datos de posición de la diana, de formaque:

[selfdia] = getpos(i,'Location'); % Vector de estado de la diana(consigna de posición Robot “i”)% selfdia () = [xdia ydia libre]% selfdia (1) = coordenada x% selfdia (2) = coordenada y% selfdia (3) = variable libre

1- Recuperar la estructura de datos del setpoint (state = get(POSITIONS(i),'UserData')).2- Asignar al parámetro de salida el dato solicitado en el propertyname.

function [value] = getpos(i,propertyname)... ... ...state = get(POSITIONS(i), 'UserData'); % Recuperar del manejador

los datos tipo Userdataloc = state(2:4); % Vector de estado de la diana (coordenadas

x-y, variable libre)... ... ...% Comprobar y devolver en value el dato solicitado en el

propertynameif propertyname(1:2) == 'lo' value = loc;elseif propertyname(1:2) == 'st' value = state;end; ... ... ...

2.7.7 Funciones Asociadas al Tratamiento de Datos del Robot

Como se ha comentado en los apartados anteriores, se ha definido para el Robotuna estructura de datos para guardar diversa información necesaria, para el desarrollo de lasimulación.

En este punto, se presentan las funciones donde se crea esta estructura (funciónagent(...)) además de las funciones asociadas para el tratamiento de los datos durante lasimulación (sagent(...), gagent()), mostrando el código de programa utilizado para crear ydesarrollar la estructura de datos.

2.7.7.1 Creación de un Nuevo Robot (función agent(...))

PUSH-BUTTON Add Robot. El push-button [1] Add Robot, asociado a la función agent(),crea un nuevo Robot. En esta función se inicializa toda la estructura de datos tipo“UserData” del patch gráfico, además de las coordenadas ‘Xdata’-‘Ydata’ del patch [1](función dibu_a(...)) para la representación gráfica del Robot. El manejador del patch seguarda en la posición del vector AGENT correspondiente al índice del Robot(AGENT(índice del Robot)). La estructura de funcionamiento de la función agent() se basaen los siguientes puntos:


17

1- Crear diferentes elementos de la estructura de datos (redon, goal, confina, etc.).2- Asignación del índice al nuevo Robot.3- Lectura de las coordenadas y águlo de rotación inicial (limitación a el MAX/MIN).4- Evitar superposición con otros elementos de la simulación.5- Definir coordenadas del patch gráfico que define el Robot.6- Crear patch gráfico del Robot.7- Guardar la estructura de datos del Robot.8- Crear setpoint asociado al Robot.9- Renovar popup de índices.10- Crear variables de datos para los resultados de la simulación.11- Abrir pantalla de diseño del Robot.

function[n]=agent(n,self,filename1,typefile,filename2,graf_sen,dades_sen) ... ... ... dades_sen = [0 0 0 0 0]; %Inicializar vector de datos de los

sensores % Inicializar gráfico "line" para dibujar sensores for j = 1:MAX_SENSORS

graf_sen(j)=line('EraseMode','xor','Visible','on','XData',0,'YData', 0,'color','g',...

... ... ... if nargin < 2 % Si parámetros de entrada < 2, nuevo índice

blankspots = find(AGENTS == 0); % posiciones igual a cero % El valor "n" contiene el nuevo índice del Robot if blankspots

n = blankspots(1); else

n = length(AGENTS) + 1; end % Lectura de los datos introducidos por teclado x = str2num(get(x_coor,'string'));% Leer coordenada "x" del button

edit ... ... ... ... % Control límites del terreno de juego del Robot "a" rk45('bounds',temps,[],[1e-6 temps temps 0 0 2]); ... ... ... end; % end if nargin < 2

% Evitar superposición Robot con otros elementos de la simulaciónerror = noencima(self,n,'r');... ... ...% Función "dibu_a" para definir las coordenadas de dibujo del Robot[xydata redon] = dibu_a(self(1),self(2),self(3),self(4),redon);

% Crear el patch gráfico que define al Robot.a = patch('XData', xydata(:,1), 'YData', xydata(:,2),'EraseMode',

'xor', 'FaceColor', color(n),...

% Guardar los diferentes patch de datos del Robot en el vectorstate, definido como dato% "UserData" del patch principal del Robot.state = [n self goal confina mast redon graf_sen];

set(a,'UserData',state);% El manejador del patch se guarda en la posición "n" del vector

AGENTS.AGENTS(n) = a;


18

El Robot, tiene asociados diferentes elementos en la simulación, como la diana, quetambién se crea en esta función agent(), gracias a la función position(i) (”i” índice delRobot).

% Crear setpoint asociado al Robot (consigna de posición del Robot)position(n);

Además en esta función se realizan otras operaciones, como el renovar el popup delos Robots con el nuevo índice y la inicialización de la estructura de datos (matriz) para larepresentación gráfica de los resultados correspondientes al Robot (variables log... deintensidad, velocidad y par de los motores).

% Función renovar índices de los Robots en el popup de la pantallapopup(AGENTS,agNum,0);

Finalmente, desde la misma función agent() se abre la pantalla de diseño del Robot,presentando el entorno de interfaz con el usuario para la edición-diseño del Robot.

% Función presentación de la pantalla de diseño del Robot (tamaño,sensores, etc).

edirobot(n);

2.7.7.2 Función set Datos en la Estructura del Robot (función sagent(...))

En la función sagent((i,’propertyname’,value), se realiza el set de los datospertenecientes a la estructura del Robot, además de redibujar el Robot en una nuevaposición al realizar el set del vector de estado (self).

[value] = sagent(i,’propertyname’,value);

Cuando se desea modificar alguno de los datos de la estructura del Robot, y a la vezalmacenarlo, se utiliza esta función, como se puede observar a lo largo de todo elprograma.

function [value] = sagent(i, propertyname, value, selfobj)... ... ...elseif nargin > 2 propertyname = lower(propertyname);

if propertyname(1:2) == 'se'% self=[x y theta radius v omega pertur]

self = value; -- vector de estado del Robot draw = 1; elseif propertyname(1:2) == 'co' confina = value; % files = [control_name_file] ... ... ... elseif propertyname(1:2) == 'go' % goal = [r psi libre?] -- coordenadas polares Robot-objetivo goal = value; end; % end if propertyname(1:2) == 'se'end; % end if nargin == 0... ... ...% Función guardar la estructura de datos del Robotstrucrob(i,i,self,goal,confina,mast,redon,graf_sen);


19

2.7.7.3 Función get Datos de la Estructura del Robot (función gagent(...))

En el caso anterior, se trataba de guardar el dato (set) en la estructura, pero, con lafunción gagent(i,’propertyname’), el dato se recupera (get) de la estructura del Robot.

[value] = gagent(i,’propertyname’);

Esta función es muy útil, al igual que en el caso de sagent(...), para trabajar con losdatos de la estructura del Robot durante el desarrollo del programa.

function [value] = gagent(i, propertyname)... ... ...% Función leer la estructura de datos del Robot[state i self goal confina mast redon graf_sen dades_sen] =

strucrob(i);

% Comprobar y pasar el value según el propertyname de entradaif nargin == 2 propertyname = lower(propertyname); if propertyname(1:2) == 'se' value = self; % self = [x y theta radius v omega

pertur] -- vector de estado del Robot elseif propertyname(1:2) == 'go' value = goal; % goal = [r psi libre?] -- coordenadas

polares Robot-objetivo elseif propertyname(1:2) == 'co' value = confina; % files = [control_name_file] ... ... ...end;


20

2.7.8 Pantalla de Diseño del Robot

La Pantalla de diseño del Robot, figura “Design Robot”, declarada en la funciónedirobot(i) (donde “i” corresponde al índice del Robot) crea una nueva figura, con unnuevo entorno de interfaces entre el usuario y el programa (push-buttons [1], edit-buttons[1], etc.). En esta misma pantalla se representan unos nuevos ejes de referencia, con ungráfico del Robot ampliado, dotando a este gráfico de todas las características propias delRobot en el momento de ser editado (sensores, tamaño, etc.)

En esta pantalla se dispone del interfaz para la interacción con el usuario (Figura7):

- Cambio de tamaño del Robot- Sensores: Add, Del, Update- Edición, creación del fichero de control con Simulink (cálculo de consigna,

motores, PID, etc.)

Figura 7. Pantalla de diseño del Robot

Barra de menús

Popup de índices de los sensores

Push-button

Ventana de diseño

Button tipo radio

Buttons edit


21

2.7.8.1 Cambio de Tamaño del Robot

PUSH-BUTTON Change Size. El push-button Change Size, asociado a la función chsize()realiza la operación de cambio de tamaño. La estructura de funcionamiento de esta funciónse basa en los siguientes puntos:

1- Recuperar la estructura de datos del Robot.2- Validar el dato introducido por teclado del nuevo radio.3- Redibujar el Robot con el nuevo tamaño.4- Función escribir-guardar la estructura de datos del Robot.

El dato introducido con el teclado por parte del usuario en el button tipo edit [1], esvalidado con el push-button Change size, asociado a la función chsize(), donde se realiza elcambio de tamaño.

function[] = chsize()... ... ...% Lectura del nuevo radio, transformar de string a númeroradius_nuevo = str2num(get(size_ro,'string'));% self(4) guarda elradius viejo

Antes de realizar cualquier modificación, es necesario comprobar que los datosintroducidos sean válidos, y en caso de superar los valores configurados de máximo ymínimo, corregirlos.

function[] = chsize()... ... ...% Comprobar datos correctos de tamañoif MAX_SIZE < str2num(get(size_ro,'string')) set(size_ro,'string',MAX_SIZE);

elseif MIN_SIZE > str2num(get(size_ro,'string')) set(size_ro,'string',MIN_SIZE);end;... ... ...

x,y 0º

90º

180º

270º

distance2 distance1

0º180º

270º

90º

x,y

radio_anterior radio_nuevoFigura 8. Ejemplo gráfico para el cambio de tamaño del Robot

En caso de haberse definido algún sensor, al cambiar el tamaño del Robot, esnecesario recalcular la nueva posición de los sensores respecto al centro del Robot (verFigura 8).

)1(nuevo_radio*anterior_radio

2cetandis1cetandis =

distance1 = nueva distancia origen sensor-centro del Robotdistance2 = anterior distancia origen sensor centro del Robot


22

2.7.8.2 Diseño y Creación de Sensores de Proximidad

En un sistema robotizado, existen diferentes sensores: encoders, de contacto, deproximidad, etc., que aportan diversa información al sistema central de control. En estecaso de simulación de un Robot móvil, es de gran utilidad y aplicación el diseño de unsistema de sensores de proximidad como el que se presenta en este apartado.

El sistema que se presenta, permite crear un número de n sensores de proximidad(limitado en la configuración del programa), con características aportadas por parte delusuario gracias a los elementos de interacción de la pantalla de diseño del Robot. Estossensores, servirán para aportar información sobre posibles obstáculos, y así, programar laactitud y la trayectoria del Robot ante estos obstáculos (programación de la Máquina deEstados).

En los siguientes apartados, se explican los diferentes elementos de interacción parala creación de los sensores.

Crear un Sensor (función asensor())

PUSH-BUTTON Add sensor. Asociado a la función asensor(), se encarga de crear un nuevosensor, con los datos aportados por el usuario en los buttons tipo edit. Esta función utilizala estructura de datos del Robot referente a los sensores, formada por el vector demanejadores de elementos tipo line graf_sen(...) [1] y de la matriz de datos de estossensores dades_sen(i,j):

dades_sen(núm_sensor 1, j) = [distancia_sensor, ángulo_ sensor, ángulo_rotate_sensor,position_dis, position_ángulo]dades_sen(núm_sensor 2, j) = [distancia_sensor, ángulo_ sensor, ángulo_rotate_sensor,position_dis, position_ángulo]... ... ...dades_sen(núm_sensor i, j) = [distancia_sensor, ángulo_ sensor, ángulo_rotate_sensor,position_dis, position_ángulo]

dato_columna 1 = distancia de detección del sensor (distancia_sensor)dato_columna 2 = ángulo de detección del sensor (ángulo_ sensor)dato_columna 3 =ángulo de rotación del sensor respecto a su punto de origen (ángulo_rotate_sensor)dato_columna 4 =distancia entre el centro del Robot y el punto de origen del sensor (position_dis)dato_columna 5 =ángulo entre el centro del Robot y el punto de origen del sensor (position_ángulo)

En la Figura 9, se muestra la significación de cada dato almacenado en la matrizdades_sen, pero, se ha de destacar que el ángulo de rotación se mide respecto al origen delsensor, con un ángulo positivo cuando se gira al contrario de las agujas del reloj, ynegativo si se gira en el mismo sentido horario.

Es importante no confundir el ángulo de rotación, con el ángulo de posición delsensor referenciado respecto al origen del Robot. Dependiendo de la posición del sensor enel cuerpo del Robot, en el frontal, lateral, parte trasera, el ángulo de rotación necesita unosajustes, antes de ser almacenado en la matriz dades_sen, restándole un ángulo de 90º, ya


23

que el Robot que se presenta en la ventana de diseño tiene un ángulo de rotación pordefecto de precisamente 90º.

x,y 0º

90º

180º

270º 270º

180ºx,y 0º

90º

En este caso,ang_position = 0

En este caso,ang_position = 270º (-90º)

ang_rotate_sensor = 0º

ang_rotate_sensor = 0º

ang_sensor

distan_position

ang_positionang_position

ang_sensor

distan_position distan_sensor

distan_sensor

Figura 9. Significado de cada dato característico de los sensores

x,y 0º

90º

180º

270º 270º

180ºx,y

0º

90ºen este caso,entre 0º y -90º

en este caso,entre 0º y 90º

ang_sensor

ang_rotate_sensor

distan_sensor

distan_sensor ang_sensor

ang_rotate_sensor

Figura 10. Ejemplo del significado del dato ang_rotate_senor

Conocida la estructura de datos del sensor, se presentan a continuación los puntosque se han seguido para desarrollar la función asensor():

1- Recuperar la estructura de datos del Robot.2- Leer índice del sensor.3- Leer buttons tipo edit, comprobando que los datos introducidos sean correctos.4- Asignar posición con el mouse al sensor.5- Calcular distancia y ángulo desde el centro del Robot al punto de origen del sensor.6- Función dibujo del sensor acabado de crear.7- Función escribir-guardar el nuevo sensor en la estructura de datos del Robot.


24

Comprobados los valores introducidos por parte del usuario en los buttons tipo edit,el programa permite a través del mouse, la colocación exacta del origen del sensor en laestructura del Robot (en el borde de su cuerpo), asignándole un índice correlativo a sucreación. Si la colocación es correcta, se han de calcular la distancia y el ángulo entre elcentro del Robot y el punto origen del sensor (dades_sen(n,4) y dades_sen(n,5)).

function[] = asensor(n)... ... ...% Pedir coordenadas para la posición del sensor si no hay problemaspts = ginput(1); % Coordenadas, con clic de mouse

% Calcular distancia y ángulo respecto al centro del Robotdistance = sqrt((x0_ampli-pts(1,1))^2+(y0_ampli-pts(1,2))^2);% Distancia

angle = atan2((pts(1,2)-y0_ampli),(pts(1,1)-x0_ampli)); % Ángulo

% Comprobar que la situación establecida del sensor es correctaif (distance>(self(4)*1.1*ampli)) | (distance<(self(4)*0.25*ampli))set(text2_error,'String','Warning: Point of the sensor too much

far','visible','on'); ... ... ... return;end; % end if (distance>(self(4)*1.1*ampli))...

Si el sensor se coloca en la parte delantera o trasera, el ángulo de posición de sensorintroducido en la matriz dades_sen siempre es proporcional a 5 grados, para conservar unacierta simetría en la distribución de los sensores.

function[] = asensor(n)... ... ...% Controlar el valor del ángulo calculado, para adaptarlo a unángulo proporcional a 5% grados para conservar una cierta simetría

for j = 1:64 if (j*(pi/32)) >= angle j = j - 1; angle = (pi/32) * j; break;end;end;

En cambio, en los laterales del Robot, el ángulo es exactamente el introducido almarcar el origen del sensor con el mouse, debido a problemas derivados de la forma cónicadel Robot, que se comenta a continuación. La colocación del sensor en los laterales delRobot requiere de un cálculo diferente al utilizado si se coloca en la parte frontal o trasera,debido a la forma cónica del Robot (ver Figura 11).

Cuando se detecta la colocación del sensor en alguno de los laterales del Robot, sealmacena la coordenada “y” obtenida del mouse, entonces a partir de una serie de cálculos,se obtiene la pendiente de la recta que representa los laterales del Robot. Para después, conesta pendiente y la coordenada ”y” almacenada anteriormente, se calcula el valor de lacoordenada “x”. Ahora, sólo cabe calcular como en el caso de la colocación en la partefrontal o trasera, la distancia de posición y el ángulo de posición desde el centro del Robota las coordenadas de origen del sensor acabadas de calcular.


25

x,y en este caso,

distan_position270º

180º entre 0º y 90º0º

distan_sensor ang_sensor

ang_rotate_sensor

90º

Figura 11. Ejemplo de colocación del sensor en la lateral del Robot

Conociendo todos los datos del sensor, se procede a guardar estos datos en la matrizdades_sen y a partir de graf_sen, objeto de dibujo tipo line [1] de la estructura de datos delRobot, se realizará la representación gráfica. Este objeto de dibujo tipo line, graf_sen, y suUserData, la matriz dades_sen, facilitan el manejo de los datos del sensor durante lasimulación dinámica. La misma función, dibu_a(), que calcula las coordenadas de dibujodel Robot a partir de la coordenada central de éste, también se encarga de dibujar lossensores con los datos de la matriz dades_sen.

function[] = asensor(n)... ... ...% Lectura de los datos característicos del sensor% Lectura button edit distancia deteccióndades_sen(n,1)=str2num(get(dis_sen,'string'));... ... ...% Función dibu_a(), cálculo de las coordenadas del Robot número “i”a partir de las coordenadas centrales x-y, además de su ángulo degiro, theta.% El objeto tipo line graf_sen, dibuja los sensores, con los datosde la matriz dades_sen.[xydata redon graf_sen]=dibu_a(x,y,theta,radius,graf_sen,dades_sen,i)

% Set de las nuevas coordenadas en el patch AGENTS(i)... ... ...% Función strucrob, escribir-guardar la estructura de datos delRobotstrucrob(i,i,self,goal,confina,mast,redon,graf_sen)

Una vez creado el sensor, se renueva el popup de la pantalla de edición, añadiendoel nuevo índice.

function[] = asensor(n)... ... ...% Función renovar índices sensores en el popup de la pantalla de

diseñopopup(dades_sen(:,1),num_sen,0);


26

Seleccionar el Sensor (función chcolsen())

Cuando con el popup [1] de índices se elige el índice de un sensor, éste cambia decolor gracias a la función chcolsen() asociada al popup. Esta función, se define en lossiguientes puntos:

1- Leer índice sensor del popup de la pantalla.2- Bucle para el cambio de color de los sensores.3- Cambiar el color del sensor elegido y set de los datos característicos en los buttons tipo

edit.

function[] = chcolsen(i)... ... ...if nargin == 0 % Si no hay índice de entrada % Índice sensor seleccionado en el popup i = popup(dades_sen(:,1),num_sen,1,text2_error,'Warning: Not

exist sensor');... ... ...end;end;... ... ...% Inicializar el color de todos los sensores a verdefor index = 1:MAX_SENSORS if index == i % Índice seleccionado en el popup set(graf_sen(index),'color','m'); % Cambiar el color del

sensor "i" seleccionado en el popup else set(graf_sen(index),'color','g'); % Color sensor a verde end; % end if index == iend; % end for index = 1:MAX_SENSORS

Pero, además de cambiar el color, se leen los datos característicos del sensorseleccionado y se realiza el set de estos datos en los buttons tipo edit de Distance, Anglesensor y Rotate sensor.

function[] = chcolsen(i)... ... ...% Set de los parámetros del sensor "i" en los buttons tipo edit% Set distancia de detección del sensor 'i'set(dis_sen,'string',dades_sen(i,1)/ampli);% Set ángulo de detección del sensor 'i'set(ang_sen,'string',dades_sen(i,2)*180/pi);

% Set del ángulo de rotación del sensor ‘i’ en grados

if (dades_sen(i,5)>=(3*pi/4)) & (dades_sen(o,5)<=(5*pi/4)) set(rot_sen,'string',(dades_sen(i,3)-pi)*180/pi); return; % Parte traseraelseif (dades_sen(i,5)>(pi/8)) & (dades_sen(o,5)<(3*pi/4)) set(rot_sen,'string',(dades_sen(i,3)-(pi/2))*180/pi); return; % Lateralelseif (dades_sen(i,5)>(5*pi/4)) & (dades_sen(o,5)<((2*pi)-pi/8)) set(rot_sen,'string',(dades_sen(i,3)-(3*pi/2))*180/pi); return; % Lateralend; set(rot_sen,'string',dades_sen(i,3)*180/pi);


27

El ángulo de rotación de los sensores tiene por defecto un ángulo añadido a cadasensor debido a la posición de éste en la estructura del Robot tal y como se observa en lasiguiente representación.

x,y

180º

90º

-90º

Figura 12. Ángulo de rotación mínimo por defecto según la posición del sensor

Cuando se resta este ángulo, el ángulo almacenado en la estructura de datos delRobot, matriz dades_sen(i,j), se obtiene el ángulo de rotación del sensor respecto a suorigen que es realmente el dato útil para el usuario.

Actualizar Datos de un Sensor (función updasen())

PUSH-BUTTON Update sensor. Asociado a la función updasen(), carga los nuevos datosintroducidos por teclado en los buttons tipo edit, para modificar las características delsensor.

1- Leer índice del sensor en el popup de la pantalla (índice sensor “i”).2- Recuperar la estructura de datos de los sensores del Robot (llamada a la función

asensor(i)).2.1- Comprobar que los datos introducidos sean correctos.2.2- Aplicar los nuevos datos leídos.2.3- Función dibujo del sensor y del Robot.2.4- Escribir-guardar la estructura de datos sensores del Robot.

Para renovar los datos de un sensor ya creado, primero se elige el índice en elpopup de la pantalla de edición. En esta pantalla, aparece en los buttons tipo edits, losdatos relativos al sensor elegido, gracias a la función chcolsen(), asociada al popup deíndices. Estos datos en los buttons tipo edit, se renuevan con el teclado por parte delusuario y después se validan con el push-button Update. La función updasen() asociada aeste button, lee el índice del sensor en el popup de la pantalla, y llama a la funciónasensor(i), con el parámetro de entrada “i”, que corresponde al índice leído. En estafunción, asensor(i), al tener como entrada un índice “i”, ya no crea un nuevo sensor, sinoque le renueva los datos con los valores de los buttons tipo edit. Es decir, gracias a estesistema, se aprovecha el código de programa de la función asensor(i), sin alargar ni repetirla programación en la acción de actualización.


28

function[] = UpDatesensor()... ... ...% Control del número de sensor, leyéndolo del popup de la pantalla% [sen1 sen2 ... sen"n"] vector con los índices de los sensoresi = str2num(get(num_sen,'String'));i = i(get(num_sen,'Value')); % i = i(n);% Llamada a la función de añadir sensor, pasando como entrada elnúmero de sensor “i”, para cargar los nuevos datosasensor(i);

NOTA: Recordar que cuando se crea un sensor, aparece junto a éste, su índice en colorrojo.

Borrar un Sensor (función dsensor())

PUSH-BUTTON Del sensor. Asociado a la función dsensor(), borra el sensor seleccionadocon el popup de la pantalla de diseño.

En esta función dsensor(), se inicializa a cero la fila “i” de la matriz dades_sen(i,j),y el objeto tipo line, graf_sen(i) (“i” corresponde al índice del sensor leído del popup de lapantalla). En ese instante, se borra el sensor “i” ya que al inicializar los ‘Xdata’-‘Ydata’ degraf_sen(i), además, se renueva el popup de los índices de los sensores en la pantalla dediseño, con los sensores aún existentes. Más que borrar, se puede entender como unvaciado de los datos, ya que el utilizar la orden delete, supondría perder el objeto linegraf_sen(i), creado para la representación gráfica del sensor.

1- Leer índice sensor del popup de la pantalla.2- Recuperar la estructura de datos de los sensores del Robot.3- Inicializar fila de matriz de datos y borrar line gráfico del sensor.4- Escribir-guardar la estructura inicializada de datos del sensor borrado.

Borrar el sensor, y eliminar los datos de la estructura del Robot, se realiza de lasiguiente manera:

function[] = dsensor(i)... ... ...% Borrar los datos del sensordades_sen(i,:) = [0 0 0 0 0]; % Borrar datos sensor "i"% Inicializar line gráfico sensor "i", y el dato de "UserDataset(graf_sen(o),'XData',0,'YData',0, 'UserData',dades_sen(i,:));% Función guardar la estructura de datos del Robotstrucrob(a,j,self,goal,confina,redon,graf_sen);

% Borrar sensor de la pantalla de diseñostate = get(AGENT_AMPLI, 'UserData');graf_sen = state(1:MAX_SENSORS);% Inicializar line gráfico sensor "i", y el dato de "UserData"set(graf_sen(i),'XData',0,'YData',0, 'UserData',dades_sen(i,:));... ... ...

En esta función también se permite borrar el sensor “i”, pasando este índice comoparámetro de entrada, dsensor(i). Entonces, el índice ya no es leído del popup de lapantalla. Una útil herramienta en caso de querer borrar algún sensor desde cualquierfunción del programa.


29

2.7.8.3 Fichero Simulink de control

Modelado del Sistema de Control

En la simulación, es muy importante el modelar un sistema de control (motores,controlador, cálculo de consignas, etc...) que se aproxime todo lo posible a la realidad. LosRobots móviles, basan su funcionamiento en un sistema de control digital, semejante alque se presenta en la Figura 13 [2] [3] [5] [11].

S/Hy

A/D DIGITALD/A MANTENEDOR

ACTUADORPWM

ETAPA DEPOTENCIA

MOTORESCONSIGNA

RELOJMUESTREO

TRANSDUCTORSENSORES

S/H = muestreo y retención (sample & hold)A/D = conversor analógico/digitalD/A = conversor digital/analógico

SISTEMA DE CONTROL DE LOS MOTORES (CPU CENTRAL ó INDEPENDIENTE)

+-

CONTROLADORCALCULO

CONSIGNAS

CPU CENTRAL

Figura 13. Diagrama de un sistema de control con retroalimentación

En la Figura 13 se aprecia como el sistema de control de los motores puede estarintegrado en el sistema de control central o, por el contrario, ser independiente y recibirsólo el valor de la consigna. Ahora bien, el control de la CPU central puede realizarse dedos modos, tal y como se expone a continuación.

REMOTE-BRAINLESS

El Robot, tiene su propio sistema de dirección, controlado a partir de una CPUcentral propia. El control de los diferentes datos estratégicos: de posición, de velocidad,etc., son transmitidos y controlados por la CPU central, y no por el µC del Robot. En laCPU central se toman las decisiones y se realizan los cálculos, transmitiéndolos al Robot através de un sistema de comunicación.

Características:

- Necesita una CPU central de gran potencia, en caso de existir más de un Robot.- El sistema de comunicación debe de transferir muchos datos de control.- El µC del Robot realiza pocas operaciones.

BRAIN ON-BOARD

En este sistema, el µC del Robot toma todas las decisiones sobre sus funcionesvitales: velocidad, posición, etc... El sistema central de CPU (en caso de existir) tomadecisiones poco relevantes en el control interno del Robot.


30

Características:

- La CPU central de poca potencia de cálculo, menos sobrecargada.- El sistema de comunicación transfiere pocos datos, menos sobrecargado.- El µC al realizar más operaciones del Robot, necesita ser de mayor potencia.

En la simulación, no afecta el tipo de control central utilizado, sino que únicamentese pretende simular-modelar la respuesta de los motores y del controlador, para observarlos resultados del sistema motriz del Robot y estudiar como afecta el tipo de motores y/ode control utilizado. Es decir, se simula el control interno que pueden realizar los sistemasde control anteriormente comentados sin conocer si es Remote-Brainless o Brain on-board.

El fichero de control Simulink, donde se modela el Robot móvil, se presentadividido en bloques en el diagrama de la Figura 14:

VARIABLES

(MANTENEDOR)

+-

ENTRADA CALCULOCONSIGNA CONTROLADOR MOTORES

MODELO OBTENERRESULTADOS

FEEDBACK

Figura 14. Diagrama en bloques de un sistema de control con retroalimentación

Ahora bien, lo importante en este programa de simulación es que el usuario modelelibremente los motores y el sistema de control. En el fichero simulink de control utilizadopor defecto en la simulación, defaultc.m, se aplica un ejemplo, utilizando motores de CC ycontroladores tipo PID. El modelado de estos motores así como, el modelo del controladorPID, se explica en los apartados siguientes.

Modelado de los Motores de CC

Los motores de CC, son los sistemas motrices más avanzados y utilizados en laactualidad para las aplicaciones de control en los Robots móviles (como se requiere en esteProyecto). Con anterioridad, para estos propósitos de control, se utilizaban pequeñosservomotores de variedad CA. Éstos son de difícil control, sobretodo en aplicaciones decontrol de posición. Además de tener características no lineales, que los hacen másdifíciles de analizar y modelar. Por otro lado, la utilización de los motores de CC conllevados inconvenientes: aumento del coste, al ser estos más caros debido a las escobillas yconmutadores, y las limitadas aplicaciones de control de los motores de CC de flujovariable. Realmente, la evolución de los motores de CC se debe a la utilización de latecnología de imán permanente “tierras raras”, consiguiendo unas características muyapropiadas para las aplicaciones de control [3] [4]:

- Alta relación par-volumen- Costo razonable- Mínimo mantenimiento- Aumento de los sistemas electrónicos de control de potencia- Baja inercia- Par-inercia muy alta- Constantes de tiempo bajas


31

Principios de Operación Básicos de Motores de CC

La idea de funcionamiento de un motor de CC es la de convertir energía eléctricaen energía mecánica, actuando como un transductor de par (energía eléctrica-energiamecánica). De forma que se obtiene directamente un par en el eje proporcional, al flujo enel campo y en la corriente de la armadura [4].

)2(iKT amm φ=

Tm Par del motorφ Flujo magnéticoia Corriente de armadura

Cuando el rotor del motor se mueve en el campo magnético se genera un voltaje ensus terminales. Este voltaje, la fuerza contraelectromotriz, es proporcional a la velocidaddel eje, y tiende a oponerse al flujo de corriente.

)3(Ke mmb ωφ=

eb Fuerza contraelectromotrizωm Velocidad del eje

Modelado Matemático Motores de CC de Imán Permanente

Para poder estudiar la aplicación en este Proyecto de un motor de CC en el sistemade control del Robot, se establece la necesidad de un modelado matemático de éste. Eldiagrama posterior muestra un circuito equivalente, para representar un motor de CC deimán permanente. La armadura, está modelada con un circuito con resistencia Ra conectadaen serie a una inductancia La, con una tensión eb de alimentación que representa la fuerzacontraelectromotriz en la armadura cuando gira el rotor [4] [11].

-

+

-

e

+

R L

i

T

M

ωT

magnéticoFlujo

bea

O

O m

m

L

a

a

a

Figura 15. Equivalente eléctrico de un motor de CC

En el circuito anterior (Figura 15), la tensión ea(t) representa la tensión de controlque se aplica en los terminales de la armadura. Un análisis lineal, supone que el pardesarrollado por el motor es proporcional al flujo en el entre hierro, y a la corriente de laarmadura.


32

)4()t(i)t(K)t(T amm φ=

Siendo φ(t) cte., la ecuación se escribe como:

)5()t(iK)t(T aim = donde Ki es la cte. de par (N-m/A).

Las ecuaciones resultantes del circuito anterior son:

)6()t(eL1)t(i

LR)t(e

L1

dt)t(di

ba

aa

aa

a

a −−=

)7()t(Kdt

)t(dK)t(e mbm

bb ω=θ=

)8(dt

)t(dJB)t(T

J1)t(T

J1

dt)t(d m

m

mL

mm

m2

2m θ−−=θ

donde TL(t) representa el par de carga debido a la fricción de Coulomb.

Las variables de estado del sistema se pueden definir como ia(t). Al sustituirdirectamente y al eliminar las variables que no son de estado en las anteriores ecuaciones:

)9()t(T

0J10

)t(e00

L1

010

0JB

JK

0LK

LR

dt)t(d

dt)t(d

dt)t(di

Lm

a

a

m

m

m

i

a

b

a

a

m

m

a

−

−−

−−

=

θ

ω

Del modelado en la Transformada de Laplace, se obtiene la función detransferencia entre la velocidad de entrada y la de salida, es decir, la respuesta de velocidada una consigna dada en la entrada. En el diagrama de bloques de la Figura 16, se aprecia latransferencia entre las diferentes variables de estado, presentando la función detransferencia entre ωREF(s) y ωm(s) [4] [11].

aE (s) iKL s + R- a a

LT (s)

1+ i (s)am(s)θ

K b

+ J s + Bm m s(s)T (s)m - 1 ωm 1

e (s)b

Figura 16. Diagrama de bloques del modelado de un de un motor de CC (2º grado)


33

)10()BRKK(s)LBJR(sJL

K)s(E)s(

maibamma2

ma

i

a

m

++++=ω

ia(t) = corriente de armadura (A) Ra = resistencia de armadura (Ω)eb(t) = fuerza contraelectromotriz (V) TL(t) = par de carga (N·m)Tm(t) = par del motor (N·m) θm(t)= desplazamiento del rotor (rad)Ki = constante del par (N·m/A) La = inductancia de la armadura (H)ea(t) = voltaje aplicado (V) Kb = cte. de la fuerza contraelectromotriz (V·s/rad)φ(t) = flujo magnético en el entre hierro ω(t) = velocidad angular del rotor (rad/s)Jm = inercia del rotor (Kg.m2) Bm = coeficiente de fricción viscosa (N·m/(rad/s))

Aunque un motor de CC es por sí mismo un sistema en lazo abierto, el diagrama debloques de la Figura 16 muestra que el motor tiene “interconstruido” un lazo realimentadoprovocado por la fuerza contraelectromotriz. Físicamente, la fuerza contraelectromotrizrepresenta la realimentación de la señal que es proporcional al negativo de la velocidad delmotor. Como se observa en la ecuación 10 de la función de transferencia, la constante de lafuerza contraelectromotriz Kb representa un término que se añade a la resistencia Ra y alcoeficiente de fricción viscosa Bm. Por tanto, la fuerza contraelectromotriz es equivalente auna “fricción eléctrica” que tiende a mejorar la estabilidad del motor, y en general, laestabilidad del sistema [4] [11].

Ahora bien, considerando la ausencia de rozamiento dinámico Bm = 0, se puederealizar la siguiente aproximación a partir de la ecuación característica (fórmula 11):

)11(0Bmparaticacaracterísecuación0KKsRJsJL biam2

ma ==++

)12(JL2

KKJL4)JR(JRs

ma

bima2

mama2,1

−±−=

En los motores que se utilizan en aplicaciones prácticas, La es tan pequeñaque: 0422 >− bimama KKJLJR , entonces, aproximando:

a

a2

maa

bia

ma

m2a

biamama

2ma

bi1

ma

m2a

biamama

2,1m

2a

bia

m2a

bia

am

2a

bia

ma

m2a

biamama

2,1

LRs

JRR

KKLparaJL2

JRKKL21JRJR

sJRKKs

JL2JR

KKL21JRJRsahorasiendo

JRKKL21

JRKKL41

entoncesLpara1JR

KKL4siJL2

JRKKL41JRJR

s

−≈

<<

−−−

=−=

−±−

=−≈−

↓↓<<−±−

=


34

Consiguiendo una simplificación a polos reales negativos,

)14()1s()1s(

K1)s(

)s(

KKJRy

RLsi

)LRs()

RJKKs(

JLK

)s()s(

em

b

REF

m

ib

mam

a

ae

a

a

am

ib

ma

i

REF

m

+τ+τ=

ωω

=τ=τ+++

=ωω

Observando la función de transferencia anterior (fórmula 14), se aprecia laexistencia de dos únicos polos en los valores de 1/τe y 1/τm (Figura 17), siendo lasconstantes de tiempo eléctrica y mecánica respectivamente, donde normalmente τm > (10τe). Por tanto, la función de transferencia se puede aproximar en algunos casos por (Figura18) [11]:

1s + 1m

1s-

+E (s)amω (s)

θ (s)mτ τ s + 1e

1

Figura 17. Diagrama de bloques simplificado del modelado de un de un motor de CC (2º grado)

1s + 1m

1s-

+E (s)amω (s)

θ (s)mτ

Figura 18. Diagrama de bloques simplificado del modelado de un de un motor de CC (1º grado)

)15(KKJRdonde

)1s(K1

)s()s(

ib

mam

m

b

REF

m =τ+τ

=ωω

La ventaja de utilizar el diagrama de bloques, es que proporciona una visión clarade la relación de las funciones de transferencia entre cada bloque del sistema. Apreciandoque un motor de CC es esencialmente un dispositivo integrador entre las variables derotación del eje y la tensión de entrada al motor. Ya que, para ea(t) cte. el desplazamientode salida del motor será la salida de un integrador; esto es, se incrementará linealmente conel tiempo [4] [11].


35

Relación Ki y Kb

Aunque funcionalmente la constante del par Ki y la constante de la fuerzacontraelectromotriz Kb son dos parámetros separados, para un motor dado, sus valoresestán estrechamente relacionados. Para mostrar esta relación, la potencia mecánicadesarrollada en la armadura se escribe como [4]:

)16()t(i)t(eP ab=

La potencia mecánica también se expresa como,

)17()t()t(TP mm ω=

igualando las dos ecuaciones anteriores, (unidades del Sistema Internacional)

)18(s/rad

VKAm·NK

K)t(T)t(K)t()t(Tentonces

K)t(T)t(i)t(K)t(e

Am·N

s/radV)t(i)t(e)t()t(TP

bii

mmbmm

i

mambbabmm

=

→ω=ω

=ω=

==ω=

Es decir, que la relación de igualdad entre los valores de Ki y Kb existe realmente.

Relación Par-Velocidad de un Motor CC

Las relaciones par-velocidad de un motor CC describen la capacidad de producciónde un par estático del motor con respecto al voltaje aplicado, y a la velocidad del motor. Enestado estacionario, el efecto de la inductancia es cero, y la ecuación del motor es:

)19(R

)KE(KiKTa

mbaiaim

ω−==

donde Tm, Ia, Ea y ωm representan los valores en estado estacionario del par del motor, lacorriente, el voltaje aplicado y la velocidad.

Ia = corriente de armadura (A) TL = par de carga (N·m)Tm = par del motor (N·m) ωm = velocidad angular del rotor (rad/s)La = inductancia de la armadura (H) Ea = voltaje aplicado (V)Ki = constante del par (N·m/A) Kb = cte. de la fuerza contraelectromotriz (V·s/rad)

Para un voltaje aplicado dado ea, la ecuación anterior describe la relación lineal delas características par-velocidad del motor de CC. En realidad, el motor puede estar sujetoa dos tipos de saturaciones o limitaciones. Una limitación se debe a que la corriente de laarmadura aumenta cuando ea se incrementa, el circuito magnético se satura, por lo que elpar del motor no puede exceder de un valor máximo. La segunda limitación se debe a lacorriente máxima que el motor puede manejar debido a la disipación, es decir, la potenciaconsumida por el motor (P = ea·ia).


36

Se aplica un sistema no lineal de saturación para obligar al conjunto del sistema adar como resultado valores dentro de un margen real, característico del motor.

Figura 19. Modelado de los motores de CC en un fichero simulink (modelo de 2º orden)

Recordar el significado de cada variable:

Ra = resistencia de armadura (Ω) Ki = constante del par (N·m/A)La = inductancia de la armadura (H) Kb = cte. de la fuerza contraelectromotriz (V·s/radJm = inercia del rotor (Kg.m2) Bm = coeficiente de fricción viscosa (N·m/(rad/s))

3

in_3

1

in_1

3

out_3

5

out_5

Ki

Intensidad M1

1

out_1

Kb

-+ Jm.s+Bm

1

Fricción M1Velocidad M1

Diagrama representatiu del motor1

Out_1-Out_2 Velocidad (rad/s) de los motores

Out_5-Out_6 Par de los motores (N·m)

Out_3-Out_4 Intensidad (A) de los motores

2

in_2

6

out_6

Kb

2

out_2

+- La.s+Ra

1

Armadura M2

-+Ki Jm.s+Bm

1


Intensidad M2

4

out_4

8

out_8


+- La.s+Ra

1

Armadura M1

in_2 consigna de velocidad motor2


in_3 perturbación de entrada


s1

7

out_7

s1

Out_7-Out_8 Espacio recorrido por los motores (rad)


37

Modelado del Controlador PID

El controlador tipo PID (Figura 20), del fichero defaultc.m, aporta las cualidadespropias de este tipo de controladores. La combinación PID, es con la que se obtienenmejores resultados de control, ajustando los valores de KP , TD y TI (ver apartado 2.8Ejemplos de simulación) [4] [5] [11]:

( ) )20(d)(TK

)t(d)t(dTK)t(KtC

t

tI

pDpP

0

∫ λλε+ε⋅⋅+ε⋅=

)21(sT

KsTKK)s()s(C

I

pDPP

+⋅⋅+⋅ε=

⋅

Transf. Laplace

CONTROLADORε(s)

CONTROLADOR(t)ε

C(s)

C(t)

Figura 20. Función de transferencia de un controlador

- El término derivativo, reduce el comportamiento oscilatorio.

- En un controlador PD, existe un error en régimen estacionario. Para reducir esteerror, se añade el término integrativo, pero aumentará el comportamientooscilatorio (inestabilidad).

1s-

+ω (s)REFmω (s)

θ (s)mCONTROLADOR

PLANTAPROCESO

(MOTORES)

ε(s) C(s)

PID

Figura 21. Aplicación de un controlador a una Planta

Edición y Aplicación del Fichero de Control Tipo Simulink

Al usuario, se le permite modificar totalmente el fichero Simulink de control,modelando su sistema, motores, controlador, etc... de forma más conveniente para uncorrecto estudio de su simulación. El sistema de control, en un formato de ficheroSimulink, permite un fácil tratamiento y modelado de la función de transferencia de losmotores y del controlador. La ventaja de utilizar ficheros de Simulink y representar elsistema de control en bloques, es que proporciona una visión clara, y de fácilrepresentación, de la relación entre funciones de transferencia de cada bloque del sistema.


38

El fichero de Control Tipo Simulink (modelado de motores)

En la simulación, el modelado del Robot (motores, control, etc.) es la parte másimportante para obtener unos resultados fiables en el estudio de la respuesta del Robot.

El modelado del Robot, se realiza en un fichero Simulink, utilizando unarepresentación en bloques que facilita la visión de la relación entre las funciones detransferencia de cada bloque del sistema.

Como se ha comentado en los apartados anteriores, los modelos de motores y decontrol pueden ser varios dependiendo del propio usuario y del sistema que desee simular.Ahora bien, el fichero Simulink por defecto del programa, defaultc.m, sirve al usuariocomo ejemplo de aplicación en el modelado de motores de CC con un controlador PID, yasí, desarrollar él su propio sistema basándose en este ejemplo.

En el fichero simulink de control, además de modelar los motores y el controlador,también se puede modelar el sistema de control para el cálculo de las consignas develocidad y posición. Estos cálculos pueden estar o no incluidos en este fichero,dependiendo de si el usuario desea realizar un sistema propio de cálculo de las consignas, opor el contrario desea utilizar el existente por defecto, en el programa (funcióndynamics(a,self)).

Para comunicar al programa que el fichero de control Simulink contiene loscálculos de las consignas, es necesario tener el button tipo radio Only Motors/All Systemen “on”, de ese modo, el programa de simulación utiliza el fichero de control Simulinkpara modelar totalmente el sistema. En cambio, cuando se decide separar el modelado delos motores-controlador del cálculo de las consignas (que se realiza por código escrito en lafunción dynamics(a,self)), entonces el button tipo radio Only Motors/All System, ha deestar en ”off”.

Edición del Fichero de Control

La edición del fichero de control, se realiza pulsando el push-button Edit de lapantalla de diseño del Robot, asociado a la función opensimu(). Esta función basa sufuncionamiento en los siguientes puntos:

1- Leer nombre del fichero simulink.2- Eliminar extensión del nombre del fichero leído.3- Set nombre del fichero en el button tipo edit (name file).4- Abrir fichero.

En el inicio de la ejecución de la función, se abre una caja de diálogo uigetfile [1],donde se selecciona el nombre del fichero de control Simulink.


39

function[file] = opensimu()... ... ...% Uigetfile, leer nombre del fichero de control[namefile, pathname] = uigetfile('*.m','File Control for Open');

if namefile == 0 % Comprobar lectura correcta ... ... ... return; end;... ... ...% Colocar el nombre del archivo en la pantallaset(nafi_con,'string',file);eval(file); % Llamar-abrir fichero de Simulink

Una vez seleccionado el nombre del fichero que se desea editar, se abre y se hacenlas modificaciones, correcciones, etc. que el usuario crea necesario. Cuando ya se haeditado el fichero, y el usuario ha modelado su sistema de control, es necesario grabar elfichero, y en la pantalla de edición, escribir el nuevo nombre, sin extensión, en el buttontipo edit de File Control System, para que el programa reconozca el nuevo fichero decontrol, y guarde el nombre en la estructura de datos del Robot.

NOTA IMPORTANTE: Para que el programa reconozca el nuevo fichero de control, esnecesario salir, de la pantalla de diseño a través del label Quit de esta pantalla (ver punto2.7.8.6 Salir de la pantalla de diseño del Robot).

El nombre del fichero de control, se guarda-almacena en la estructura del Robot, deforma que:

function[] = quit1(op)... ... ...% control_name_file variable de texto con el nombre del ficheroset(confina,’UserData’,control_name_file);

% tipo_control variable numéricaset(confina,’Xdata’,tipo_control)

% 0 fichero Simulink modela sólo los motores% 1 fichero Simulink modela todos los cálculos

Recordar que es necesario, en el fichero Simulink, conservar las variables deentrada y de salida, para comunicar el este fichero con el resto del programa, obteniendolos resultados para la representación gráficas de las variables de entrada y salida del Roboty de sus motores.

NOTA: Todas las variables utilizadas en el fichero de tipo Simulink, han de ser declaradascomo variables globales al inicio del programa, o sino, estas variables no son reconocidasen la ejecución del fichero.


40

En la Figura 22 se presenta un ejemplo de fichero Simulink (defaultc.m), con elmodelado de cálculo de consignas, modelo del controlador y modelado de los motores.

+

-

Mux

-+

+

-

+

+

+

-

uyjv(a)

uyjv

KPV

KPV

d_rel

d_rel

pertur

pertur

a_rel

a_rel

y0

yy

uxjv(a)

uxjv

Zero-OrderHold1

M

Motors

+-

+

+

-

+-K-

1/(2*R)

0.5

1/2

v_m_i

velo_mot_izq

i_m_d

in_mot_dere

i_m_i

in_mot_izq

p_m_d

par_mot_dere

p_m_i

par_mot_izq

v_lin

v_lin

v_ang

v_ang

Zero-OrderHold2

v_motor_izqui.

v_motor_dere.

veloc

veloc

v_m_d

velo_mot_der

GIRO

KIV

KIV

1/dt

W = theta/dt

-+

x0

xx

1/dt

v = e/dt

Inputs/GLOBALS: uxjv, uyjv, xx, yy, bb (d_rel), rr (a_rel), pertur

Outputs: v_lin, v_ang, veloc, v_m_i, v_m_d, i_m_d, i_m_i

p_m_i, p_m_d

Law control of the Physical Agent/Mobile Robot

CONSIGNA_V

Veloc = e / t (Velocidad Robot)

f(u)

VELO.

PID's

PID's

KPA*180/pi

rad->º

Figura 22. Fichero simulink de control. Diagrama de control del sistema


41

2.7.8.4 Guardar Configuración del Robot (función filecfg(...))

En la pantalla de diseño del Robot, se presenta en la parte superior de la figura, unmenú (label Options). Esta opción de menú, abre otra barra de menú, con los labels tipoSave Config Robot y Load Config Robot. Dentro de este menú, la opción Save ConfigRobot, se divide en un submenú Data Only y All Config que tienen asociados las funciónesfilecfg(1) y filecfg(2), respectivamente.

Tal y como indica el label de estos submenús, estas funciones sirven para grabar enun fichero de texto la estructura de datos del Robot: sensores, nombre del fichero decontrol, etc.

La función filecfg(op) (donde op =1 ó op = 2), para salvar la estructura de datos delRobot en un fichero de texto, se basa en los siguientes puntos:

1- Leer índice del Robot.2- Recuperar la estructura de datos del Robot.3- Nombre del fichero donde escribir los datos.4- Abrir fichero.5- Escribir-grabar datos en el fichero.6- Cerrar el fichero de datos.

Una vez obtenido el índice del Robot, y recuperada su estructura de datos, con lafunción de aplicación uiputfile [1] se consigue, a través de una caja de diálogo, el nombredel fichero donde se han de salvar los datos (por defecto se utiliza en los ficheros deextensión *.cfg).

function[file] = filecfg(op)... ... ...% Uiputfile, leer nombre del fichero de cfg para Save[namefile, pathname] =uiputfile('*.cfg','Name to File for Save the Config of the Robot');... ... ...fid = fopen(file,'w'); % Abrir fichero para escribir

Conocido el nombre del fichero, es necesario abrirlo en formato de escritura,fopen(...), y así comenzar a grabar los datos de la estructura: sensores, radio, etc.

Dependiendo del label seleccionado, la operación de Save, permite diferenciar entresalvar toda la estructura de datos del Robot (All Config parámetro de entrada funciónfilecfg(2) es 2), o solamente, los datos relativos a los sensores y al tamaño del Robot, sinincluir los datos del fichero Simulink de control ni del fichero de programación de laMáquina de Estados de los sensores (Data Only parámetro de entrada función filecfg(1) es1). Este tipo de opciones, en la función filecfg(op), se consigue gracias al cambio deparámetro de entrada “op” a la hora de asociarlas a los respectivos buttons tipo label.


42

function[file] = filecfg(op)... ... ...if op>=1 % Save dato de Radius del Robot y datos de los sensores% Save dato de Radius del Robot fprintf(fid,'Radius (Size) of the Robot\n'); ... ... ...% Save matriz de datos de los sensores fprintf(fid,'dades_sen(:,:)\n'); for j=1:MAX_SENSORS fprintf(fid,'%g %g %g %g %g\n',dades_sen(j,:)); end; end; % end if op>=1

if op==2 % Save nombre del fichero para el control fprintf(fid,'Sensors File Name\n'); ... ... ... fprintf(fid,'Control File Name\n'); ... ... ...end % end if op==2

Antes de salir de la función filecfg(op), es necesario cerrar el fichero de texto(fclose).

fclose(fid); % Cerrar fichero para Save

Seguidamente, se presenta un ejemplo del formato de un archivo de configuracióndel Robot:

% Inicio de la información con el dato del radio de RobotRadius (Size) of the Robot4MAX_SENSORS % Número de sensores de esta configuración8dades_sen(:,:) % Inicio de la matriz de datos de los sensores10 55 0 3.69552 08 55 -30 3.86181 343.1255 55 300 2.65357 314.3765 55 270 2.27578 268.8025 55 90 2.27578 91.19815 55 60 2.70781 43.9048 55 30 3.76792 11.250 0 0 0 0Control File Name % Inicio de datos, nombre del fichero de controlmotors % Nombre del fichero de control0 % Tipo de fichero de control

NOTA: Los ángulos de la matriz dades_sen, se guardan en grados, aunque en el programase utilicen en forma de radianes.


43

2.7.8.5 Recuperar Configuración del Robot (función filecfg(...))

Los datos grabados en un fichero, se pueden recuperar y cargar en la estructura dedatos del Robot, gracias al label Load que se muestra en de la parte superior de la figura(menú Options), en la barra de menús. En esta barra de menú, la opción Load ConfigRobot, se divide en un submenú de Data Only y All Config, que tienen asociada la funciónfilecfg(3) y filecfg(4), respectivamente.

En la operación de Save, se distinguen dos casos a la hora de salvar la estructura dedatos Data Only o All Config. De igual forma, al recuperar la información del ficherotambién se diferencia entre estos dos casos; recuperar simplemente los datos referentes alos sensores y al radio del Robot (Data Only) o por el contrario, recuperar toda lainformación existente en el fichero (All Config), incluyendo el nombre del archivo decontrol Simulink y el nombre del archivo de programación de la Máquina de estados de lossensores, además de los datos característicos de los sensores y el radio.

Con esta opción de Load Config Robot, se pueden recuperar los datos deconfiguración del Robot, salvados en un fichero de texto con un formato determinado (verapartado 2.7.8.4 Guardar configuración del Robot (función filecfg(...))). La funciónfilecfg(3) y filecfg(4), para recuperar los datos del Robot, se basa, en los siguientes puntos:

1- Leer índice del Robot.2- Recuperar la estructura de datos del Robot.3- Nombre del fichero para Load.4- Abrir fichero.5- Leer-recuperar datos en el fichero (sensores, radio, etc.)6- Cerrar el fichero de datos.7- Guardar datos en la estructura del Robot.8- Reinicialización pantalla de diseño del Robot.

Conociendo el índice del Robot y la estructura de datos de éste, se obtienen elnombre del fichero a leer, con la función de aplicación uigetfile [1], que retorna el nombredel archivo a partir de la presentación de una caja de diálogo (por defecto se utiliza en losficheros la extensión *.cfg).

function[file] = filecfg(op)... ... ...% Uigetfile, leer nombre del fichero para Load[namefile, pathname] =uigetfile('*.cfg','Name to File for Load the Config of the Robot');... ... ...fid = fopen(file,'r'); % Abrir fichero para lectura

Antes de iniciar la lectura, es necesario abrir el fichero en modo de lectura, leyendolos datos del fichero a partir de un bucle hasta encontrar el final del archivo.


44

Como en el caso de Save, se puede diferenciar entre salvar Data Only o All Config,en Load, es necesario diferenciar entre recuperar todos los datos del fichero, All Config:sensores, radio, nombre del fichero de control, etc., o simplemente, con Data Only, serecuperan los datos referentes a sensores y radio del Robot (opción a partir del parámetrode entrada en la función de filecfg(op)).

function[file] = filecfg(op)... ... ...lectura = fgetl(fid); % Lectura del fichero

while ~(feof(fid)) & lectura~='' % Leer mientras no final

lectura=lower(lectura); % Convertir en minúsculas ... ... ... Acción recuperar datos

... ... ... lectura = fgetl(fid); % Lectura del ficheroend; % end while ~feof(fid)

fclose(fid); % Cerrar fichero para Load

Este formato, facilita una rápida lectura, y además, al ser un fichero de texto,permite la posibilidad de ser modificado, editándolo desde el exterior de la simulación,pero, conservando una determinada estructura en el fichero.

En la acción de Load, los datos de los sensores almacenados en la estructura delRobot serán como máximo MAX_SENSORS, y no el número indicado en el fichero delectura.

function[file] = filecfg(op)... ... ...while ~(feof(fid)) & lectura~='' % Leer mientras no final lectura = lower(lectura); % Convertir en minúsculas

% Lectura del Radius if strcmp(lectura,'radius (size) of the robot') & op>=3 self(4) = str2num(fgetl(fid)); end; % Lectura del MAX_SENSORS if strcmp(lectura,'max_sensors') & op>=3 ... ... ... end;end; % Lectura datos de sensores if strcmp(lectura,'dades_sen(:,:)') & op>=3 ... ... ... lectura = fscanf(fid,'%g',[5 MX_SEN]); ... ... ... end;

if strcmp(lectura,'control file name') & op == 4 % Lectura del nombre del fichero de control ... ... ... end; lectura = fgetl(fid);end; % end while ~feof(fid)


45

Observar que durante la acción de Load, se borra la configuración actual del Robot,perdiendo, por ejemplo, los sensores existentes en ese momento.

Una vez que se ha realizado todo el Load de datos se almacenan en la estructura delRobot, para reinicializar después la pantalla de diseño (borrando de la memoria todas lasvariables no necesarias, función quit1(1)), y así presentar la nueva configuración delRobot.

function[file] = filecfg(op)... ... ...% Reinicializar pantalla de edición Robotquit1(1); % Cerrar pantalla de edición del Robot% Llamada a la pantalla de edición para renovarla con los datos% leídosedirobot(i);

NOTA: Los ángulos de la matriz dades_sen están guardados en grados, y debenconvertirse en radianes antes de almacenarlos en la estructura del Robot.

2.7.8.6 Salir de la Pantalla de Diseño del Robot (función quit(...))

Para que el cambio de los nombres de los ficheros de control, fichero simulink yfichero de programación de la Máquina de Estados, queden guardados y reconocidos en laestructura del Robot, es necesario que la pantalla de edición se cierre a partir del label Quitpresentado en la barra de menús Options. Además, es importante el salir de esta pantalla dediseño a través de esta opción, ya que en la función asociada quit1(1) se borran de lamemoria todas las variables utilizadas durante el diseño del Robot, y que al cerrar lafigura-pantalla de diseño, ya no son necesarias (liberar memoria) [1].

function[error] = quit1(op)... ... ...if get(ra_fi_con1,'value') % Selección tipo de fichero de

% control Simulink... ... ...if exist(name_file) % Comprobar la existencia del fichero % Guardar fichero simulink en la estructura de datos del Robot set(confina,'UserData',get(nafi_con,'string'));... ... ...else % Si no existe el fichero, presentar mensaje de aviso... ... ...end % end if exist(name_file)

clear ... ... ... % Vaciar de la memoria las variables utilizadas% en el diseño del Robot


46

2.7.9 Herramientas de la Pantalla de Simulación

Para dar inicio a la simulación, después de realizar los pasos expuestos en la 1ª fasey en la 2ª fase, ahora en esta 3ª fase se inicializa la simulación del Robot móvil.

Con los diferentes elementos de interacción creados en la 1ª fase, figura “PhysicalRobot Simulator”, se realizan las acciones de inicialización del sistema, como por ejemplo:

- Elección de tipo de movimiento: grasping, tracking, etc.- Set del Robot en unas coordenadas determinadas por el usuario, ya sea a través del

mouse o del teclado.- Inicio de la simulación (Step, Run).- Presentar gráficas de resultados.- Borrar, si el usuario lo cree conveniente, el Robot.- Activar la opción de Trail (rastro en el movimiento del Robot).- Activar la opción de Logger (almacenar datos para realizar las gráficas).- Lectura fichero de datos de la simulación.- Grabar datos de la simulación en un fichero.- Editar, en caso necesario, el Robot.- Presentar gráficas de resultados.

2.7.9.1 Selección del Robot (función chrobot())

Antes de realizar cualquier acción sobre un Robot, es necesario elegir el índice deéste en el popup de índices de los Robots. El popup de índices tiene asociado la funciónchrobot(), donde se realiza el set de los datos propios del Robot (coordenadas del Robot,coordenadas del objeto grasping, coordenadas del setpoint) en los buttons tipo edit de lapantalla de simulación.

La función chrobot() basa su funcionamiento en los siguientes puntos:

1- Leer el índice del Robot del popup de la pantalla.2- Recuperar la estructura de datos del Robot (vector de datos “self”).3- Recuperar datos relativos al tipo de movimiento (grasping, other).4- Set de datos en los buttons tipo edit.5- Cambio de color del popup de índices.

Además, como detalle, también se puede observar el cambio de color del popup deíndices al mismo color del Robot cuando se selecciona el índice de éste.

function [] = chrobot()... ... ...self = gagent(i,'Self'); % Recuperar vector de estado del

Robot% Set coordenadas objeto de grasping en los buttons tipo editif get(grasping,'value') ... ... ...end; % end if get(grasping,'value')% Set coordenadas del setpoint en los buttons tipo editif get(other,'value')... ... ...end; % end if get(other,'value')


47

% Set de las coordenadas en los buttons tipo edit de la% pantalla de simulaciónset(x_coor,'String',num2str(self(1))); % Coordenada x Robot... ... ...% Cambiar el color de fondo del popup al color del Robot% seleccionadoset(agNum,'BackgroundColor',color(i));

2.7.9.2 Edición del Robot (función edirobot)

PUSH-BUTTON Edit Robot, asociado directamente a la función editrobot, donde se define yse crea la Pantalla de Diseño del Robot, figura “Design Robot”, en caso que el usuarioquiera modificar el diseño del Robot una vez ya ha sido creado.

2.7.9.3 Mover el Robot con el Mouse (función sagent())

PUSH-BUTTON Set Mouse. Este push-button tiene asociado la función sagent() sinparámetros de entrada (nargin == 0). La orden de MATLAB ginput [1], proporciona losmedios para seleccionar coordenadas de la ventana de simulación a partir del mouse. Unprimer clic, realiza la entrada de las coordenadas “x-y” del mouse, y se las asigna al Robot.Gracias, al segundo clic, se consigue el ángulo de rotación del Robot de forma que:

)20(xxyy

arctgRobot_Ángulo

y,xRobot_sCoordenada

CLIC_SEGUNDOCLIC_PRIMER

CLIC_SEGUNDOCLIC_PRIMER

CLIC_PRIMERCLIC_PRIMER

−−

=

=

Una vez obtenidas las nuevas coordenadas y el nuevo ángulo, se realiza el set deestos datos en la estructura del Robot, para redibujarlo después en las nuevas coordenadas.

Por tanto el esquema de la función sería:

1- Leer el índice del Robot del popup de la pantalla.2- Recuperar la estructura de datos del Robot.3- Obtener coordenadas x-y del mouse.4- Calcular ángulo existente entre el primer clic y el segundo clic.5- Comprobar que las coordenadas sean correctas (fichero de Simulink bounds.m).6- Redibujar Robot en las nuevas coordenadas.8- Escribir-guardar la estructura de datos del Robot.

Leído el índice del Robot del popup de la pantalla y recuperada la estructura dedatos, se obtienen las coordenadas y el ángulo de posición del Robot (vector de estadoself).

function [value] = sagent(i, propertyname, value, selfobj)... ... ...if nargin == 0 % Leer índice del Robot i = popup(AGENTS,agNum,1,text1_error,'Warning: Not exist Robot'); ... ... ...end;end; % end if nargin == 0


48

% Función leer la estructura de datos del Robot[state i self goal confina graf_sen dades_sen] = strucrob(i);

% Renovar datos del vector UserData state, y redibujar el Robot enlas nuevas coordenadas si es necesario

if nargin == 0 pts = ginput(2); % coordenadas del ratón self(1) = pts(1,1); % x posición click del ratón self(2) = pts(1,2); % y posición click del ratón

if ~diff(pts)% NO hay diferiencia entre primer click y segundoself(3) = 0; % rotación Robot = 0

else% SI hay diferiencia de ángulo entre primer click y segundo% Calcular el ánguloself(3) = atan2(pts(2,2) - pts(1,2),pts(2,1) - pts(1,1));

end;

% Evitar superposición Robot con otros elementos de la simulación error = noencima(self,i,'r');... ... ...% Comprobar que no existe remolque en caso de graspingif get(grasping,'value') selfobj = ggras(i,'selfobj');% Set objeto juntamente con el Robot en caso de remolque if selfobj(5) == 1 selfobj(1) = self(1); ... ... ... end % end if selfobj(5) == 1end % end if get(grasping,'value')

Gracias a la función error = noencima(self,i,'r'), se comprueba que no existasuperposición con obstáculos u otros Robots en las nuevas coordenadas del Robot.Además, en la acción grasping, en caso de transportar el objeto, es necesario realizartambién el set del objeto en las mismas coordenadas del Robot, tal y como se aprecia en elanterior código de programa.

Una vez obtenida y comprobada la nueva posición del robot, se redibuja,recalculando las coordenadas que define el patch gráfico (draw = 1).

function [value] = sagent(i, propertyname, value, selfobj)... ... ...if draw axes(WORLD_AXIS);

% Control límites del terreno de juego del Robot "a" ... ... ... % Cálculo de las coordenadas que definen el Robot en la nueva % posición [xydata redon graf_sen] = dibu_a(self(1),self(2),self(3), self(4),redon,graf_sen,dades_sen,i);

% Guardar datos modificados del Robot set(AGENTS(i),'XData',xydata(:,1),'YData',xydata(:,2));


49

% Control set coordenadas para la pantalla de simulación menu = find(AGENTS); % Índice del Robot seleccionado en el popup j = menu(get(agNum,'Value')); if j == i % Set coordenadas del Robot seleccionado en el popup

% en los buttons tipo edit set(x_coor,'string',num2str(self(1))); ... ... ...

end; % end if j = i

% Set TRAIL (rastro del Robot). Marcador de coordenadas del Robot if get(trailButton,'Value') == 1 & stop == 0 ... ... ... end; % end if (get(trailButton, 'Value') == 1...

value = self; % Parámetro de la salidaend; % end if draw

2.7.9.4 Actualizar Posición del Robot (función sagentk())

PUSH-BUTTON Set Keyboard. Para actualizar la posición del Robot, con las coordenadasintroducidas por teclado, primero es necesario introducir los nuevos valores en los buttonstipo edit, y realizar la validación con el push-button setkeyboard. Este push button tieneasociado la función sagentk(), donde se leen los datos de los buttons tipo edits para realizarel set en la estructura del Robot a partir de la función sagent(i,‘self’,self) pero, pasandocomo parámetros de entrada, el índice del Robot y el vector de estado con las coordenadasleídas. Con estos parámetros de entrada en la función sagent(), se realizan las mismasoperaciones de comprobación, ya explicadas anteriormente en el caso del setmouse (verapartado 2.7.9.3 Mover el Robot con el Mouse).

1- Leer índice Robot del popup de la pantalla.2- Recuperar la estructura de datos del Robot.3- Obtener coordenadas x-y, ángulo de la pantalla (no superposición).4- Llamada a la función sagent(i,’self’,self).

4.1- Comprobar que las coordenadas sean válidas (fichero de Simulink bounds.m).4.2- Redibujar Robot en las nuevas coordenadas.4.3- Escribir-guardar la estructura de datos del Robot.

Seguidamente, se presenta parte del programa de la función sagentk(), donde sepuede observar que las acciones ha realizar son muy parecidas a las desarrolladas en lafunción sagent(), pero obteniendo las coordenadas del teclado y no del mouse.

function [] = sagentk()... ... ...% Comprobar las nuevas coordenadas introducidas por tecladoif (get(x_coor,'string')=='' | get(y_coor,'string')=='' ... ... ...end % end if (get(x_coor,'string')=='' | ...

% Leer de los buttons tipo edits los datos de coordenadas, en forma% de string (transformar de string a número con la función de% aplicación str2num).


50

% Leer coordenada "x" del button editself(1) = str2num(get(x_coor,'string'));... ... ...% Comprobar no superposición del Roboterror = noencima(self,i,'r');

% Comprobar remolcado en caso de graspingif get(grasping,'value') selfobj = ggras(i,'selfobj');... ... ...end % end if get(grasping,'value')

% Llamada a la función de set en la estructura del Robotsagent(i,'Self',self);

2.7.9.5 Selección del Movimiento Programado

Para ayudar al usuario a la hora de realizar la simulación, se han diseñado una seriede herramientas dirigidas al tipo de movimiento o función a realizar por parte del Robot.Estos movimientos o acciones consisten en una serie de opciones presentadas en la pantallade simulación, figura “Physical Robot Simulator”, donde a partir de unos buttons tiporadio [1] se permite elegir entre diferentes movimientos programados para el Robot. Lafinalidad de tener programadas estas acciones, es la de facilitar la simulación, pruebas y elestudio de los resultados.

Opciones de movimiento:- Button tipo radio Grasping asociado a la función fungras().- Button tipo radio Tracking asociado a la función funtrac().- Button tipo radio Forward asociado a la función funforw().- Button tipo radio Other asociado a la función funother().

Programando un tipo de acción a realizar y observando las respuestas obtenidasvariando, por ejemplo, el sistema de control, se pueden sacar conclusiones defuncionamiento del sistema robotizado (motores, control, sensores).

Grasping (función fungras())

En esta acción el Robot esta programado para ir a buscar un objeto (triangular) yllevarlo, simulando el transporte de éste, hasta unas coordenadas de destino (por defecto x= 120 - i * 5 y = 75, siendo “i” el índice del objeto).

La función asociada al button de Grasping, fungras(), activa o desactiva losdiferentes elementos de interacción (buttons, textos, etc.) para el funcionamiento en estetipo de movimiento, como por ejemplo crear el objeto asociado a los Robots o activar elbutton de Update de las coordenadas del objeto.

function [] = fungras()... ... ...set(text1_error,'visible','off');set(forw,'value',0); % radio de forward a 0set(tracking,'value',0); % radio de tracking a 0... ... ...


51

for i=1:length(AGENTS) if AGENTS(i) ~= 0 & OBJECT_GRAS(i) == 0 % Existe Robot ?

% Crear objeto asociado al Robot (movimiento Grasping) gras(i);end % end if AGENTS(i) ~= 0...end % end for i=1:length(AGENTS)

% Borrar recorrido de la diana (función Tracking)if TRAILDIANA ~= 0 ... ... ...end % end if exist(TRAILDIANA ~= 0)

La función de asignación de la consigna de posición para el Robot en este tipo demovimiento assig_po(), controla las coordenadas del objeto, asignando la posición dedestino del Robot (coordenadas del setpoint) de forma que éste busque siempre el objeto, sies que no esta siendo ya transportado.

Funciones Asociadas al Movimiento Grasping

SET GRASPING (SGRAS())

Para realizar el set de las coordenadas del objeto se utiliza la función sgras(), dondese redibuja el objeto en las coordenadas pasadas como parámetros de entrada, almacenandoestas nuevas coordenadas en la estructura del objeto.

% selfobj = [xobj yobj theta radius remocaldo] vector de estado del objeto

% Coordenadas de dibujo del objeto triangular[xdata ydata] =dibu_o(selfobj(1),selfobj(2),selfobj(3),selfobj(4),2);

% Guardar datos modificados del Objecto.set(OBJECT_GRAS(i),'XData',xdata,'YData',ydata);

El set de las coordenadas del objeto es muy parecido al set de las coordenadas delRobot, realizando las mismas operaciones, como por ejemplo, limitar las coordenadas conla función de Simulink bounds().

ACTUALIZAR DATOS DEL OBJETO GRASPING

El push-button Update asociado la función sgras() (sin parámetros de entrada),valida las nuevas coordenadas del objeto introducidas por el teclado en los buttons tipo edit(movimiento grasping). Una vez leídas las coordenadas, se realiza el set y se renuevan losdatos en la estructura del objeto.

function [] = sgras(i, propertyname,value,self)... ... ...if nargin == 0

% Índice del Objeto i=popup(AGENTS,agNum,1,text1_error,'Warning: Not exist Object,create Robot'); ... ... ...


52

% Recuperar "UserData" state del object state = get(OBJECT_GRAS(i),'UserData'); ... ... ... MAR_FINAL = state(7);

if (get(xobj_coor,'string')=='' | get(yobj_coor,'string')=='') % Mensaje de error, faltan datos ... ... ... end; % end if (get(xobj_coor,'string')=='' | ... % selfobj = [xobj yobj theta radius ¿remocaldo?] datos propios del Objecto % Leer coordenadas introducidas por teclado selfobj(1) = str2num(get(xobj_coor,'string')); ... ... ... % Evitar superposición objeto "grasping" con otros elementos de la simulación error = noencima(selfobj,i,'r'); ... ... ...

Las coordenadas del objeto grasping, pueden actualizarse introduciéndolas por tecladoaunque este sea remolcado, ya que se anula el flag de transporte (reinicialización a ceroselfobj(5) = 0).

GET GRASPING ([SELFO] = GGRAS(I,’SELFO’))

Esta función es la contraria a la anterior, y devuelve el dato de la estructura delobjeto grasping solicitado. Como parámetros de entrada es necesario el índice del objeto yla referencia del dato que se solicita.

% selfobj = [xobj yobj theta radius ¿remocaldo?] datos propios del Objectoselfobj = ggras(a,'self');

function [value] = ggras(i, propertyname)... ... ...% vector de estado del objeto% selfobj = [xobj yobj theta radius remocaldo]state = get(OBJECT_GRAS(i),'UserData'); selfobj = state(2:6);

% patch gráfico, marcador de destino del objetoMAR_FINAL = state(7);... ... ...% Comprobar y pasar el value según el propertyname de entrada... ... ...elseif propertyname(1:2) == 'ma' value = MAR_FINAL;elseif propertyname(1:2) == 'st' value = state;end

La función ggras() es muy útil durante la simulación para conocer de forma rápiday sencilla el vector de estado (coordenadas de posición) del objeto grasping.


53

Tracking (función funtrack())

Esta acción asociada al button tipo radio Tracking, programa un movimiento a ladiana-objetivo en forma de figuras, es decir, el Robot sigue a la diana-objetivo, la cualdescribe en su movimiento una determinada figura: un círculo, un cuadrado o una elipse.

Con este button tipo radio se activan los buttons tipo edit de:- Popup de selección de la figura descrita en el movimiento

1: Circular2: Arco3: Circular en espiral4: Cuadrado

- Button tipo edit de: ángulo de desfase para el movimiento circular

function [] = funtracking()... ... ...% Desactivar buttons no necesarios para este movimientoset(obSetk,'Visible','off');set(xobj_Text,'Visible','on','String','FigureType:','Position',[.75 .65 .09 .03]);

% Crear button tipo popup (fomas de las figuras)xobj_coor =uicontrol('Visible','on','Style','popup','Units','normalized','Position',[.845 .65 .07 .04], 'String','Circular|Arc|Spiral|Square','BackgroundColor', 'w','HorizontalAlignment','Center','Callback','');

set(yobj_Text,'Visible','on','String','Angle (Degrees):','Position', [.75 .61 .12 .03]);

% Button tipo edit para lectura del ángulo de desfaseset(yobj_coor,'String','0','Visible','on','Position',[.87 .61 .03.03]);

for i = 1:length(OBJECT_GRAS) % Borrar objetos a transportar (Grasping)... ... ...end % end if OBJECT_GRAS(i) ~= 0end % end for i = 1:length(OBJECT_GRAS)

Con este tipo de movimiento, programado para el Robot en la función assigpo2(),se permite comprobar y ejecutar una serie de pruebas para estudiar la respuesta del sistemade control y de los motores, ante diferentes consignas de posición que cambiancontinuamente.


54

Forward (función funforw())

Este movimiento asociado al button tipo radio Forward y activado en la funciónfunforw(), programa el Robot en un movimiento libre siempre hacia delante. La funcióndonde se programa este movimiento assigpo3(), asigna una consigna de posición a la dianaen línea recta y con el mismo ángulo del Robot.

En el caso que el Robot tenga sensores de proximidad, y se programe la Máquinade Estados para evitar los obstáculos, el Robot corregirá su trayectoria sin hacer caso a laprogramación del movimiento tipo forward. Pero cuando los sensores no detectan nada, elRobot continua su movimiento en línea recta.

Other (función funother())

El tipo de movimiento activado con el button other, asociado a la funciónfunother(), asigna unas coordenadas a la diana-destino (setpoint) a través del teclado. Estascoordenadas de destino de la diana, se introducen en los buttons tipo edit activados a talefecto en la función funother(), para después ser validadas con el push-button Updateasociado a la función de sother(), donde se leen estas coordenadas y se guardan en laestructura de datos del setpoint.

function [] = funother()... ... ...

% Cambio de función de llamada push-button "UpDate"set(obSetk,'visible','on','Callback','sother;');

% Activar buttons necesarios para este movimientoset(xobj_Text,'Visible','on','String','X Coordinate (Setpoint):');set(yobj_Text,'Visible','on','String','Y Coordinate (Setpoint):');set(xobj_coor,'Visible','on');... ... ...

El usuario asigna por teclado, en los buttons tipo edit y una vez validadas (push-button Update), las coordenadas a las cuales el Robot debe dirigirse directamente, si no esque el sistema de sensorización detecta un obstáculo. En la función assigpo4() se recuperan(función sother()) y se asignan las coordenadas a la diana-destino (setpoint) durante lasimulación del programa.


55

2.7.9.6 Borrar Robot

Borrar la Estructura de Datos y Gráfico del Robot (función dagent())

PUSH-BUTTON Del Robot. Para borrar un Robot, se ha creado el push-button Del, asociadoa la función dagent(). El índice del Robot “i” a borrar, es leído en el popup de índices, pararecuperar toda la estructura de datos de éste. Una vez recuperada, a partir del manejadordel patch del Robot, almacenado en el vector AGENTS(i), se borra el gráfico con la opcióndelete. Este manejador, posteriormente, también es borrado (AGENTS(i) = 0).

function [] = dagent(i)... ... ...% Leer índice del Robot en el popupif nargin == 0 i = popup(AGENTS,agNum,1,text1_error,'Warning: Not exist

Robot');... ... ...end; % end if nargin == 0

if AGENTS(i) ~= 0

% Función recuperar la estructura de datos del Robot ... ... ... clear dades_sen; % Borrar matriz de datos de los sensores ... ... ... % Borrar estructura del Robot delete(redon); % Delete círculo espacio vital de Robot ... ... ... if get(grasping,'value') % Si existe objeto grasping, borrarlo MAR_FINAL = ggras(i,'MAR_FINAL'); delete(MAR_FINAL); ... ... ... end

if get(other,'value') % Borrar coordenadas buttons tipo edit ... ... ... end

% Borrar el patch del Robot delete(AGENTS(i)); % Delete AGENTS(i) % Borrar el manejador que apunta al patch gráfico del Robot AGENTS(i) = 0; % Borrar manejador patch gráfico del Robot

end; % end if AGENTS(i) ~= 0

delpos(i); % Borrar la diana del Robot número "i"


56

En la parte final de la función dagent(), se ha de renovar el popup de los índices dela pantalla de simulación. El vector AGENTS se reconstruye de forma que:

% Mientras último valor igual a cerowhile AGENTS(length(AGENTS)) == 0% Eliminar la ultima posición del vector AGENTS = AGENTS(1:length(AGENTS)-1);

if length(AGENTS) == 0 % Si no existen Robots, salir break; end; % end if length(AGENTS) == 0end % end while AGENTS(length(AGENTS)) == 0


Borrar el Setpoint (función delpos(i))

Se puede observar que, en la función dagent(), también se borra con la ayuda de lafunción delpos(i) el texto “O” que representa el setpoint del Robot (diana), utilizando elíndice “i” pasado como parámetro de entrada.

function [] = delpos(i)... ... ...% Borrar el patch de datos de la diana "position"if POSITIONS(i)~= 0; state = get(POSITIONS(i), 'UserData'); delete(POSITIONS(i)); % Borrar patch de la diana POSITIONS(i) = 0; % Inicializar manejador del patchend % end if POSITIONS(i)~= 0;

2.7.9.7 Opción Logger

El Logger (registrar), es un button tipo radio que permite al programa, almacenarlos datos de coordenadas, velocidad lineal, velocidad angular, intensidad motores, etc... decada Robot. Estos datos almacenados podrán ser representados gráficamente, gracias alpush-button plotdata asociado a la función plotdata(0).

function[] = paso()... ... ...% Logger activado? loggButton variable del button tipo radioif get(loggButton,'Value') SI ! Acción ... % Almacenar resultados de la simulación ... ... ...end;

En el caso que el Logger no esté activado, los datos no se almacenan, y por tanto,no se pueden obtener resultados gráficos.


57

2.7.9.8 Opción Trail

Este button tipo radio permite en la función sagent(), en caso de ser activado, eltrazar un pequeño patch gráfico en las coordenadas x-y del Robot (sagent(...) función deset en la estructura del Robot).

function [value] = sagent(i, propertyname, value, selfobj)... ... ...% trailButton variable del button tipo radio% Marcador (TRAIL) de coordenadas del Robotif (get(trailButton,'Value') == 1 & stop==0)

p = patch('XData',[self(1)+.2 self(1)+.2 self(1)-.2 self(1)-.2],'YData',[self(2)-.2 self(2)+.2 self(2)+.2 self(2)-.2 ], ...'EraseMode','xor','FaceColor',get(AGENTS(i),'FaceColor'),...'EdgeColor','none');% Patch de rastro (TRAIL)

TRAIL = [TRAIL p]; % Almacenar manejadores de los patch’send; % end if (get(trailButton, 'Value') == 1...

x,y

Figura 23. Ejemplo de Trail durante la simulación

donde x = self(1), y = self(2)

Activada la opción de trail, al marcar un pequeño patch gráfico en las mismascoordenadas del Robot, se obtiene su trayectoria (deja una estela al paso del Robot). Losmanejadores de estos patch se almacenan en el vector TRAIL.

2.7.9.9 Clear (función limpiar())

PUSH-BUTTON Clear. Este push-button clear, asociado a la función limpiar(), borra yreinicializa las variables que almacenan los resultados para la representación gráfica.Además, borra los manejadores de los pequeños patch’s gráficos (clear del vector TRAIL)que definen la trayectoria del Robot. Siempre y cuando las respectivas opciones de trail ylogger estén activas (buttons tipo radio).

function [] = limpiar(i)... ... ...% Limpiar los pequeños patch's gráficos de TRAIL% Borrar si el Trail está activadoif get(trailButton,'value') & TRAIL ~= []... ... ...end % end if get(trailButton,'value')

if TRAILDIANA ~= 0 % Borrar recorrido de la diana... ... ...end % end if exist(TRAILDIANA ~= 0)


58

% Inicializar vectores del Logger de velocidades, intensidades,% coordendas, par de cargaif get(loggButton,'value') % Borrar si el Logger está activado % Inicializar tiempos de simulación % Set buttons text inicializados set(minutos,'string','0 min'); ... ... ... for j = 1:length(AGENTS) if AGENTS(j) ~= 0

% Actualizar las coordenadas actuales del Robotselfa = gagent(j,'Self');x_ag(j,1) = selfa(1);y_ag(j,1) = selfa(2);... ... ...

end; % end if AGENTS(j)~= 0 end; % end for j = 1:length(AGENTS)end; % end if get(loggButton,'value')

Observar que los datos gráficos y el rastro de la trayectoria del Robot se borran sólosi sus respectivos buttons tipo radio (loggButton, 'value') y (loggtrail, 'value') estánactivados en el momento de pulsar el push-button Clear.

2.7.9.10 Reset Simulador (función reset())

PUSH-BUTTON Reset. El push-button Reset asociado a la función reset(), tal y como indicasu propio nombre, realiza una reinicializacion del programa, en el mismo momento que sepulsa, siempre y cuando la simulación este en Stop, en caso contrario, esta pulsación notiene ningún efecto. Antes de la reinicialización, se borra el espacio de memoria y secierran todas las figuras y pantallas abiertas (función quit1(0)), para después, mediante a lafunción de inicio comensa(), inicializar de nuevo el programa de simulación.

function [] = reset()... ... ...quit1(0); % Función salir de la simulación (si stop == 1)comensa; % Función de inicio del programa

2.7.9.11 Salir del Simulador

PUSH-BUTTON Quit. El push-button Quit, asociado a la función quit(0), cierra y borra elprograma de simulación. Este button se utiliza para salir del entorno de simulación yvolver al entorno de MATLAB V4.2.1.c [1]. Es importante borrar totalmente de lamemoria el espacio de trabajo de la simulación, utilizando este button para cerrar la sesión.Sólo se cerrará el simulador si éste esta parado (pulsar Stop).

function[] = quit1(op)... ... ...if op == 0

% Antes de salir se ha de comprobar si la simulación esta en STOP if stop == 1 close all; % Cerrar todas las pantallas-gráficas actuales. clear all; % Vacía el espacio de trabajo MATLAB V4.0. clc; % Mensaje de despedida fprintf(1,'\n\n This program was developed by Albert Oller

and Santos Barquero.\n\n'); end; ... ... ...


59

2.7.10 Pantalla de Diseño de Obstáculos

2.7.10.1 Editar los Obstáculos

Una de las más útiles herramientas en la simulación es la posibilidad de crearobstáculos fijos. Estos obstáculos ayudan a simular diversas situaciones y actitudes delRobot, estudiando la respuesta obtenida de los motores, del sistema de control y de laMáquina de Estados programada.

A partir del push-button Edit (obstacles) se abre la figura donde se crea y edita elentorno de obstáculos. En esta nueva pantalla “Figure for create obstacles”, se presenta unentorno con nuevos ejes de referencia, idénticos y con las mismas dimensiones a los ejesde la pantalla de simulación “Physical Robot Simulator”. El usuario juntamente con estosejes, dispone de una serie de objetos de entorno interactivo para las operaciones decreación, setmouse, delete, save, etc., de los obstáculos (ver Figura 24).

Figura 24. Pantalla de diseño de obstáculos

Barra de menús

Popup de índices de los obstáculos

Button tipo radio

Button edit

Push-button


60

2.7.10.2 Selección del Obstáculo

El popup de índices de los obstáculos en la pantalla de diseño, esta asociado a lafunción chcolobs(). Esta función se divide en los siguientes puntos:

1- Leer índice obstáculo.2- Comprobar índice leído.3- Leer la estructura en un bucle de todos los obstáculos.4- Cambiar el color del obstáculo elegido y set de los datos característicos en los buttonstipo edit.

Cuando con el popup se elige el índice de un obstáculo, éste cambia de colorgracias ha que el popup tiene asociado la función chcolobs(), donde se lee la estructura detodos los obstáculos existentes, cambiando el color del obstáculo escogido en el popup.Además de cambiar el color, los datos característicos del obstáculo se escriben en losbuttons tipo edit: coordenadas “x-y”, ángulo de rotación, tamaño y forma.

function[] = chcolobs()... ... ...% Leer índice del obstáculo “i”... ... ...% Recuperar "UserData" state del obstáculostate = get(OBSTACLES2(i),'UserData');... ... ...selfo = state(2:6);

% Cambiar el color del obstáculo de índice "i"for j = 1:length(OBSTACLES2)

if OBSTACLES2(j) ~= 0% Cambiar el color del obstáculo seleccionado en el popup

if i == j... ... ... % Obstáculo “i” en color magenta

else % NO cambiar el color del obstáculo... ... ... % Todos los obstáculos a verde

end; % end if if i == menu(j) end; % end if OBSTACLES2(j) ~= 0end; % end for j=1:length(OBSTACLES2)

% Lectura-escritura de los parámetros del obstáculo "i" en los buttons tipo editset(xob_coor,'string',selfo(1)); % Button tipo edit coordenada x... ... ...% Set en los buttons tipo radio de elección de la forma delobstáculochshape(selfo(5));

Esta sencilla función, es de gran utilidad para conocer el índice de cada obstáculo(que aparece en el centro de cada obstáculo) y las características de éste.


61

2.7.10.3 Crear un Obstáculo (función obsta())

PUSH-BUTTON Add Obstacles. Este push-button, asociado a la función obsta(...), sirve paracrear un nuevo obstáculo.

Un obstáculo es un objeto gráfico tipo patch [1] con una estructura muy sencilla.Esta estructura de patch, muy semejante al patch gráfico de un Robot, se crea en la funciónidsply() donde se define la pantalla principal de simulación. Los manejadores de estospatch, se guardan en el vector OBSTACLES.

function [] = idsply()... ... ...% -----------------------------------------------------------------% Crear los patch's que definen el obstáculo (gráficos), UserData =% state, % añadiendo el manejador de este obstáculo en el vector% OBSTACLES% -----------------------------------------------------------------

for i = 1:MAX_OBSTACLES

% Crear patch gráfico para el Obstáculo a = patch('XData',xdata,'YData',ydata,'EraseMode','xor',

'FaceColor','w','EdgeColor','w');

% selfobs = [coordenada"x" coordenada"y" theta radius shape] selfobs = [0 0 0 0 0]; % Vector de estado inicializado a cero

state = [0 selfobs]; % Vector de datos del obstáculo

set(a,'UserData',state);% Dato "UserData" del manejador delobstáculo

OBSTACLES(i) = a; % Guardar manejador del obstáculoend; % end for i = 1:MAX_OBSTACLES... ... ...

Los patch’s de los obstáculos se definen en la función idsply() (pero inicializados acero) y no en la función de crear obstáculos obsta(), debido a la necesidad de tomar comoreferencia los ejes de la pantalla de simulación “Physical Robot Simulator” (ejes definidosen la variable WORLD_AXES). Al editar los obstáculos, y crear una nueva pantalla, secrean unos nuevos ejes de referencia y el programa MATLAB V4.2.1.c no permite trabajarcon los ejes de la Pantalla Principal de Simulación estando la pantalla de diseño deobstáculos abierta, de ahí la necesidad de crear los patch’s de los obstáculos antes de sercreados por el usuario.

La función obsta(), para crear un obstáculo, se divide en los siguientes puntos:

1- Obtener índice para el nuevo obstáculo.2- Leer coordenadas iniciales buttons tipo edit.3- Comprobar que las coordenadas iniciales son válidos bounds(...).4- Función dibujo del obstáculo acabado de crear dibu_o(...).5- Set, escribir-guardar la estructura de datos de los obstáculos.


62

Al inicio de la función, obsta(...), es necesario leer el popup de índices, para asignarel índice correlativo al nuevo obstáculo (no superar el número máximo de obstáculos). Coneste índice, se recupera la estructura vacía del patch gráfico, cuyo manejador estáalmacenado en el vector OBSTACLES. Las coordenadas iniciales (coordenada central delobstáculo), así como el ángulo y el tamaño, se leen de los buttons tipo edit presentados enla pantalla. Además, a partir de los buttons tipo radio Square/Circular se puede seleccionarla forma del obstáculo.

De la función dibu_o() se obtienen las coordenadas que definen la forma delobstáculo, calculadas a partir de los parámetros de entrada que recibe esta función:coordenada central del obstáculo, radio (tamaño) y forma. La representación en la pantallade diseño de obstáculos se realiza a través de nuevos patch’s gráficos que son copias de lospatch’s de la pantalla principal, (manejadores del vector OBSTACLES2) pero con lacondición de que únicamente sirven para la representación en la Pantalla de Diseño de losobstáculos (estos patch’s se borran de la memoria al cerrar la pantalla de edición deobstáculos).

function [xdata,ydata] = dibu_o(x,y,theta,radius,shape);... ... ...if shape == 1 % Estructura obstáculo cuadrado

xdata = [x + radius*cos(theta + (1/4)*pi); ...x + radius*cos(theta + (3/4)*pi); ...

ydata = [y + radius*sin(theta + (1/4)*pi); ... y + radius*sin(theta + (3/4)*pi); ...

elseif shape == 0 % Estructura obstáculo circular pts = 0:pi/16:2*pi; xdata = radius*cos(pts) + x; ydata = radius*sin(pts) + y;

elseif shape == 2 % Objeto triangular de graspìng xdata = [x + radius*cos(theta + (1/2)*pi); ... ydata = [y + radius*sin(theta + (1/2)*pi); ...

end; % end if cua_cir == 1

Calculadas las coordenadas del obstáculo, se inicializa el patch creadoanteriormente para la representación en la pantalla de simulación, y cuyo manejador sealmacena en el vector OBSTACLES(n) (“n” índice del obstáculo). Después, y cuando sehan guardado los datos del obstáculo en su estructura, se renueva el popup de los índices,añadiendo el índice del nuevo obstáculo.


63

function [n] = obsta(n,x,y,theta,radius,shape)... ... ...% Comprobar los datos (coordenadas, radio, etc) introducidos por tecladoif (get(xob_coor,'string')=='' | get(yob_coor,'string')=='' ... ... ... ... ... ... return;end

% Lectura de los datos introducidos por tecladox = str2num(get(xob_coor,'string'));... ... ...% Comprobar radio introducido por teclado. Válido ?... ... ...% Control límites del terreno de juego del Obsta. "n"rk45('bounds',temps,[],[1e-7 temps temps 0 0 2]);... ... ...% Asignar coordenadas en el vector de estado del obstáculo% selfo = [coordenada"x" coordenada"y" theta radius shape]... ... ...% Evitar superposición obstáculo con otros elementos de la simulaciónerror = noencima(selfo,n,'o');

% Función cálculo de coordenadas de dibujo del obstáculo[xdata ydata] = dibu_o(selfo(1),selfo(2),selfo(3),selfo(4),selfo(5));

% Crear el patch que define el obstáculo (gráfico), UserData = state,% añadiendo el código de este obstáculo al vector OBSTACLES (pantalla de% edición de obstáculos)a = patch('XData',xdata,'YData',ydata,'EraseMode','xor','FaceColor','w',

'EdgeColor','w');

% Guardar los diferentes patch de datos del obstáculo en el vector state% que corresponde a "UserData" del patch del obstáculo.state = [n selfo];

set(a,'UserData',state);% Guardar manejador del obstáculo de la pantalla de ediciónOBSTACLES2(n) = a;... ... ...% Obstáculos de la pantalla principal de simulaciónset(OBSTACLES(n),'UserData',state);

% Función renovar índices de los Obstáculos en el popup de la pantalla de% ediciónpopup(OBSTACLES2,obsNum,0);


64

2.7.10.4 Mover el Obstáculo con el Mouse (función sobsta())

PUSH-BUTTON Set Mouse Obstacles. El push-button Set Obstacles, asociado a la funciónsobsta(...), realiza la operación de set del obstáculo a partir de las coordenadas “x-y“aportadas por el usuario a través del mouse. La función sobsta(...), se divide en lossiguientes puntos:

1- Leer índice obstáculo (función [...] = popup(...)).2- Leer la estructura de datos del obstáculo.3- Obtener coordenadas x-y del mouse.4- Obtener ángulo.5- Comprobar que las coordenadas sean válidas (fichero de Simulink bounds.m).6- Redibujar obstáculo en las nuevas coordenadas.7- Set, escribir-guardar la estructura de datos del obstáculo.

Una vez recuperado el índice del obstáculo y su estructura de datos se inician lasoperaciones de setmouse con la ayuda de la función de aplicación ginput [1], queproporciona los medios para seleccionar puntos de la ventana de la figura a partir delmouse. Del primer clic del mouse, se obtienen las coordenadas “x-y” del obstáculo, y conel segundo clic el ángulo de rotación:

)21(xxyy

arctgObstáculo_Ángulo

y,xObstáculo_sCoordenada

CLICK_SEGUNDOCLICK_PRIMER

CLICK_SEGUNDOCLICK_PRIMER

CLICK_PRIMERCLICK_PRIMER

−−

=

=

Introducidas las coordenadas “x-y” y el ángulo de rotación con el mouse, secomprueba que las coordenadas no superen los límites de máximo [130,90] y mínimo [0,0]con la función bounds.m.

function [] = sobsta(i, propertyname, value)... ... ...if nargin == 0 % Índice del obstáculo i = popup(OBSTACLES2,obsNum,1,text2_error,'Warning: Not exist

Obstacles'); ... ... ... % Recuperar "UserData" state del obstáculo state = get(OBSTACLES2(i), 'UserData'); ... ... ... % selfo = [coordenada"x" coordenada"y" theta radius shape] ... ... ...

% Si no hay parámetros de entrada, entonces esperar entrada % coordenadas del mouse. pts = ginput(2); selfo(1) = pts(1,1); % x position click ratón selfo(2) = pts(1,2); % y position click ratón


65

if ~diff(pts) self(3) = 0; % NO hay diferiencia de ángulo entre primer

% clic y segundo clic else %SI hay diferencia de ángulo entre primer clic y el

% segundo. Calcular el ángulo selfo(3) = atan2(pts(2,2)-pts(1,2),pts(2,1)-pts(1,1));

... ... ... end;end;

% Forma obstáculo, cuadrado = 1, cicular = 0 selfo(5) = get(shapecua,'value');

Obtenidas las nuevas coordenadas, se realiza la actualización en la estructura dedatos, y se redibuja el obstáculo (función dibu_o()) en la nueva posición. En la parte finalde la función sobsta(), se renuevan los datos de los buttons tipo edit de la pantalla,referentes a las coordenadas, ángulo y tamaño del obstáculo.

function [] = sobsta(i, propertyname, value)... ... ...% selfo = [coordenada"x" coordenada"y" theta radius shape][xdata ydata] = dibu_o(selfo(1), selfo(2), selfo(3), selfo(4), selfo(5));

% Set manejador del obstáculo, pantalla de ediciónset(OBSTACLES2(i),'XData',xdata,'YData',ydata);... ... ...% Set manejador del obstáculo, pantalla principal de simulaciónset(OBSTACLES(i),'XData',xdata,'YData',ydata);set(OBSTACLES(i),'UserData',state);

% Set en los buttons tipo editset(xob_coor,'string',num2str(selfo(1)));... ... ...

2.7.10.5 Actualizar Datos del Obstáculo (función sobstak())

PUSH-BUTTON Update. El push-button Update, asociado a la función sobstak(), realiza elset del obstáculo con los nuevos datos (coordenadas, radio o forma) introducidos porteclado en los buttons tipo edit de la pantalla de edición.

1- Leer índice obstáculo (función [...] = popup(...)).2- Obtener coordenadas x-y, ángulo y tamaño de los buttons tipo edits.3- Función sobsta(i,'Selfo',selfo).

En la función sobstak(), simplemente, se leen las coordenadas de los buttons tipoedit y se comprueba que los datos hayan sido introducidos correctamente. Obtenidas lasnuevas coordenadas, se realiza la llamada a la función sobsta(i,'Selfo',selfo). Esta funcióncon los parámetros de entrada “i” (índice del obstáculo) y “selfo” (vector de estado),realiza las mismas operaciones de control de datos y set en la estructura del obstáculo, yaexplicadas en el caso de Set mouse, pero sin introducir las coordenadas con el mouse, yaque gracias al vector de estado pasado como parámetro de entrada se obtienen las nuevascoordenadas del obstáculo.


66

function [] = sobstak()... ... ...% Comprobar datos introducidos por tecladoif (get(xob_coor,'string')=='' | get(yob_coor,'string')=='' | ... get(ang_obs_ro,'string')=='' | get(rad_obs_ro,'string')=='') % Presentar mensaje de error, faltan datos... ... ...end

% Leer de los buttons tipo edits las coordenadas% Leer coordenada x del button tipo editselfo(1) = str2num(get(xob_coor,'string'));... ... ...% Forma obstáculo,cuadrado=1, cicular=0selfo(5) = get(shapecua,'value');

% Set de las nuevos datos en la estructura del obstáculosobsta(i,'Selfo',selfo);

2.7.10.6 Borrar un Obstáculo (función dobsta())

PUSH-BUTTON Del Obsta. La función dobsta(), asociado al push-button Del Obsta utilizadopara borrar el obstáculo seleccionado en el popup de índices, se divide en los siguientespuntos:

1- Leer índice obstáculo.2- Leer la estructura de datos del obstáculo.3- Delete estructura del patch.4- Set del patch.5- Renovar popup de índices.

Primero, es necesario leer el índice del obstáculo en el popup de la pantalla, pararecuperar la estructura de datos del obstáculo, a partir del manejador almacenado en elvector OBSTACLES. Esta estructura se inicializa a cero para borrar el patch gráfico delobstáculo. Observar que el patch del obstáculo no se borra, sino que se inicializa a ceropara no perder el patch gráfico referenciado en los ejes de la pantalla principal desimulación.

function [] = dobsta(i)... ... ...% Recuperar "UserData" state del obstáculo.% Borrar el patch del obstáculoif OBSTACLES2(i) ~= 0

% Recuperar "UserData" state del obstáculo state = get(OBSTACLES2(i), 'UserData'); ... ... ... % Inicializar vector de estado del obstáculo. selfo = [0 0 0 0 0]; ... ... ... state = [0 selfo]; % Vector de datos del obstáculo

set(OBSTACLES(i),'UserData',state); % UserData inicializado. set(OBSTACLES(i),'XData',0,'YData',0); % Inicializar patch, pero

% no borrarlo!!!end; % end if OBSTACLES2(i) ~= 0 ... ... ...


67

Con el obstáculo ya borrado, se renueva el popup de índices, con los índices de losobstáculos aún existentes.

function [] = dobsta(i)... ... ...% Modificar el número de obstáculos en el vector OBSTACLES2while OBSTACLES2(length(OBSTACLES2)) == 0 OBSTACLES2 = OBSTACLES2(1:length(OBSTACLES2)-1); ... ... ...end % end while OBSTACLES2(length(OBSTACLES2)) == 0

% Función renovar índices de los obstáculos en el popup de la% pantalla de edición de obstáculospopup(OBSTACLES2,obsNum,0);

2.7.10.7 Forma del Obstáculo

Los buttons tipo radio, Square y Circular, de la pantalla de diseño de obstáculosindican la forma seleccionada para el obstáculo.

if get(shapecua,'value') == 1 % Obstáculo cuadrado ?if get(shapecir,'value') == 1 % Obstáculo circular ?

En la función de dibujo del obstáculo, dibu_o(...), ya comentada anteriormente, seaprecia la utilidad de estas variables, ya que este dato es uno de los parámetros de entradaen la función (parámetro de forma).

Estos buttons tipo radio tienen asociada la función chshape(op) (op = 1 formacuadrada, op = 0 forma circular) donde simplemente se realiza el set de “value” de lasvariables para la señalización en la pantalla de edición de obstáculos:

function[] = chshape(op)if op set(shapecua,'value',1); % Set value a 1 forma cuadrada set(shapecir,'value',0); % Set value a 0 forma circularelse set(shapecir,'value',1); % Set value a 1 forma circular set(shapecua,'value',0); % Set value a 0 forma cuadradaend;


68

2.7.10.8 Fichero de Datos de los Obstáculos

El entorno de obstáculos creado para la simulación, permite observar la respuestadel sistema de control del Robot ante diferentes situaciones. Por tanto, la posibilidad deSave-Load las configuraciones de los entornos creados con obstáculos, aporta una útilherramienta para la combinación de simulaciones y situaciones con los diferentesmodelados y sistemas de control del Robot.

En la Pantalla de Diseño de obstáculos, existe un objeto tipo menú, Options, que semuestra a lo largo de la parte superior de la figura. Esta opción de menú abre en sí misma,otra barra de menú con los labels tipo Save Config Obstacles y Load Config Obstacles yQuit.

Guardar Entorno de Obstáculos (función fileobst())

El label Save Config Obstacles contiene un submenú data obstacles asociado a lafunción fileobst(1). En esta función, se realiza la operación de Save de datos de losobstáculos creados.

1- Nombre del fichero para Save.2- Abrir fichero para escritura.3- Leer-recuperar datos de los obstáculos.4- Grabar datos de estado de los obstáculos.5- Cerrar el fichero de datos.

Primero, es necesario obtener el nombre del fichero gracias a la función deaplicación uigetfile, que retorna el nombre del archivo a partir de la presentación de unacaja de diálogo (por defecto se utiliza en los ficheros la extensión *.obs). Después, en estefichero de texto, abierto en modo escritura, debe grabarse el vector de estado de cadaobstáculo, conjuntamente con su índice en un formato sencillo de entender.

function[file] = fileobst(op)... ... ...if op == 1 % Opción de Save fichero de Obstáculos

% Uiputfile, leer nombre del fichero de obs para Save [namefile, pathname] = uiputfile('*.obs','Name to File for Save the Config of the Obstacles'); ... ... ... fid = fopen(file,'w'); % Abrir fichero para escribir ... ... ...

La escritura de los datos en el fichero se realiza en forma de bucle gracias a laestructura de datos de los obstáculos y a que los manejadores de estos están guardados enel vector OBSTACLES(i) (“i” índice del obstáculo).


69

... ... ...% Iniciar bucle de escritura de datos en el ficherofor index = 1:MAX_OBSTACLES state = get(OBSTACLES(index), 'UserData'); ... ... ... if i~=0 % Existe patch OBSTACLES ?

% Save dato de Indice-número del Obstáculo fprintf(fid,'Indice Obstáculo \n');

... ... ...% Save Vector de datos Obstáculo

fprintf(fid,'Vector de datos Obstáculo \n');... ... ...

end; % end if i~=0end; % end for index=1:MAX_OBSTACLES


Una estructura de datos sencilla y en un formato de texto permite al usuario editarel archivo desde el exterior de la simulación.

Seguidamente se presenta un ejemplo del formato del fichero de datos relativo aobstáculos. Recordar que Vector de datos Obstáculo selfobs = [coordenada"x"coordenada"y" theta radius shape].

Indice Obstáculo1Vector de datos Obstáculo105 65 0 4 1Indice Obstáculo2... ... ...Indice ObstáculoiVector de datos Obstáculox y theta radius shape


70

Recuperar Entorno de Obstáculos (función fileobst())

En el mismo menú de la orden Save, se presenta el label [1] de Load, para recuperarlos datos característicos de los obstáculos, salvados en los ficheros de texto de extensión*.obs. La función asociada al label Load, fileobs(2) se divide en los siguientes puntos:

1- Nombre del fichero para Load.2- ¿ Existen obstáculos ?

NO (ejecutar función fileobs(5))3- Abrir fichero para lectura.4- Leer-recuperar datos de los obstáculos del fichero.5- Cerrar el fichero de datos.6- Guardar datos en la estructura del Robot.7- Reiniciar pantalla de edición de obstáculos (cargar datos en la pantalla de edición).

SI3- Ejecutar función que presenta la Pantalla de aviso de ¿ reemplazar ? (funciónavislo(...)).Si reemplazar, ejecutar función fileobs(4)No reemplazar, ejecutar función fileobs(3)4- Leer-recuperar datos de los obstáculos del fichero.Si reemplazar, el obstáculo del fichero conserva su índiceNo reemplazar, signar un índice vacío al obstáculo del fichero5- Cerrar el fichero de datos.6- Guardar datos en la estructura del Robot.7- Reiniciar pantalla de edición de obstáculos (cargar datos en la pantalla de edición).

Primero, es necesario que a partir de una caja de diálogo tipo uigetfile [1] seconsiga el nombre del fichero, de donde se han de recuperar los datos de los obstáculos.

function[file] = fileobst(op)... ... ...elseif op == 2 % Opción de Load fichero de Obstáculos

% Uigetfile, leer nombre del fichero para Load[namefile, pathname] = uigetfile('*.obs','Name to File for Load theConfig of the Obstacles');... ... ...

Obtenido el nombre del fichero, la función fileobs(2), comprueba si existen, o no,obstáculos. En caso que no existan, se ejecuta la función fileobs(5). Con este parámetro deentrada de op = 5, se realiza la operación de lectura y carga de datos de los obstáculos sinningún tipo de modificación respecto a lo leído del fichero.

if length(OBSTACLES2) ~= 0 % Si existen obstáculo, reemplazar ? avislo('Do you want replace current obstacles

?','Yes','No','Cancel');else % No existen obstáculos, recuperar fichero fileobst(5);end;


71

Ahora bien, cuando existen obstáculos creados anteriormente, para evitar perderestos obstáculos sin previo aviso, el programa ejecuta la función avislo('Do you wantreplace current obstacles ?','Yes','No','Cancel'), donde se crea una pantalla, en la que sepregunta al usuario si desea reemplazar los obstáculos existentes en ese momento por losobstáculos recuperados del fichero.

function [] = avislo(txt1,txt2,txt3,txt4)... ... ...aviso = figure('name','LoadConfig Obstacles','color', col,'menubar','none','NumberTitle','off',...

txt_avis = uicontrol('Style','text','Units','normalized','Position',[0 0.62 1 0.2],'BackgroundColor',col,'String',txt1,'visible','on');

butSI = uicontrol('Style','Push','Units','normalized','Position',[0.08 0.25 0.23 0.2],'String',txt2,'Callback','fileobst(3);');

butNO = uicontrol('Style','Push','Units','normalized','Position',[0.38 0.25 0.23 0.2],'interruptible','yes','HorizontalAlignment','Center','String' ,txt3,'Callback','fileobst(4);');

butCAN = uicontrol('Style','Push','Units','normalized','Position',[0.68 0.25 0.23 0.2],'interruptible','Yes','HorizontalAlignment','Center','String',txt4,'Callback','fileobst(6);');

Los buttons tipo push de esta pantalla, ejecutan su función asociada cada vez que sepulsan.

Push-button Yes → fileobs(3) op = 3Push-button No → fileobs(4) op = 4Push-button Cancel → fileobs(6) op = 6

Como el propio button indica, si se ha pulsado el cancel, debe de cancelarse laoperación:

elseif op > 2 % Opción de Load fichero de Obstáculos... ... ...% Existen obstáculos (presentada pantalla de reemplazar)if op ~= 5 close(aviso); % Cerrar pantalla de reemplazar ? if op == 6 % Pulsado botón de Cancel (pantalla reemplazar ?) return; % Cancelar operación de Loadend;end;

Si el usuario desea reemplazar los obstáculos existentes, se ejecuta la funciónfileobs(3), que realiza la recuperación de los datos, de igual forma que fileobs(5), es decir,sin modificar los índices leídos. Y en el caso de que alguno de los índices leídos coincidacon el índice de un obstáculo existente, éste será reemplazado por el obstáculo recuperadodel fichero.


72

Por el contrario, sino se desea reemplazar los obstáculos, se ejecuta la funciónfileobs(4), con lo que el programa modifica el índice leído (ya no le es de utilidad),sustituyéndolo por un índice vacío, que no tenga asociado un obstáculo, asignando a esteíndice vacío, los datos característicos del obstáculo recuperados del fichero. Evitando asíreemplazar los obstáculos existentes, ocupando sólo los índices vacíos.

La lectura del fichero, se realiza a partir de un bucle, aprovechando el formato delfichero *.obs, para saber que todos los datos tienen previamente indicado que tipo de datosson, es decir, si es un índice (lectura(1:2)=='in') o un vector de datos (lectura(1:2)=='ve').Gracias al bucle, se realiza la lectura, hasta encontrar el final del archivo, o un error,creando el obstáculo conforme se leen sus datos.

function[file] = fileobst(op)... ... ...elseif op > 2 % Opción de Load fichero de Obstáculos... ... ... % Leer mientras no final while ~(feof(fid)) & lectura~='' & salir == 0

... ... ...% Lectura del índice-número del Obstáculoif lectura(1:2)=='in' i = str2num(fgetl(fid)); lectura = fgetl(fid); ... ... ... % Lectura datos de los obstáculos if lectura(1:2)=='ve' & salir == 0 lectura = fscanf(fid,'%g',[5 1]); selfo = lectura'; ... ... ...

% Si reemplazar obstáculos.% Mirar obstáculos no existentes

if op == 4for index=1:length(OBSTACLES)... ... ...

end;end; % end if op == 4 % Guardar datos del Obstáculo ... ... ...

% Evitar superposición obstáculo con otros elementos de% la simulación

error = noencima(selfo,i,'o');

if error % SI Superposición% presentar mensaje de warning

... ... ... end; % end if error ... ... ...

end; % end while ~feof(fid)

fclose(fid); % Cerrar fichero para Load.

Aunque en el fichero de datos exista un mayor número de obstáculos grabados, sepermite recuperar un número máximo de obstáculos impuesto por la variableMAX_OBSTACLES (ver configuración de datos de la simulación). Si en el fichero, elíndice de un obstáculo es superior al permitido, se presenta un mensaje avisando del error.


73

if salir == 1set(text2_error,'String',['Warning:Obstacle not created.MAX_OBSTACLES,... ... ...

end;

Una vez leído el fichero y recuperados los datos de los obstáculos, se reinicializa laPantalla de diseño para presentar los nuevos obstáculos, y dar por finalizada la operaciónde lectura.

quit1(2); % Cerrar pantalla de edición de obstáculos.editobst; % Llamada a la pantalla de edición para renovarla con

los datos leídos.

2.7.10.9 Salir de la Pantalla de edición de obstáculos (función quit())

Cuando se acaba de trabajar con la pantalla de diseño de obstáculos es necesariocerrar y salir de esta pantalla con el label [1] Quit presentado en el menú Options, en laparte superior de la figura. Este label Quit asociado a la función quit1(2) realiza laoperación de cierre de la pantalla de diseño de obstáculos “Figure for Create Obstacles”.Las variables definidas y utilizadas durante la fase de diseño de obstáculos, y que al cerrarla pantalla ya no son necesarias, se borran de la memoria en la función quit1(2).

function[] = quit1(op)... ... ...elseif op==2

% Salir de la pantalla de edición de Obstáculos. ... ... ... close(DISPLAY3); % Cerrar pantalla de edición de obstáculos

% Liberar memoria, clear variables de la pantalla de edición- diseño de obstáculos clear editobst obsta sobsta sobstak dobsta quit1

clear DISPLAY3 WORLD_AXIS2 ... ... ...


74

2.7.11 Inicio, Ejecución de la Simulación

En la pantalla de simulación, figura “Physical Robot Simulator” se encuentra elentorno de interacción para dar inicio a la simulación, en dos modos posibles:

- Step a step, es decir, paso a paso, gracias al push-button Step, asociado a lafunción paso().- Continuada, a partir del push-button Run, asociado a la función mainloop().

Los dos tipos de simulación obtienen la misma respuesta del sistema, únicamente ytal como indican sus nombres, proporcionan un modo de simulación diferente paraobservar la evolución del Robot.

En la Figura 25, se aprecia el proceso (en grandes rasgos) de ejecución de una solasimulación (función paso()).

LECTURA DEDATOS DELROBOT "i"

FICHERO

SIMULACIÓN

MODELADODEL ROBOT

PANTALLADE LA

REFRESCO ALMACENAR

OBTENIDOSRESULTADOS

LOS

MOVIMIENTODEL ROBOT

YSENSORESANALIZAR

Y EJECUCIÓN

Figura 25. Diagrama de las operaciones a realizar para una sola simulación (un step)

En el caso de utilizar una simulación paso a paso, con el button Step, este procesose repite tantas veces como pulsaciones del button se produzcan. En cambio, en lasimulación Run, el programa ejecuta este proceso continuamente en forma de bucle, hastala pulsación del push-button Stop.

El programa basa su funcionamiento en pequeños intervalos de simulación delfichero simulink de control (modelado del Robot), de un determinado período de duración,grain time ”dt”). El sistema de simulación del programa se explica en el siguiente apartado.


75

2.7.12 Muestreo

Si el usuario desea crear y modificar su propio fichero para el modelado del sistemade control y de los motores, es muy importante entender el método de simulación utilizadoen la ejecución del programa (simulaciones en intervalos de duración igual a grain time).Este método consiste en la división temporal de la respuesta del sistema modelado,realizando un muestreo, con un intervalo de tiempo entre muestreo y muestreo que sedetermina a partir de la variable del programa denominada grain time (dt).

El fichero simulink de control, donde se modela el Robot, es simulado durante unperíodo de tiempo (grain time segundos), obteniendo una sola muestra (la última) de ésteperíodo de simulación. Esta muestra, nos sirve como resultado de nuestro sistema, y seguarda como tal, para después servir como valor de entrada (condiciones iniciales) en elsiguiente período de simulación.

f(t) Muestreo de la respuesta del Sistema

t (s)dt 2dt 3dt 4dt 5dt 6dt 7dt 8dt 9dt 10dt ...Muestreos en intervalos de Grain Time

Intervalos de tiempo cada Grain Time5dtdt 3dt2dt 4dt

f(t) Respuesta reconstruida (Mantenedor Orden Cero ZOH)

7dt6dt 8dt 9dt 10dt ... t (s)

Intervalo de simulación del fichero simulink

Guardar valor de la última muestraCI en el siguiente intervalo de tiempo

duración de grain time segundos

Val

ores

Mue

stre

ados

Val

ores

Mue

stre

ados

Figura 26. Método de muestreo utilizado por el simulador

Estos intervalos de tiempo se aprovechan para realizar entre período y período, eltratamiento de la pantalla, creando el efecto dinámico de movimiento de los Robots,recalculando su posición [x,y,theta] conforme se obtienen los resultados de la simulacióndel fichero simulink. Además se almacenan los resultados obtenidos en cada período demuestreo para representar después las gráficas.

La primera gráfica de la Figura 26, la respuesta del sistema en tiempo continuo. Yen la segunda gráfica, se muestra la reconstrucción de la respuesta después de un muestreode dt segundos.


76

2.7.12.1 Limitación del Grain Time (dt)

El tiempo de duración de cada intervalo de simulación y ejecución del fichero decontrol (fichero donde se modela el sistema de control del Robot), puede variarse en lapantalla de configuración del Programa “Config of Data Simulator”.

Toda señal muestreada, con una máxima componente frecuencial de fS, ha de sermuestreada como mínimo, y para evitar el efecto de “aliasing” en la respuesta delmuestreo, a una frecuencia de fMUESTREO ≥ 2fS [3] [4] [11] .

fS

MUESTREO

f

S S0

| R(j ) |ω

ω

ω−ω

S0−ω ωS

ω| S(j ) |

Mω ω + ωM Sω − ωM SM−ω − ωSM−ω −ω + ωM S

No aliasingω > 2ωM S

−ω − ω−ωMM

Aliasingω < 2ωM S

SS −ω Sω0

ω| S(j ) |

Mω SMω + ω

Aliasing

Figura 27. Muestreo de una señal (aliasing)

En este caso, la componente frecuencial fS de la señal, corresponde a la frecuenciade la respuesta del sistema de control del Robot. La constante de tiempo τSISTEMA, será laconstante de tiempo del sistema de control modelado en el fichero de control de simulink,donde:

)b22(dt1f

)c22(dt2

2dt1

)a22(1f

MUESTREO

SISTEMA

SISTEMA

SISTEMAS

=

≥τ→τ

≥⇒

τ=

Tal y como se observa en la fórmula anterior (22c), el intervalo de muestreo estalimitado para evitar una respuesta inestable e incorrecta del sistema. Para asegurar unmuestreo correcto y un buen control se recomienda que:

)d22(f10fdt10 SISTEMAMUESTREO

SISTEMA ≥→≥τ

Unas frecuencias tan alejadas en valor, aseguran que no se interfieran entre sí a lahora del muestreo (evitar efecto “aliasing”) obteniendo la máxima y correcta informaciónposible. En la Figura 28 se muestra un ejemplo de la respuesta reconstruida por elsimulador después del muestro.


77

Intervalos de tiempo cada Grain Time5dtdt 3dt2dt 4dt

Val

ores

Mue

stre

ados

7dt6dt 8dt 9dt 10dt ... t (s)

f(t) Respuesta del Sistema reconstruida después del muestreo

Figura 28. Reconstrucción de la respuesta del sistema realizada por el simulador

2.7.12.2 Precisión de Movimientos y Tiempo de Refresco de la Pantalla

La modificación de la duración de estos intervalos de simulación, afecta a laprecisión en los movimientos del Robot a la hora de realizar el movimiento gráfico de ésteen la pantalla. Una disminución del grain time (dt) conlleva unos períodos de simulaciónmás cortos, aumentando el número de muestras, para una misma constante de tiempo delsistema. Se mejora así, el control de la respuesta del Robot, al producirse una mayorprecisión en las correcciones de las consignas, al realizarse un mayor número de muestras.

Un intervalo de simulación menor, conlleva que las coordenadas calculadas para elRobot, tengan una menor diferencia en distancia, y por tanto, los movimientos son muchomás lentos en la representación gráfica (mayor precisión de movimientos al tener más“imágenes por segundo”).

En cambio, el aumento de la duración del grain time (dt), consigue un efecto deinestabilidad del sistema, empeorando el control, además de disminuir la precisión en losmovimientos del Robot.

Intervalos de tiempo Refresco de Pantallat

Refresco de la Pantalla

t (s)RF RF2t 3tRF 4tRF 5tRF ...

Figura 29. Intervalos para la representación dinámica del Robot

El tiempo de refresco de la pantalla, entre movimiento y movimiento del Robot, nodepende sólo directamente del grain time de la simulación, sino, también de la velocidadde CPU del ordenador en el procesado de la simulación. Cuando el modelo del Robot se


78

complica (modelado del sistema robotizado en el fichero simulink), entonces el ordenadortarda más en procesar la información y por tanto el tiempo de refresco de la pantallaaumenta. Este tiempo de procesado de la CPU, se ve incrementado conforme crece elnúmero de Robots a simular, o si aumenta la complejidad del modelado del sistemarobotizado (fichero simulink).

Para evitar que el tiempo de refresco de pantalla (tRF) se vea afectado por lavelocidad de la CPU y para que no existan diferencias en la simulación en el dinamismográfico de los Robots, cuando se cambia de ordenador, se programa un “tiempo mínimo”en el refresco de la pantalla, controlado en la función de simulación continua mainloop(...),de forma que el usuario, a través de la pantalla de configuración, tenga la posibilidad deelegir la velocidad de los movimientos en el dinamismo gráfico, sin afectar a los resultadosde la simulación (ver apartado 2.7.14.1 Función de simulación continuada mainloop(...)).

2.7.13 Step a Step (función paso())

La simulación en modo Step, se asocia directamente a la función paso(). Estafunción, se puede calificar como la principal función para el desarrollo y ejecución de lasimulaciones, donde se realiza el verdadero proceso de “simulación” y “dinamismográfico” de la pantalla, a partir del sistema robotizado modelado en el fichero de controlsimulink.

La función paso() basa su estructura de funcionamiento en la llamada a otrassubfunciones que realizan operaciones muy determinadas y específicas. Esta estructura queengloba estas subfunciones, se divide en los siguientes puntos:

1) Lectura de la estructura de datos del Roboti.2) Inicialización del estado de los sensores.3) Cálculo de distancias y ángulo respecto al objetivo-destino (coordenadas de la

diana) del Robot.4) Ejecución-simulación del modelo configurado del Robot de; cálculo de la consigna,

respuesta de los motores, resultados.4.1) Cálculos de posición y ángulo del Robot, velocidad, espacio recorrido...

5) Control sistema de sensores respecto a otros Robots.6) Control de choque entre Robots.7) Control del sistema de sensores respecto a los obstáculos.8) Control de choque entre Robot-obstáculos.9) Set del Robot en las nuevas coordenadas obtenidas en los cálculos (4.1).10) Máquina de estados.11) Asignar nuevas coordenadas de destino, según la opción elegida:

grasping,tracking,etc...12) Almacenar-guardar resultados de la simulación (logger).

Cada vez que se ejecuta la función paso(), se realiza una simulación parcial de larespuesta del Robot. En la ejecución de este “trozo”, se obtienen unos resultados quecuando se encadenan uno detrás de otro se consigue el efecto de simulación continuada,como es el caso de la opción Run, donde se obtiene el efecto de una simulación sinintervalos, aunque realmente este dividida en períodos (pasos).


79

Seguidamente se presenta de forma esquematizada, la función paso():

Inicio función paso()

-for agente1...agenten-[]=strucrob() % recuperar datos agentei (Roboti)-inicializar sensores Roboti-[]=simgoal % cálculo de coordenadas polares entre Roboti ysu diana-objetivo-[]=dynamics % simulación, ejecución del modelo de Robotifor ... detección de los sensores del Roboti respecto otros Robots

-recuperar datos de posición Robots []=gagent-[]=sensor() % detección otros sensores-[]=noencima() % función de choque

end;

for ... detección obstáculos por parte de los sensores del Roboti-[]=gobsta % recuperar datos de posición del obstáculo-sensor() % detección de los obstáculos-choque entre Roboti y los obstáculos

end;

if tipo de movimiento elegido es grasping-[] = object(a,self) % asignar coordenadas al objeto

% asociado al Roboti-[]=sgras() % set coordenadas del objeto grasping-[]=sagent() % set coordenadas del Roboti-[]=assigpo...() % cálculo coordenadas de destino

elseif tipo de movimiento elegido es tracking-[]=sagent() % set coordenadas del Roboti-[]=assigpo...() % cálculo coordenadas de destino

elseif tipo de movimiento elegido es forward-[]=sagent() % set coordenadas del Roboti-[]=assigpo...() % cálculo coordenadas de destino

elseif tipo de movimiento elegido es other-[]=sagent() % set coordenadas del Roboti-[]=assigpo...() % cálculo coordenadas de destino

end;

-feval(maes()...) % función de análisis de la máquina de estados% de los sensores

-setpos() % set coordenadas diana-objetivo del Roboti

if logger activado-almacenar-guardar resultados de la respuesta del Roboti paralos resultados gráficos.

end;end; % fin del bucle general for-Tratamiento en la pantalla del grain timefin de la función step

En la estructura presentada se observan los diferentes pasos a seguir en el procesode simulación del programa. Cada acción tiene asociada una función, donde se realizandiferentes operaciones específicas que servirán como parámetros de entrada a otrasfunciones o como resultados para la simulación. En estas funciones, se pueden hacerrequerimientos a otras subfunciones para obtener algún dato necesario o para realizar unaoperación específica.


80

En los apartados siguientes, se desarrollan y explican las diferentes acciones yfunciones asociadas a la simulación para poder entender el funcionamiento del programa.

2.7.13.1 Cálculo de la Distancia entre el Robot y el Setpoint (función simgoal(self,a))

Esta función simplemente realiza el cálculo de la coordenada polar (distancia yángulo) existente entre el Robot y su diana-objetivo [10].

x, y

xdia, ydia

α

r

theta

radius

ϕ

Figura 30. Cálculo de la consigna lineal y angular

selfa = [x y theta radius ...] vector datos propios del RobotM = [distan_Robot-diana ángulo_Robot-diana libre]M = [r ϕ xydia(4)];donde –180º < ϕ < 180

Esta función M = simgoal(self,a), retorna un vector formado por los datos dedistancia y ángulo entre el Robot y el setpoint asociado. Este vector se almacena en laestructura de datos del Robot a través de la función sagent(i,’goal’,M).

2.7.13.2 Simulación del Fichero Simulink (función dynamics(...))

Siguiendo la estructura de funcionamiento de la función paso(), se observa que seprocede después de calcular la distancia entre el Robot y su objetivo (diana) a la ejecuciónde la función dynamics(). En esta función se realiza la verdadera simulación del fichero decontrol donde se modela el Robot (fichero simulink).

Recordar que el diseño de este fichero simulink, se puede basar en dos posiblescasos; Only Motors/All System (ver apartado 2.7.8.3 Fichero Simulink de Control) Elfichero de control tipo Simulink). Dependiendo de este diseño, en la función dynamics(), serealiza en caso necesario, el cálculo de la consigna por código, por el contrario, se realizaen el fichero simulink.

Para realizar la simulación de una determina duración (dt), se utiliza la instrucciónde la librería de MATLAB rk45(...) [ver ayuda de MATLAB], que es una herramienta muyútil en la ejecución del fichero simulink de control a través de la función dynamics().


81

Una vez realizada la simulación de un período de duración igual a dt, los resultadosobtenidos de velocidad, intensidad, par de los motores y del Robot (variables globales) nosservirán para recalcular la nueva posición del Robot (cambio de posición para larepresentación dinámica de la simulación). Este cálculo se realiza en la funcióncalxyth(self,dt) que se explica en el siguiente apartado.

function[self] = dynamics(a,self,goal,confina,dt)... ... ...x0 = self(1); y0 = self(2); theta = self(3);R = self(4);pertur = self(7);

% Posición diana (objetivo) respecto al Robot "a", goal% Recuperar coordenadas polares entre Robot y dianarr = goal(1); % Distancia diana-Robot... ... ...% get(confina,'Xdata')% --- Tipo de controlador: 0(Motors Simulink),1(All Simulink)

% Realizar simplemente la Simulación de Motors con Simulinkif get(confina,'Xdata') == 0 % Cálculo de la consigna por código, no fichero Simulink ... ... ... v_1 = (KPV * (consigna_v-veloc) + gir); ... ... ... % Simulación de los motores con Simulink ... ... ... rk45(get(confina,'UserData'),...);

else % Realizar todo el Control con Simulink ... ... ... % Ángulo en radianes diana-Robot a_rel = bb; % confina (Userdata) = [control_file_name] rk45(get(confina,'UserData'),...);

end; % end if tipus

self(5) = v_lin; % "v" Velocidad lineal Robotself(6) = v_ang; % "omega" Velocidad angular Robot

uxjv(a) = self(1); % Guardar coordenada "x" anterior... ... ...% Función de cálculo de "x","y","theta" del Robot "a" (dinámica del% Robot)[self] = calxyth(self,dt);

Calculada la nueva posición de Robot, la función dynamics() retorna a partir delvector de estado del Robot self, los resultados de la última simulación, donde recordar que:

-- vector de datos del Robotself = [x y theta radius velolineal veloangular perturbación]


82

Función de Cálculo de la Nueva Posición (función calxyth(...))

Realizado el intervalo de simulación del sistema, se calcula en esta función la nuevaposición del Robot a partir de los resultados de velocidad obtenidos. En cada muestreo desimulación, se recalcula la posición del Robot tal y como se observa en la Figura 31:

x0, y0

x1, y1

theta0

2Rr

lm2V

Vlm1

lRV theta1

aRV

Figura 31. Dibujo sobre el cálculo de la nueva posición del Robot

Recordar que: espacio = velocidad · tiempo [10]

donde; tiempo: dtvelocidad: lineal ó angular

x1 = x0 + velo_lineal · cos(theta1) · dt (23)y1 = y0 + velo_lineal · sin(theta1) · dt (24)theta1 = theta0 + velo_angular · dt (25)

Los valores de la velocidad lineal y angular del Robot se obtienen de la simulacióndel fichero simulink, realizando estos cálculos [6] [10]:

(29)[cm]R2

[cm/s]V[cm/s]V[cm/s]

(28)2

[cm/s]V[cm/s]V[cm/s]V

(27)[cm]r[rad/s][cm/s]V

(26)[cm]r[rad/s][cm/s]V

lm2lm1aR

lm2lm1lR

am2lm2

am1lm1

−=ω

+=

×ω=

×ω=

donde, Vlm : Velocidad lineal del motorωam : Velocidad angular del motorVlR : Velocidad lineal del RobotωaR : Velocidad angular del Robot2R : Distancia entre ruedas (por defecto, diámetro del Robot)r : Radio de las ruedas del Robot


83

NOTA: En un montaje práctico el sistema de control se ve afectado por el derrapede las ruedas y el posible choque con obstáculos, que varían la velocidad y posición“ideal” del Robot, tal y como se considera en la simulación.

Función de Choque con las Paredes (límites de la simulación)

Las coordenadas [x,y] de simulación se delimitan mínimas de [0,0] y máximas de[130,90], entonces, el Robot debe ser controlado para evitar que supere estos límites(función bounds). Cuando, el Robot supera se aproxima a los límites, es necesario simularun efecto de choque y roce, reduciendo su velocidad lineal-angular y desviando sutrayectoria.

Factor de Choque Paredes

El factor de choque, variable factor_xoc, pretende simular la reducción develocidad del Robot, cuando este choca con los límites de la simulación (choque con lasparedes). El cálculo de esta variable factor_xoc, se realiza a través de las funciones[factor_xoc,self] = parets(self) y xocparet(‘car’,self) en función del ángulo de choque delRobot (Figura 32).

Límite de la simulación (pared)

Figura 32. Ejemplo de choque del Robot contra la pared

Función xoc_paret(‘car’,self)

La función xoc_paret(‘car’,self) (‘car’ puntos cardinales ‘nord’ ‘sud’ ‘est’ ‘oest’)realiza la operación de control de proximidad del Robot para detectar el choque con lasparedes. Gracias a esta función, conocemos qué límite es rebasado por el Robot, es decir si:

0 + factor_proximidad < coordenada xROBOT < 130 - factor_proximidad0 + factor_proximidad < coordenada yROBOT < 90 - factor_proximidad

Esta función recibe como parámetro de entrada el punto cardinal a comprobar y elvector de datos de posición del Robot. Retorna un “1” en caso de coincidir el puntocardinal de entrada con el punto cardinal rebasado, ó un “0” en caso contrario.

function [ret,self] = xocparet(car,self)... ... ...if strcmp(car,'est') % Comprobar punto cardinal del Robot... ... ...end % end if y < proximitatelseif strcmp(car,'oest')% Comprobar punto cardinal del Robot... ... ...end % end if y > mxf-proximitat


84

elseif strcmp(car,'sud')% Comprobar punto cardinal del Robot... ... ...end % end if x <= 0+proximitatelseif strcmp(car,'nord')% Comprobar punto cardinal del Robot... ... ...end % end if x >= mxc-proximitatend % end if strcmp(car,'est')

% Factor de perturbación (según el ángulo reduc_par_car)% debido al choqueper_choque = 0.01 * reduc_par_car;% Perturbación del par de los motores debido al choque con los% límitesif ret == 1% Perturbación par motores, al producirse choque con la pared self(7) = self(7) + per_choque;end % end if ret == 1... ... ...

Del mismo modo que se reduce la velocidad lineal debido al choque, tambiénaumenta el par de carga (perturbación en los motores, proporcional al par máximosoportado). En esta función se aumenta más o menos la perturbación de carga de losmotores dependiendo del ángulo de choque del Robot.

Una vez conocido el límite rebasado, se establece el valor del factor de choque enfunción de la posición del Robot y del ángulo de choque (función [factor_xoc,self] =parets(self)).

Función [factor_xoc,self] = parets(self)

Realizada la operación de comprobar el límite rebasado, se aplica ahora, un métodopara asignar el valor del factor_xoc en función del ángulo de choque del Robot (ver Figura33).

Límite de la simulación (pared) Límite de la simulación (pared)

Lím

ite d

e la

sim

ulac

ión

(par

ed) Lím

ite de la simulación (pared)

factor_xoc 0velo_lineal 0 velo_lineal 0

factor_xoc 0

velo_lineal 1factor_xoc 1 factor_xoc 1

velo_lineal 1

Figura 33. Ejemplos de choques del Robot contra las paredes de la ventana de simulación


85

Cuando el choque con los límites es frontal, el factor de reducción de lavelocidad_lineal será próximo a “0”, de forma que:

velocidad_lineal1 = velocidad_lineal0 * factor_xoctiende a ser 0 cuando factor_xoc → 0

2.7.13.3 Función de Detección de Obstáculos (función sensor(...))

Una de las diversas aplicaciones de este programa de simulación es la posibilidadde dotar al Robot de un sistema de sensores de proximidad, para la programación de laMáquina de Estado que controla y programa los movimientos del Robot.

El sistema de sensores es controlado en la función paso(...), donde se realiza elproceso de simulación del sistema robotizado. En el inicio de cada intervalo o ejecución deesta función, el estado de los sensores es inicializado a “0”, es decir, no detecta obstáculo,para después ser actualizado (función sensor(...)) durante el proceso de ejecución de lafunción.

La información sobre el estado de los sensores se recoge en forma de vector para unfácil manejo durante la simulación.

ESTADO_SENSORES(j) = “0” el sensor “j” NO detecta obstáculoESTADO_SENSORES(j) = “1” el sensor “j” SI detecta obstáculodonde 1 ≤ j ≤ MAX_SENSORS

Función sensor(self,graf_sen,dades_sen,x_obs,y_obs,ra_obs,ESTADO_SENSORES)

Esta función realiza la operación de detección de los obstáculos a partir de lascoordenadas centrales de éste, obtenidas del parámetro de entrada (x_obs, y_obs, ra_obs).Su funcionamiento se basa en un bucle donde se analiza cada sensor existente del Robot,con la posición del obstáculo que se ha pasado como parámetro de entrada (Figura 34) [7].

function [ESTADO_SENSORES] =sensor(self,graf_sen,dades_sen,x_obs,y_obs,ra_obs,ESTADO_SENSORES);... ... ...for sensor1... hasta... sensorMAX_SENSORS

-leer datos del sensorif distancia entre sensor y obstáculo < distancia de detección

-cálculo del ángulo entre origen sensor-coordenadas obstáculo-corrección ángulo entre origen del sensor y coordenadas delobstáculo según posición del Robot-cálculo ángulo detección obstáculo según radio del obstáculo.-sumar ángulo detección sensor + ángulo detección según elradio del obstáculo-el ángulo de detección total, contiene el ángulo entrecoordenadas de sensor y coordenadas del obstáculoif ángulo detección > ángulo entre origen sensor y obstáculo

-si se detecta obstáculo, se cambia el color del sensor “i”a rojo, y su estado a “1” en el vector de estados de lossensores de forma que ESTADO_SENSORES(i) = 1

end;end;

end;


86

Este método facilita el control del estado de los sensores, de forma compacta yrápida. En la Figura 35 se muestra el sistema y los cálculos realizados para la detección delobstáculo, así como la utilidad de los diferentes datos empleados en el desarrollo de lafunción sensor(...) [7].

Objeto detectado

Objeto NO detectado

Obstáculo

Obstáculo

distance2

distance2

Figura 34. Ejemplo de detección de un obstáculo circular

Como se observa en la Figura 35 y en el código de programa de la función, elmétodo de detección consiste en comprobar que la distancia entre el origen del sensor y elcontorno del obstáculo es inferior a la distancia de detección. Y en ese caso, cuando elobstáculo esta suficientemente cercano, se procede a comprobar el ángulo existente entre elorigen del sensor y el centro del obstáculo de forma que si este ángulo supera el ángulo dedetección del sensor, quiere decir que el obstáculo no llega a ser detectado por este sensor.O, por el contrario, si este ángulo entre el origen del sensor y el centro del obstáculo, esmenor al ángulo de detección del sensor, el obstáculo si es detectado.

distance

Obstáculo detectadoObstáculo ϕ

distance2

angle

ra_obs

Figura 35. Ejemplo de detección, donde se caracteriza el significado de cada dato de la función sensor(...)

Cuando se calcula el ángulo (angle) existente entre el origen del sensor y el centrodel obstáculo, no se ha tenido en cuenta el espacio vital ocupado por el obstáculo (ra_obs)en el cálculo de este ángulo, por eso es necesario aplicar un ángulo de corrección, para queel ángulo total “contenga” también el contorno del obstáculo. El ángulo que se presenta enlos siguientes cálculos tiene en cuenta el tamaño del obstáculo respecto al origen del sensory la distancia de detección de éste.


87

dades_sen(n,1)

ra_obs

distancedistance

ϕ

angleObstáculo detectado

Obstáculo

2

Figura 36. Ejemplo de detección, con el ángulo ϕ de corrección

Por el teorema del coseno (ver Figura 36);

)30(cos·(n,1))(dades_sen·)(distance·2(n,1))(dades_sen)(distance(rad_obs) 222

22 ϕ−+=

)31((n,1))(dades_sen·)(distance·2

(n,1))(dades_sen)(distance(rad_obs)cosarc2

222

2

−

−−=ϕ

Pero si la zona de detección del sensor supera el centro del obstáculo (Figura :

Obstáculo detectado

2

dades_sen(n,1)

ra_obs

distance distance

Figura 37. El sensor supera el centro del Robot

)32(distancerad_obsarcsin

distancerad_obssin

22

=ϕ→=ϕ

NOTA: El ángulo existente entre el origen del sensor y el centro del obstáculo, selimita a un margen de ±90º, para la comparación con el ángulo de detección del sensor.

Para indicar que el sensor detecta el obstáculo, se cambia el color de éste a rojo,además de actualizar el vector de estado; ESTADO_SENSORES(n) = “1” (“n” índice delsensor).


88

En el inicio de cada ejecución de la función paso(), se inicializa el estado de todoslos sensores, señalizando que no se detecta nada (color verde), pero en el caso que en eltranscurso de la ejecución se detecte un obstáculo, el sensor en cuestión cambia de color,rojo, hasta el inicio del siguiente intervalo de ejecución, donde se vuelve a inicializar acero (color verde, no detección). Si en este nuevo intervalo, el sensor vuelve a detectar elobstáculo, éste cambia de color, a rojo, provocando un efecto de intermitencia en surepresentación.

El dinamismo y la sencillez de la función sensor(...) permite ser aplicada para ladetección tanto de obstáculos como para detectar otros Robots. Gracias a que en estafunción no se diferencia entre los diversos elementos de la simulación, sino que elparámetro de entrada necesario en la función simplemente corresponde a una coordenadacentral y un radio (tamaño) del obstáculo, sin necesidad de aportar más información.

ELEMENTOS CUADRADOS DE LA SIMULACIÓN

La detección de los elementos de forma cuadrada de la simulación, requiere de laaplicación de un control especial en la función sensor(...). Se calcula la distancia (dis_obs)desde el centro del obstáculo hasta el punto del contorno que se cruza con la recta que uneel punto de origen del sensor y el centro del obstáculo. En la siguiente representación semuestra el significado de este valor:

Obstáculo detectado

Obstáculo

ra_obs theta_obs

dis_obs

angle2

Figura 38. Ejemplo de detección de un obstáculo cuadrado

Función de cálculo de dis_obs:dis_obs = discua(angle2,ra_obs,theta_obs)

El valor dis_obs nos sirve para conocer la verdadera distancia desde el centro delobstáculo hasta su contorno según el ángulo desde el que mira, y por tanto, conocer ladistancia existente entre el origen del sensor y el contorno del obstáculo.

Con los elementos circulares, este cálculo no es necesario, porque la distancia desdeel centro del elemento a cualquier punto de su contorno, siempre es la misma (ra_obs).


89

2.7.13.4 Simulación del Choque entre Robots

En el programa existe la posibilidad de simular más de un Robot a la vez, estoconlleva la necesidad de tener una función donde se controle y se modele el choque entreRobots. Este programa de simulación no pretende un especial estudio de los efectos delchoque, simplemente, en este caso, se realizan las operaciones más esenciales para evitar lasuperposición entre los Robots, dejando a elección de usuario el realizar un estudio másdetallado, y por tanto, el realizar una función más compleja.

La función xoc_robot(...) donde se controla el posible choque entre Robots seejecuta en cada intervalo del proceso de simulación, de forma que el vector de estado decada Robot sea modificado de forma conveniente para simular los efectos del choque encaso de producirse.

xa2, ya2

angle

xa1, ya1 xa1, ya1

angle

xa2, ya2

Choque entre Robotsevitar superposiciónChoque entre Robots

superposición

Figura 39. Ejemplo de choque entre Robots

En la ejecución de la función paso(), cuando se simula el Robot, uno de los pasos aseguir durante el proceso es la simulación del choque entre Robots. De ahí que el vector deestado del Robot que se simula en ese momento sea uno de los parámetros de entrada y desalida de la función xoc_robot().

Función xoc_robots(self1,j,self2)

La función xoc_robots() donde se simula y se controla el choque entre Robots tieneun sencillo funcionamiento. Gracias al paso del parámetro como los vectores de estado delos Robots, se obtienen las coordenadas de posición, de ambos, para que a través delcálculo de la distancia existente sepamos si existe choque o no.

Ahora bien, es a partir del conocimiento del choque entre Robots, cuando se decideel método de simulación o modelado (más o menos complejo y real) del choque entreRobots. En el caso de la función presentada, se ha desarrollado un sistema muy sencillo ynada complicado, como se observa en los siguientes puntos en las que se divide la función:


90

function [self1] = xoc_robot(self1,j,self2);... ... ...% -- vector datos de los Robots% self1,2 = [xa ya theta radius velolineal veloangular pertur]% "a" índice del Robot1 "j" índice del Robot2... ... ...-Cálculo distancia Robot1-Robot2if distancia entre Robots es menor a la suma de sus radios -Calcular el ángulo del Robot1 al Robot2 if tamaño Robot2 es mayor, si hay choque pero no hay efectos del

choque if existe la misma dirección de movimiento, no hay choque ?

-Dibujar Robot "j" con las coordenadas corregidas debido alchoque -return;

end; end; -Si hay choque, aumentar la perturbación de los motores. -Corregir las coordenadas de los Robots, simulando el choque -Corregir el ángulo Robot "a" debido al choque, según en que

ángulo se produce el choque entre Robots -Set Robot "j" con las coordenadas corregidas debido al choque ... ... ... else % else if distancia entre Robots es menor ... -Corregir coordenadas Robot "a" por el choque(no superposición). -Si hay choque, aumentar el par de perturbación de los motores

del Robot "a".end; end;

De todas las funciones del programa, quizá sea la función de choque entre Robots(o las funciones de choque general) la menos restrictiva para que el usuario las desarrolle yprofundice aún más en su estudio. Por eso en este caso, no sea necesario una explicacióndetallada de cómo el proyectista a resuelto del modo más sencillo el choque entre Robots.

Choque entre Robotssuperposición

Choque entre Robotsevitar superposición

xa1, ya1

xa2, ya2xa2, ya2

xa1, ya1

thetaa2

angleangle

factor_xoc

Rrobot2

Rrobot1

Figura 40. Ejemplo del método de simulación del choque entre Robots

% Corrección coordenadas, evitar la superposición entre Robots% (simulación del choque)self(1)= xa2 = xa1 + cos(angle) * (R robot1 + Rrobot2 + factor_xoc);self(2)= ya2 = ya1 + sin(angle) * (R robot1 + R robot2 + factor_xoc);self(3)= thetaa2 = thetaa2 ± angle_xoc;


91

Principalmente, esta función de choque entre Robots realizada por el proyectista,trata de evitar la superposición, es decir, evitar la invasión del espacio vital ocupado por unRobot por parte del otro, simulando el efecto de desplazamiento debido al choque(factor_xoc). Además, dependiendo del ángulo de choque, se realiza la corrección delángulo de giro del Robot, self(3) = theta, sumando ó restando el valor de angle_xoc.

Los robots se aproximan a elementos cuadrados, aunque su forma sea mástrapezoide. Con la utilización de la función [Rrobot1,2] = discua(angle, Rrobot1,2,theta_robot) que calcula la distancia desde el centro de los Robots al contorno respectivodonde se produce el choque entre ambos. Estos valores servirán, para corregir la posiciónde los Robots para evitar la superposición.

2.7.13.5 Simulación del Choque Robot-Obstáculo

En la simulación, la posibilidad de crear un entorno de obstáculos, sirve al usuariopara estudiar la respuesta del Robot ante diferentes entornos. Pero, hay que tener en cuenta,que se ha introducido un elemento más en la simulación, sobre el cual hay que aplicar unaatención especial para la interacción con el Robot.

Durante el intervalo de simulación (función paso(...)), y tal y como se ha realizadocon el choque entre Robots, es necesario la simulación del choque contra los obstáculos.

Cuando el obstáculo es cuadrado, se aplica un cálculo especial para corregir suradio dependiendo del ángulo de choque con el Robot, ya que un obstáculo cuadrado nosiempre tiene la misma distancia desde el contorno al centro de éste, dependiendo delángulo desde que se mire.

Función xoc_obstacles(self,selfobs)

La función de choque presentada es muy sencilla, y no se basa en un estudiocomplejo, sino que simplemente pretende evitar la superposición del Robot sobre elobstáculo.

function [self] = xoc_obstacles(self,selfobs);... ... ...-Cálculo distancia Robot-Obstáculo

if cálculo distancia Robot-Obstáculo < suma de los radios ...... (ROBSTÁCULO + RROBOT)

-Cálculo ángulo del Robot al Obstáculo -Aumento perturbación par motores Robot al producirse choque -Corregir coordenadas del Robot, para que no se superponga al

Obstáculo -Corregir ángulo del Robot, debido al choque (si es frontal) ... ... ...end; % end if cálculo distancia Robot-Obstáculo < suma de ...


92

Choque Robot-obstáculosuperposición

xa1, ya1

xobs, yobs

Choque Robot-obstáculoevitar superposición

Ra1

angle

Ra1

xa1, ya1

xobs, yobs

angle

Robs

Figura 41. Ejemplo del método de simulación del choque entre el Robot y un obstáculo cuadrado

% Corrección de las coordenadas del Robot, para que no se superponga al% Obstáculoself(1) = xa1 = xobs - cos(angle) * (Ra1 + Robs);self(1) = ya1 = yobs - sin(angle) * (Ra1 + Robs);self(3) = self(3) + ang_xoc; % Corregir ángulo Robot debido al choque

% Aumento perturbación par motores Robot al producirse choqueself(7) = self(7) + per_choque;

Cuando el obstáculo es circular, el Robs corresponde al radio de este obstáculo,pero en caso de ser cuadrado, es necesario utilizar la función [Robs] = discua(angle, Robs,theta_obs), que calcula la distancia existente entre el centro del obstáculo y el punto delcontorno que se cruza con la recta que une los centros de ambos elementos de lasimulación (ver Figura 41). Con el cálculo de este valor, se consigue simular realmente elchoque del Robot contra un obstáculo cuadrado.

NOTA: El robot, para la simulación del choque con un obstáculo, siempre se aproxima aun objeto circular de radio Ra1 (Figura 42).

ROBOT

"grasping"

Espacio vital ocupado por el elemento de la simulación

R

R

Figura 42. Aproximación realizada para el Robot

El aumento del par de perturbación derivado del choque, siempre será proporcionalal par máximo soportado por el motor.


93

2.7.13.6 Control del Movimiento Asociado a las Acciones del Robot

Los diferentes tipos de movimientos o acciones presentados en la pantalla desimulación, tienen asociadas funciones, assigpo...(...), donde se realizan las operaciones decontrol y asignación de las coordenadas del setpoint (diana) del Robot. Cada accióncomporta un modo de movimiento concreto para el setpoint, sin afectar al control realizadosobre el Robot, haciendo necesario utilizar funciones específicas asociadas a cadamovimiento, y que se ejecutan en la función principal de simulación paso() dependiendodel tipo de acción seleccionada en los buttons radio de Movement.

Acción Grasping

Este tipo de movimiento lleva asociado otro elemento más en la simulación, y estocomporta la necesidad de tener unas funciones que interrelacionen el Robot con su objeto“grasping” durante la ejecución del programa.

Función Object(a,selfa)

Como ya se ha explicado anteriormente en el desarrollo de esta memoria, la acción“grasping” consiste en que el Robot transporte un objeto, que ha de ir a buscar a unascoordenadas determinadas y transportarlo hasta las coordenadas de destino.

El control del objeto, se realiza en la función object(...), donde se le asigna laposición (coordenadas) en la simulación dependiendo de sí éste es transportado o no.

Cuando el Robot busca y llega a superponerse al objeto, se simula en el programa,la acción de cargarlo por parte del Robot (selfobj(5) = 1), de forma que las coordenadas deeste objeto serán las mismas que las coordenadas de posición del Robot, simulando a lavez el transporte. Esta acción, conlleva el aumento de la perturbación (par de carga) de losmotores en el Robot debido al transporte del objeto.

function [selfobj,selfa] = object(a,selfa)... ... ...if existe objeto if objeto NO transportado -Cálculo distancia objeto-Robot. ... ... ... if distancia objeto-Robot < suma radios de objeto-Robot -Objeto pasa ha ser transportado por el Robot

(selfobj(5) = 1) -Visulalizar destino set(MAR_FINAL,'visible','on'). -Coordenadas del objeto = coordenadas del Robot. -Aumento de la perturbación en el Robot debido a la carga

del objeto end; % end if distancia objeto-Robot < suma ... elseif Objeto SI transportado if distancia coordenadas destino-Robot < factor de cercania -Objeto deja de ser transportado por el Robot

(selfobj(5) = 0) -NO visulalizar destino set(MAR_FINAL,'visible','off'). -Coordenadas del objeto = coordenadas de origen del objeto -Disminución de la perturbación en el Robot debido a la carga

del objetoend; end; end;


94

Los parámetros de salida de la función object(...), que corresponden a los vectoresde estado del Robot y del objeto, una vez han sido tratados debidamente, sirven de entradaa las funciones de set de datos respectivas, como son ‘sagent(...)’ y ‘sgras(...)’.

Las coordenadas de destino del objeto se marcan en la pantalla con un triángulo, sinrelleno, del mismo color que el Robot. El manejador de este triángulo (objeto line [1]creado en la función gras(...)) se guarda en el vector MAR_FINAL(a) para ser activado(visible,’on’) en el momento que el objeto sea transportado. Del mismo modo, sedesactivará (visible,’off’) cuando el Robot que carga el objeto, llegue a las coordenadas dedestino, para entonces, reinicializar el objeto en las coordenadas de origen, y entonces,volver a empezar el ciclo.

Función sgras(a,'Self',selfobj)

Para redibujar el objeto en unas coordenadas determinadas, se utiliza la función deset de datos del objeto sgras(a,'Self',selfobj), a la vez que se almacena el nuevo dato en laestructura de datos del objeto.

Función [self] = sagent(a,'Self',self)

En la parte final de cada intervalo de simulación, cuando el vector de estado delRobot ha sido tratado (coordenadas, velocidades, perturbación) se realiza el set del nuevovector de estado en la estructura de datos del Robot, de forma que quede almacenado parael siguiente intervalo de simulación. Además, el set del vector de datos, conlleva elredibujar el Robot en las nuevas coordenadas (self(1) = xROBOT, self(2) = yROBOT, self(3) =ánguloROBOT).

Función [self selfdia] = assig_po(a,self,selfobj)

En la asignación de las coordenadas de posición del setpoint (diana) asociado alRobot (consigna de posición), es donde se encuentran las principales diferencias entre lasposibles acciones: grasping, forward, tracking, etc.

En este caso, en la acción grasping, es necesario conocer el estado del objeto(parámetro de entrada selfobj de la función assig_po(...)) de forma que siempre que éste nosea transportado, el Robot tendrá como consigna de posición, las coordenadas del objeto,para que pueda ir a buscarlo. En cambio, sí el objeto ya está siendo transportado, laconsigna de posición del Robot corresponde a las coordenadas de destino del objeto.

function [selfa,selfdia] = assig_po(a,selfa,selfobj)... ... ...if objeto “grasping” NO transportado % Coordenadas de consigna iguales a las del objeto xdia = selfobj(1); % El destino del Robot, son las coordenadas del objeto ydia = selfobj(2);elseif objeto “grasping” SI transportado


95

% Coordenadas de consigna del Robot, iguales a las coordenadas de% destino para dejar el objeto xdia = 120 - a * 5; ydia = 75;end; % end if selfobj(5) == 0

% Asignación vector de datos de la diana (setpoint) parámetro de% salidaselfdia = [xdia ydia 0];

Observar que en la función [self selfdia] = assig_po(a,self,selfobj), se retorna comoparámetro de salida el vector de estado del setpoint (selfdia), que corresponde a la consignade posición del Robot.

Acción Tracking

La acción tracking, consiste en el seguimiento por parte del Robot de su setpoint(diana), el cual describe en su recorrido una determinada figura (marcada-señalizada conpequeños patch’s) seleccionada en el popup presentado en la pantalla de simulación.


Función de set del parámetro de entrada, de forma que:

function [value] = sagent(i, propertyname, value, selfobj) donde"i" representa el número de Robot"propertyname" referencia del dato "UserData" para realizar el set"value" nuevo valor a guardar en la estructura del Robot

En este caso, el parámetro referido corresponde al vector de estado (coordenadas deposición, velocidades, etc.) del Robot.

-- datos propios del Robotself=[xROBOT yROBOT theta radius velolineal veloangular pertur]

Función [self selfdia] = assigpo2(a,self)

En la acción tracking, las coordenadas de posición del setpoint (consigna deposición del Robot), se calculan en la función assigpo2(...) donde se aplican las fórmulasque describen figuras determinadas y que son visibles gracias a un pequeño patch’s gráficoque dibuja la figura.

function [selfa,selfdia] = assig_po2(a,selfa)... ... ...% a = índice del Robot% selfdia = vector de estado de la diana (setpoint)% --- seldia = [coordenada"x" coordenada"y" libre]% selfa = vector de estado del Robot% --- selfa = [xa ya theta Ra velolineal veloangular pertur]%% El Robot persigue a la diana (setpoint), que describe una% determina figura en su movimiento% Opción = 1 Circular con desfase% Opción = 2 Elipse con desfase


96

% Opción = 3 Circular con desfase, pero con un radio% decreciente a cada vuelta% Opción = 4 Cuadrado... ... ...-Asignar coordenadas del setpoint TRACKING. Circulo con ángulo de desfase(opción 1)if get(yobj_coor,'string') == '2' % Elipse, con ángulo de desfase(opción 2)... ... ...elseif get(yobj_coor,'string') == '3' % Circulos cada vez más

pequeños(opción 3)... ... ...elseif get(yobj_coor,'string') == '4' % Movimiento cuadrado(opción 4)... ... ...end; % end if get(yobj_coor,'string') == 1

-Patch gráfico que define la forma de la figura que describe elsetpoint (diana)... ... ...selfdia = [xdia ydia 0]; % Asignación vector de datos de la

% diana (parámetro de salida)

Dependiendo de la opción escogida, se aplica una fórmula u otra para asignar lascoordenadas al setpoint (diana), para que describa en su movimiento una determinadafigura. Estas coordenadas se retornan de la función [selfa,selfdia] = assig_po2(a,selfa), através del parámetro de salida.

% Asignación vector de datos de la diana (parámetro de salida)selfdia = [xdia ydia 0];

Acción Forward

De todas las acciones, la de forward, es la más sencilla, ya que simplementeconsiste en hacer que el Robot siempre vaya hacia delante, en línea recta.







97


Para realizar la acción de forward, al Robot se le asigna como consigna de posiciónunas coordenadas que quedan en la misma dirección, de forma que el Robot siga una línearecta hacia adelante. Estas coordenadas se calculan a través del ángulo de rotación delRobot (theta), y con una distancia que supondrá una velocidad constante al ser estascoordenadas la consigna de posición-velocidad del Robot.

function [selfa,selfdia] = assig_po3(a,selfa)... ... ...% selfdia = vector de estado de la diana (setpoint)% --- seldia = [coordenada"x" coordenada"y" libre] ---% selfa = vector de estado del Robot% --- selfa = [xa ya theta Ra velolineal veloangular pertur]... .. ...% Datos correspondientes al Robot "a"% self = [xa ya theta Ra velolineal% veloangular pertur]xa = selfa(1); % Coordenada x del Robotya = selfa(2); % Coordenada y del Robottheta = selfa(3); % Ángulo del RobotRa = selfa(4); % Radio del Robot

% Movimiento de la diana (setpoint), sigue la dirección del Robot,% siempre hacia adelantexdia = xa + 8 * Ra * cos(theta);ydia = ya + 8 * Ra * sin(theta);

% Asignación vector de datos de la diana parámetro de salidaselfdia = [xdia ydia 0];

Este tipo de acción, no es muy útil, sino se dispone de una Máquina de Estados(sistema de detección de obstáculos mediante sensores) que dé un sentido a losmovimientos del Robot.

Acción Other

Esta acción, en la que se inicializa el Robot por defecto al ejecutar el programa(comensa()) consiste en asignar directamente las coordenadas del setpoint con el teclado através de los buttons de tipo edit. Gracias a este tipo de acción, se da la oportunidad deobservar el control realizado sobre el Robot, dando una consigna de posición determinadapor el usuario.


98







En la acción other el Robot se dirige a las coordenadas designadas por el usuario através del teclado. En la función assigpo4(...) se recuperan estas coordenadas quepermanecen guardadas en la matriz COOR_DIANA. Los datos de esta matriz se recuperana partir de la función sother(a), que es la función asociada al push-button Update quevalida las coordenadas introducidas por el usuario en los buttons tipo edit, pero que en estecaso devuelve como parámetro de salida (cuando se le pasa como entrada el índice delRobot) el valor de estas coordenadas almacenadas en la matriz COOR_DIANA.

function [selfa,selfdia] = assig_po4(a,selfa)... .. ...% a = índice del Robot% selfdia = vector de estado de la diana (setpoint)% --- seldia = [coordenada"x" coordenada"y" libre] ---% selfa = vector de estado del Robot% --- selfa = [xa ya theta Ra velolineal veloangular pertur]---%... .. ...% Obtener coordenadas de destino del Robot asignadas con el% push-button Update[xdia ydia] = sother(a);

% Asignación vector de datos de la diana en el parámetro de salidaselfdia = [xdia ydia 0];... ... ...

% Función sother(a)function [xdia,ydia] = sother(i,xdia,ydia).. ... ...elseif nargin == 1 % Sólo un parámetro de entrada “a” % Obtener coordenadas de destino en movimiento other xdia = COOR_DIANA(a,1); ydia = COOR_DIANA(a,2);.. ... ...


99

2.7.13.7 Función de Programación de la Máquina de Estados

Realizado el control de las diferentes acciones: grasping, forward, etc... parasimular el movimiento del Robot y asignadas las coordenadas de posición del setpoint(diana) asociado, el siguiente paso a realizar en la simulación es la ejecución de la funcióndonde se programa la Máquina de Estado a partir de la información aportada por lossensores de proximidad del Robot. Esta Máquina de Estado, modifica la dirección delRobot dependiendo del estado de los sensores de proximidad que informan al Robot de laexistencia de obstáculos (Robots, paredes de la simulación, obstáculos).

Gracias al vector ESTADO_SENSORES se conoce el estado (detección obstáculo= 1) de cada sensor, así, se puede realizar todas las combinaciones posibles en la funciónmaes(...) y asignar las coordenadas de destino (diana) al Robot según el estado de sussensores.

function[] = paso()... ... ...

% vector datos del Robot% self = [xa ya theta radius velolineal veloangular pertur] % selfdia = [x y libre] vector datos de la diana% ESTADO_SENSORES(a,:) matriz de estado de los sensores del% Robot "a"% ESTADO_VIEJO_SENSORES(index) = 0 Vector de estado anterior% sensores% Programación Máquina de Estados, función almacenada en% get(mast,'UserData')[selfdia] =feval(get(mast,'UserData'),self,selfdia,ESTADO_SENSORES(a,:),ESTADO_VIEJO_SENSORES,a);

La función maes(...) tiene como parámetro de entrada, además de la matrizESTADO_SENSORES, el vector de estado de la diana (setpoint) y del Robot. Estos datosse utilizan durante la programación de la Máquina de Estados, para corregir la dirección yel destino del Robot, dependiendo del estado de los sensores.

2.7.13.8 Función setpos(a,x,y,theta)

El set de datos referentes al setpoint (diana) asociado a cada Robot se realiza con lafunción setpos(...). Una vez calculada la nueva posición del setpoint (diana) gracias a lasfunciones assigpo(...) y maes(...), se redibuja en el proceso de ejecución de la simulaciónpara indicar las nuevas coordenadas de destino del Robot. Este dato de posición [x,y], quese guarda en la estructura de datos del setpoint, (ver apartado 2.7.6 Funciones Asociadas alTratamiento...), nos servirá después para el cálculo de la consigna de posición del Robot,permitiendo un tratamiento sencillo y dinámico.


100

2.7.13.9 Datos para la Representación Gráfica

Finalizado todo el proceso de ejecución de las diferentes funciones donde serealizan las operaciones necesarias para la simulación, se procede a guardar los resultadosde la simulación (en caso de estar activado el button logger) para realizar después larepresentación gráfica.

function[] = paso()... ... ... % Guardar datos para realizar la gráficas si LoggButton == 1 if get(loggButton,'Value') % Sólo SI Logger activado ... ... ... % Guardar resultados de la x_ag(a,b) = self(1); y_ag(a,b) = self(2); log_vlin(a,b) = self(5); ... ... ... end;... ... ...

2.7.13.10 Control del Total Grain Time

En cada ejecución de la función paso(...), se realiza una simulación del ficherosimulink de control de una duración, igual al tiempo llamado grain time (gt). En la figura“Physical Robot Simulator” de simulación se muestra el llamado total grain time, que esel valor acumulado de grain time por cada ejecución de la función paso(...) realizada. Estevalor, nos puede servir como referencia para conocer el tiempo total de simulaciónacumulado. Como se observa en la parte final de la función paso(...), es donde se realiza elcontrol y acumulación del total grain time, así como del tiempo total empleado por la CPUpara el proceso de simulación.

function[] = paso()... ... ...

% Tiempo de muestreo mínimo para la monotorización% Tiempo transcurrido en una simulación entera de la función% paso etime(clock,t0)tirepan = etime(clock,t0) + segun;

if tirepan >= 60 % Añadir un minuto... ... ... % set de los minutos de CPU en la pantalla set(minutos,'string',[num2str(minu) ' min']);% minutos CPUend;set(segundos,'string',[num2str(tirepan,4) ' s']);% segundos CPU

... ... ...% Valor actual de Grain Time Total

... ... ...dt_total = str2num(vector2) + dt;

% set Total Grain Time pantallaset(dt_edit,'string',[num2str(dt_total,6) ' s']);


101

2.7.14 Run

El modo de simulación Run consiste en la ejecución continuada y automática de lafunción paso(...), de forma que el Robot tiene un movimiento simulado, continuo. Estemétodo obtiene los mismos resultados que en la simulación paso a paso (push-button Step),al realizar la ejecución de la función paso(...) a través de un bucle en la funciónmainloop().

2.7.14.1 Función de Simulación Continuada mainloop()

La función mainloop() asociada al push-button Run, realiza la ejecución continuadade la función paso() a través de un bucle tipo ‘while’ [1]. El proceso continuo de ejecuciónno se detiene hasta la pulsación del push-button Stop, que es cuando la variable global stoppasa a ser “1”.

function [] = mainloop()... ... ...% Selección de la velocidad del dinamismo gráfico de la pantalla% de simulación (valor de configuración)if veloSimula == 1 tiempo_refresco = 0.075; % Rapidelseif veloSimula == 2 tiempo_refresco = 0.2; % Mediumelseif veloSimula == 3 tiempo_refresco = 0.4; % Slowend;

while stop == 0... ... ... paso; % Función principal de simulación pause(1e-5); % Pausa durante 1e-5 segundos

while ... tiempo_transcurrido <= tiempo_refresco ... ... ... end;end; % end while stop == 0

En el inicio de esta función de simulación, se controla la existencia del Robot,además de asignar el tiempo de refresco de la pantalla (ver apartado 2.7.12.2 Precisión deMovimientos y Tiempo de Refresco de la Pantalla), seleccionado en el popup de la pantallade configuración VALUE VELOCITY SIMULATOR Rapid, Medium y Slow. Este tiempode refresco (delay) consiste en retrasar las sucesivas ejecuciones de la función paso(), y portanto reducir el tiempo entre movimiento y movimiento “gráfico” de cada Robot. Si no secontrolará este tiempo, cuando el usuario utilizará ordenadores de diferentes frecuencias dereloj, notaría el cambio en la velocidad del dinamismo gráfico del programa.


102

2.7.15 Tratamiento de los Resultados de la Simulación

Finalizado el proceso de simulación, el usuario dispone de una serie deherramientas que le permiten estudiar los resultados obtenidos. El usuario, además deobservar la evolución dinámica de los Robots en la pantalla, puede salvar, recuperar (deanteriores simulaciones) y representar gráficamente los resultados.

Estas sencillas herramientas, ayudan en el estudio de los datos obtenidos de lasimulación, que es la finalidad del programa desarrollado.

2.7.15.1 Guardar los Resultados de la Simulación (función filedata())

Los resultados obtenidos en la simulación pueden ser tratados de dos maneras:representados gráficamente o salvados en un fichero de texto de forma que se puedanrecuperar para un posterior estudio o un desarrollo más amplio. En este apartado, seexplica el método utilizado para salvar en un fichero los resultados de la simulación.

El label [1] Save data, para salvar los resultados de la simulación, se presenta en labarra de menús Options en la parte superior de la figura principal de simulación “PhysicalRobot Simulator”.

Este label asociado a la función filedata(1) realiza la acción de grabar los datos dela simulación: velocidades, intensidades, etc... La función filedata(1) se divide en lossiguientes puntos:

1- Leer y comprobar índice del Robot.2- Obtener el nombre del fichero de datos (caja de diálogo).3- Abrir fichero para escritura.4- Salvar-escribir datos en el fichero de texto.5- Cerrar fichero de texto.

Los ficheros de datos, por defecto, tienen la extensión *.dat, utilizada en la caja dediálogo para obtener el nombre de este fichero de escritura. Esta caja de diálogo se abrecon la función de aplicación uiputfile [1].

Una vez el fichero se abre en modo escritura, se empiezan a guardar los datos conun formato y estructura determinada, para después y conociendo este formato, sea mássencillo el recuperar los datos. Primero, se escribe el nombre de referencia del vector dedatos y después se escribe en el fichero los datos de este vector. La función de aplicaciónutilizada para escribir en los ficheros es fprintf, que permite escribir texto además devariables numéricas, cambiando el parámetro (parámetro de entrada de la funciónfprintf(fid,'%g\n',length(x_ag(i,:)))).

function [] = filedata(op)... ... ...% Dependiendo del parámetro de entrada, se realiza la operación de% Save o de Load.% op = 1 --- Save% op = 2 --- Load... ... ...% Leer índice Robot de la pantallai = popup(AGENTS,agNum,1,text1_error,'Warning: Not exist Robot');


103

% Comprobar si hay datos alnacenados... ... ...if op == 1 % Opción abrir-Save fichero datos-resultados % Uiputfile, leer nombre del fichero de datos para Save [namefile, pathname] = uiputfile('*.dat','Name to File for Save

the Config of the Robot'); ... ... ... fid = fopen(file,'w'); % Abrir fichero para escribir

% Save dato nº de elementos de cada vectos de datos fprintf(fid,'Grain Time\n'); fprintf(fid,'%g\n',dt);

% Save dato nº de elementos de cada vectos de datos fprintf(fid,'nº_elementos\n'); fprintf(fid,'%g\n',length(x_ag(i,:))); ... ... ... fprintf(fid,'logp_m_i\n'); % Save dato de logp_m_i del Robot fprintf(fid,'%g\n',logp_m_i(i,:)); ... ... ... fprintf(fid,'Self\n'); % Save vector de estado del Robot fprintf(fid,'%g\n\n',self);

fclose(fid); % Cerrar fichero para Save ... ... ... return; % Salir de la función Save Data

Observar, que entre otros datos se salva el vector de estado del Robot para cuandosea recuperado posteriormente se realice el set del Robot con este mismo vector de estado.

Durante el proceso de Save, en la pantalla aparece un mensaje de espera, el cualindica finalmente el nombre del fichero en el cual se han salvado los datos.

Seguidamente se presenta un ejemplo del formato del fichero de datos relativo a losresultados de la simulación (en este caso sólo hay almacenados 2 elementos por cada dato).

Grain Time0.015nº_elementos2x_ag120119.909y_ag5555.5973log_vlin040.277log_vang010.0692logv_m_d080.554logi_m_d0↓

↓3logv_m_i00logi_m_i0-3logp_m_d00.315logp_m_i0-0.315Self118.77357.60162.3055438.10979.527430


104

2.7.15.2 Recuperar los Resultados de la Simulación (función filedata())

En la misma barra de menú Options, en la parte superior de la figura principal desimulación “Physical Robot Simulator”, se encuentra el label Load data asociado a lafunción filedata(0) con el que se recuperan los datos a partir de la lectura de un fichero,donde anteriormente han sido guardados en un formato determinado.

La función filedata(0), se divide en los siguientes puntos:

1- Leer y comprobar índice del Robot.2- Obtener el nombre del fichero de datos (caja de diálogo).3- Abrir fichero para lectura.4- Lectura-recuperar datos en el fichero de texto.5- Cerrar fichero de texto.5- Set vector de datos del Robot (no superposición con otros elementos).

Leído el índice del Robot, y a través de la caja de diálogo uigetfile [1], el nombredel fichero (extensión por defecto *.dat), se abre este fichero en modo de lectura.

A partir de un bucle, se leen sucesivamente los diferentes datos, para almacenarlosen las variables de representación gráfica log... Este método de lectura, en un bucle, esaplicable gracias al formato realizado a la hora de salvar-grabar los datos.

function [] = filedata(op)... ... ...elseif op == 2 % Opción abrir-Load fichero datos-resultados

% self = [coordenada"x" coordenada"y" theta radius velolineal % veloangular pertur] self = gagent(i,'Self'); % Leer dato self del Robot

% Uigetfile, leer nombre del fichero para Load [namefile, pathname] = uigetfile('*.dat','Name to File for Load

the Config of the Robot'); ... ... ... fid = fopen(file,'r'); % Abrir fichero para lectura

lectura = fgetl(fid);

while ~(feof(fid)) & lectura~='' % Leer mientras no final

lectura = lower(lectura); % Convertir en minúsculas

if strcmp(lectura,'grain time') % Lectura Grain Timedt = str2num(fgetl(fid));

end; % end if strcmp(lectura,'nº_elementos')

% Lectura nº elementos de los vectores if strcmp(lectura,'nº_elementos')

n_ele = str2num(fgetl(fid)); end; % end if strcmp(lectura,'nº_elementos') ... ... ...


105

% Lectura datos característicos del Robot if strcmp(lectura,'self')

lectura=fscanf(fid,'%g',[7 1]);self = lectura';

end; % end if strcmp(lectura,'self')


end; % end while ~(feof(fid)) & lectura~=''

fclose(fid); % Cerrar fichero para Load ... ... ... % Evitar superposición del Robot con otros elementos de la % simulación error = noencima(self,i,'r'); ... ... ... end; % end if op == 1

Recuperados los datos de intensidad, velocidad y posición (variables globaleslog...), se recupera el vector de estado almacenado en el fichero, para asignarlo al Robotcon la función sagent(...), comprobando antes la no superposición del Robot con otroselementos de la simulación.

Recordar, que a la hora de salvar los datos de la simulación, también se ha salvadoel grain time, que ha de ser recuperado del fichero al realizar el Load de datos.

2.7.15.3 Representación Gráfica de los Resultados (función plotdata())

La finalidad de este programa es la simulación de un Robot móvil modelando losmotores y el sistema de control, para estudiar la respuesta obtenida y poder sacarconclusiones sobre las características del sistema diseñado (motores, sistema de control,sensores). Uno de los métodos utilizados para interpretar estos resultados es larepresentación gráfica (evolución temporal), presentada gracias al push-button plotdata dela pantalla principal de simulación, figura “Physical Robot Simulator”.

Este push-button asociado a la función plotdata(), realiza la representación de losdiferentes resultados obtenidos; velocidad lineal, velocidad angular, intensidad y par de losmotores. Esta función utiliza como principal herramienta la orden de representaciónbidimensional plot. [2]

% Representación de las coordenadas del Robotplot(x_ag(i,:),y_ag(i,:),color(i));

Los datos para la representación gráfica, se almacenan en matrices, donde la fila deestas matrices se asocian a los índices de los Robots “a” y las columnas al número deintervalo de simulación “b”:

...logv_m_d(a,b) = v_m_d;logv_m_i(a,b) = v_m_i;logi_m_d(a,b) = i_m_d;...


106

Para cada dato representado, se crea una nueva figura, hasta un total de tres, quefacilita la presentación de los resultados.

figure('Name',['Graphic of the Dates of the Motors Robot 'num2str(i)],'interruptible','yes','Units','normalized','Position',[0 0 1 1]);... ... ...

La representación en función de la variable de tiempo (dt) que se realiza de lasvariables de velocidad, intensidad y par, se obtiene de valores muestreados cada grain timesegundos (variable dt). Entonces, el vector de tiempos debe de tener valores proporcionalesal grain time de muestreo y una longitud igual al número de muestreos realizados.

% Vector de tiempos. División temporal cada grain time segundos.% b = longitud del vector resultado (nº de muestras realizadas)t = [0:dt:(b-1)*dt];... ... ...

2.7.16 Pantalla de Configuración de la Simulación

Todo programa de ordenador necesita de unos datos por defecto que lo configuren deinicio de forma que establezcan unas mínimas condiciones y restricciones en sufuncionamiento.

Cuando se ejecuta la función de inicio del programa, comensa, en la función dedeclaración de variables, invargen(...), se realiza la lectura de los valores por defecto decada variable de configuración del programa, grabados en el fichero de texto llamado‘varcfg.m’. Gracias a este fichero de texto ‘varcfg.m’, con una estructura determinada, sepueden guardar y leer posteriormente los valores de las variables para la configuración delprograma.

Figura 43. Pantalla de Configuración del simulador

Número máximo deRobotsTamaño máximo delRobotTamaño mínimo delRobot

Número máximo desensores

Distancia mínima dedetecciónÁngulo máximo dedetecciónÁngulo mínimo dedetecciónMáximo ángulo derotación del sensor

Número máximo deobstáculosMáximo tamaño delobstáculo

Tiempo de duración delos intervalos de muestreo

Push-button de Salidade la configuración

Selección de la velocidad de la animacióngráfica en simulación continua (Run).

Distancia máximade detección


107

En esta nueva pantalla, mostrada con la pulsación del label Change Data Config delmenú Options de la pantalla de simulación, se pueden modificar los valores de lasdiferentes variables que establecen los valores máximos y mínimos permitidos en lasimulación, como por ejemplo, el número de sensores, número de Robots, distancia dedetección, etc. Una de estas importantes variables de configuración, el grain time de lasimulación, se configura en esta pantalla.

Cuando el usuario inicia la sesión de trabajo con el programa, primero tiene quecomprobar los valores configurados por defecto en el programa, y, corregirlos o no, segúnsu conveniencia. En caso de cambiar algún valor, es necesario cerrar la pantalla deconfiguración utilizando el push-button Quit Save. Esto se debe a que este push-buttontiene asociada la función quit1(3), donde se reconfiguran los valores de cada variable y laestructura de datos del Robot, según las modificaciones realizadas por el usuario.

2.7.16.1 Cerrar la Pantalla de Configuración

La función quit1(3) que se ejecuta cuando se pulsa el push-button Quit Save de lapantalla de configuración, realiza la lectura de todas las variables de esta pantalla, parasalvar los valores en el fichero de texto ‘varcfg.m’ y reconfigurar el programa desimulación con los nuevos datos de configuración.

Salvados los datos de configuración en el fichero ‘varcfg.m’, y después dereconfigurar las diferentes estructuras de datos de los elementos de la simulación, seactualizan los valores de las variables de configuración, gracias a la ejecución deinvargen(0), donde se vuelve a leer del fichero ‘varcfg.m’, los nuevos datos deconfiguración, para cerrar después la pantalla de configuración, validando los cambiosrealizados y liberando de la memoria las variables auxiliares utilizadas durante el procesode cambio de la configuración.

function[error] = quit1(op)... ... ...elseif op==3 % Salvar datos de configuración en el fichero de "varcfg.m" fid = fopen('varcfg.m','w'); % Abrir fichero para escribir

% ------------------------------------ % Inicio salvar datos de configuración % ------------------------------------ fprintf(fid,'diana\n'); % Save dato diana ... ... ... if get(DIN,'value') fprintf(fid,'on\n','on'); diana = 'on'; else fprintf(fid,'off\n','off'); diana = 'off'; end % end if get(DIN,'value') ... ... ...


108

% Al producirse un cambio en la configuración, es necesario % estructurar los datos ya existentes tanto de los obstáculos % como de los Robots for i = (MAX_OBSTACLES+1):MAX_OBST % Crear patch gráfico para Obstáculo ... ... ... end; % end for i=(MAX_OBSTACLES+1):MAX_OBST

for i = 1:length(AGENTS) ... ... ... end; % end for i = 1:length(AGENTS)

invargen(0); % Función declaración variables globales% generales (actualizar)

% Liberar memoria, clear variables de la pantalla de configuración clear ... ... ...

NOTA: En algún caso, puede ser necesario reconfigurar la estructura de datos delRobot, cuando se cambia, por ejemplo, el número de sensores. De igual forma, en caso decambiar el número de obstáculos, también es necesario reconfigurar el vector demanejadores de los obstáculos.

2.7.17 Anexo de Funciones Auxiliares

Durante la ejecución de la simulación se utilizan diferentes funciones auxiliares (noasociadas a ningún elemento o acción), las cuales realizan operaciones muy determinadas ycaracterísticas, que se repiten a lo largo de la ejecución del programa. Estas funciones sepresentan en los siguientes apartados.

2.7.17.1 Función de no Superposición noencima(selfobj,i,tipo)

En la simulación, hay elementos, como Robots, obstáculos, etc. que pueden sermovidos por el usuario a partir de las herramientas de set de la posición [x,y]. Para evitar lasuperposición entre los elementos cuando el usuario asigna la posición, se utiliza la funciónnoencima(selfobj, i, tipo).

Esta función, noencima(selfobj, i, tipo) evita la superposición entre elementos de lasimulación y basa su funcionamiento en calcular la distancia entre las coordenadas delelemento referenciado en los parámetros de entrada de la función y el resto de elementosexistentes en la simulación.

Este cálculo se realiza a través de dos bucles tipo ‘for’ [1], donde se recuperan lasposiciones de los elementos de la simulación para calcular de forma iterativa la distanciaentre los elementos. Cuando esta distancia es inferior a la suma de los radios de los doselementos, sabemos que los elementos se superponen, entonces, se retorna como parámetrode salida ‘error = 1’.


109

function[error] = noencima(selfobj,i,tipo)... ... ...% selfobj = [xobj yobj theta radius...] vector de estado objeto a% comprobar, no hay superposición% "i" índice del elemento, ya sea Robot, obstáculo u objeto% grasping% "tipo" tipo de elemento referenciado% --- Robot 'r' 'o' Obstáculo ---

error = 0;

% Control del choque entre los Robots, no superposiciónfor j = 1:length(AGENTS) % existe Robot

% Recuperar "UserData" state de los Robots % Existe Robot? if ((AGENTS(j) ~= 0) & (~error) & ((j~=i) | tipo == 'o')) -Recuperar vector de datos del Robot 'j'

self = [x y theta radius velolineal veloangular pertur] -Cálculo de la distancia Robot 'j'-Objeto entrada

if distancia Robot 'j'-Objeto entrada < suma de radios,superposición ?

-SI ! superposición con Robot 'j' end; % end if distancia Robot 'j'-Objeto ... -Comprobar superposición objeto “grasping” ... ... ...end; % end for j = 1:length(AGENTS)

if ~error & tipo ~= 'o' % Superposición obstáculos-Robots ? % Control superposición con los Obstáculos de la simulación for j = 1:length(OBSTACLES) -Recuperar "UserData" state del obstáculo “j”

if (u ~= 0) & (~error) % Existe obstáculo?-Recuperar vector datos del Obstáculo-Cálculo distancia objeto entrada-Obstáculo

if distancia objeto entrada-Obstáculo < suma de radios,superposición ? -SI ! superposición con obstáculo "j"end; % end if distancia objeto entrada-Obstáculo...

... ... ...end; % end if ~error

Es importante el conocer, con los parámetros de entrada, el índice de los elementosy el tipo de elemento (Robot tipo = 'r', obstáculo tipo = 'o') para evitar que el elemento seacomparado consigo mismo, provocando un “error” de funcionamiento en la funciónnoencima(selfobj,i,tipo).

NOTA: Sólo se permite que exista superposición entre los obstáculos, para facilitarla composición de diferentes entornos de simulación. Esta superposición esta limitada, yaque se obliga a que exista una distancia mínima de seguridad entre los obstáculos (mirarla función noencima(selfobj,i,tipo) en el Anexo de programación), para evitar unasuperposición total.


110

2.7.17.2 Función de Control de los Límites de la Simulación bounds.m

Para evitar que los elementos de la simulación superen los límites espaciales desimulación se utiliza una sencilla función en formato simulink. Este fichero simulink,‘bounds.m’, consiste en dos elementos de saturación, con los márgenes de máximo ymínimo de las coordenadas [x, y] de la simulación. Antes de realizar el set de lascoordenadas en la estructura de datos de cualquier elemento de la simulación, es necesariocomprobar que las coordenadas no superen los límites, corrigiéndolas en caso necesario.

Figura 44. Fichero simulink de saturación de las coordenadas

Recordar que las variables utilizadas en los ficheros simulink, tienen que serdeclaradas como variables globales en el programa [1], sin ser necesario el paso deparámetros en este tipo de ficheros [rk45... consultar ayuda de MATLAB].

Ejemplo de programación, para la ejecución de la función de simulink ‘bounds.m’.

% -----------------------------------------------------% wwx wwy wx wy R -> variables del fichero de Simulink.% bounds -> fichero simulink% Función de saturación de las coordenadas del Robot% xmax = [0+R 130-R], ylim = [0+R 90-R].% -----------------------------------------------------

temps = 1e-5;wwx = self(1);wwy = self(2);R = self(4);

% Control límites del terreno de juego del Robot "a"rk45('bounds',temps,[],[1e-6 temps temps 0 0 2]);self(1) = wx;self(2) = wy;

Wx

WWx

130 - Radio elemento

0 + Radio elemento

WWx1

WWx1

x2WW

WWx2

WWx3WWx4

wy

wy

wwy

wwy

wwx

wwx

wx

wx

Función para la saturación de las coordenadas del Robot u otros elementos

Las variables wwx, wwy, wx, wy y R son variables tipo GLOBALS

Parámetros saturación : R = Radius Object, Xmax = 130, Ymax = 90


111

2.7.17.3 Función [índice] = popup(vector,pop,opción,txt,mensa)

Esta función auxiliar se divide en dos bloques y acciones a realizar dependiendo dela “opción” de entrada seleccionada. Cuando ‘opción = 1’, la función retorna en elparámetro de salida ‘índice’ el índice seleccionado por el usuario en el uicontrol popupreferenciado [1] con el parámetro de entrada ‘pop’. En caso de estar vacío el vectorreferenciado ‘pop’, es decir, que el popup de la pantalla no tiene elementos, se presenta elmensaje de error ‘mensa’ pasado como parámetro de entrada y referenciado en la variablede texto ‘txt’.

En ‘opción = 1’, se seleccionarán las posiciones no vacías del vector ‘vector’, y unavez confirmado que este vector no está vacío, se obtiene la posición elegida en el popup dela pantalla (ver Figura 45).

1 0 3 4 0 6'vector'

vector menu 1 643

popup pantalla 1

3

4

6

Si seleccionamos en el popup

get('pop','value') = 2

índice seleccionado en el popup

de la pantalla el índice nº 1

menu(get('pop','value')) = 1índice de salida = 1

Figura 45. Lectura del índice seleccionado en el popup

function [indice] = popup(vector,pop,opcion,txt,mensa)... ... ...if opcion % Índice seleccionado en el popup

menu = find(vector); % Índices ~= 0 en el popup

if ~isempty(menu); % Comprobar vector no vacío % get(pop,'Value') contiene la posición selecciona en el vector % popup indice = menu(get(pop,'Value')); % Obtener índice, value = posición vector popup

else % NO hay índice, presentar mensaje set(txt,'string',mensa,'visible','on'); end;


112

En cambio, cuando ‘opción = 0’, la operación a realizar es la actualizar el popup deíndices de la pantalla, con las posiciones no vacías del vector ‘vector’ (ver Figura 46). Eneste caso, no es necesario el mensaje de error ‘mensa’, ni el manejador de la variable detexto ‘txt’.

1 0 3 4 0 6'vector'

vector menu 1 643

popup pantalla 1

3

4

6

valores ~= 0

vector de datos'string'

Figura 46. Actualizar los índice existentes en el popup

function [indice] = popup(vector,pop,opcion,txt,mensa)... ... ...else % Renovar "popup" con los índices del vector 'vector'

% Update Menu. Renovar índices del popup "pop" menu = find(vector); % Índices ~= 0 en el popup

if length(menu) == 0 % Comprobar que existen elementos indice = 0; set(pop,'String','','Value',0);

else % else if length(vector) == 0

indice = menu(1); s = sprintf('%2d',menu(1)); for j=2:length(menu) % Ordenar índices s = [s sprintf('|%2d',menu(j))]; end; % end for j=2:length(menu) set(pop,'String',s); % Set en el popup referenciado

end; % end if length(vector) == 0end; % end if op...


113

2.7.17.4 Función [distan] = discua(angulo,radio_obs,angu_obs)

Esta función auxiliar se encarga del cálculo de la distancia entre el centro y elcontorno de un elemento cuadrado, dependiendo del ángulo desde el que se mira esteelemento. Esta función sólo se aplica en los elementos cuadrados, ya que no tiene sentidoen los circulares al ser esta distancia siempre la misma (ra_obs).

Obstáculo

ra_obs angu_obs

distan

anguloangu_obs

distanangulo

distanra_obs

ra_obs

angulo

Figura 47. Ejemplos de utilización de la función discua(...)

NOTA: El robot, en la representación gráfica, no es totalmente cuadrado, pero se realizauna pequeña aproximación para no complicar la programación.

)33()ángulocos(

)45cos(·obs_radiostandis =

Este valor distan, que sólo es aplicable en los elementos cuadrados de lasimulación, no siempre es utilizado en las funciones de control de choque entre loselementos de la simulación, porque sería necesario realizar un estudio más amplioreferente a los choques en la simulación, para que tuviera algún sentido “físico” el aplicarel cálculo de esta distancia o de cualquier otro valor necesario.

function [distan] = discua(angulo,radio_obs,angu_obs);... ... ...angulo = angulo - angu_obs; % Corregir ángulo existente entre el% obstáculo-Robot y otro punto de referencia según el ángulo de% rotación del obstáculo-Robot

while angulo < -pi angulo = angulo + pi;end;

% Distancia desde el centro del obstáculo-Robot hasta su contorno% según el ángulo "angulo"if abs(angulo) <= (pi/4) | abs(angulo) >= (3*pi/4)% Distancia de separación adaptada a la forma cuadrada distan = abs((radio_obs*cos((1/4)*pi)) / cos(ángulo));else distan = abs((radio_obs*sin((1/4)*pi)) / sin(ángulo));end;end;


114

2.8 Ejemplos de Simulación

En los apartados anteriores, se ha explicado toda la estructura y el desarrollollevado a cabo para realizar este programa de simulación. Ahora bien, una vez se conoce laprogramación y el funcionamiento interno del programa se plantean una serie de ejemplosde simulación para poner a prueba y estudiar el correcto funcionamiento del mismo, esdecir, si realmente toda la programación realizada obtiene unos resultados coherentes quepuedan ser estudiados de forma didáctica.

2.8.1 Modelado del Sistema de Control y de los Motores del Robot

El primer ejemplo de simulación consiste en modelar en el fichero simulink elsistema de control y los motores del Robot (sistema motriz). El modelo del sistema decontrol que se pretende modelar se estructura como se muestra en la Figura 48 (posiblemontaje práctico) [4] [5] [7]:

A CT U A DO RPW M

E T AP A DEPO T E NC IA

PLA N TA

CO NSIGNA S

R E L O JM UESTRE O

TRA N SD UC TORSE N SO RE S

SISTEM A D E CO N TR OL DE LO S M O TO R ES (CPU C ENTR AL ó IND EPEND IEN TE)

- CO N TRO LAD OR(PI, PID ,...)

CPU C EN TRA L

VA RIABLESDE

EN TRAD A

CA LCU LO+

+

(M O TO R 1)VA RIABLES

SALIDADE

(M O TO R 2)PLA N TACO N TRO LAD OR

(PI, PID ,...)PO T E NC IAE T AP A DE

PW MA CT U A DO R

SE N SO RE STRA N SD UC TOR

M UESTRE OR E L O J

-

R E L O JM UESTRE O

Figura 48. Diagrama de bloques del sistema de control

NOTA: Recordar que el fichero simulink, donde se modela el sistema de control, essimulado en intervalos de duración de grain time segundos, que equivale al muestreo(reloj). Es decir, se obtiene una muestra-respuesta del sistema cada grain time segundos.

En el diagrama anterior se muestra el conjunto del sistema de control del Robot queen este ejemplo se pretende modelar. Éste se divide en tres partes:

a) Una CPU central (MASTER) que distribuye el control de los procesos adiferentes controladores (SLAVE’s), realizando sólo el cálculo de lasconsignas.

b) Un µC que se encarga de forma independiente de controlar la velocidad-posición el funcionamiento de los motores a través de un actuador (PWM).

c) La planta-proceso, que en este caso son los motores.


115

El modelo del sistema motriz del Robot se basa en una CPU central que distribuyeel control de los procesos a diferentes µC’s para que actúen de forma independiente. Estossólo necesitan la consigna o valor de referencia en su entrada, obteniéndolo de la CPUcentral. En este caso, el dato será la consigna de velocidad-posición para los motores.

Para iniciar este ejemplo, empezaremos por el tercer punto, modelar la planta-proceso, que en este caso son los motores.

2.8.1.1 Modelado del Motor

La posibilidad de modelar diferentes motores de forma sencilla gracias a losficheros simulink, permite estudiar de forma didáctica la idoneidad y los factores másimportantes para la elección de un motor en este tipo de aplicaciones. Por ejemplo, sepueden comparar motores de diferentes fabricantes, con diferentes características paraestudiar la respuesta obtenida y decidir que motor nos es más rentable y se adapta mejorpara una determinada aplicación.

Para este ejemplo, el motor que se desea estudiar es un motor de CorrienteContinua del fabricante MAXON Motor, con los siguientes datos característicos (ver Anexode información técnica).

Fabricante: Maxon Motor.Alimentación: Corriente Continua.Tipo de motor: Motor de escobillas de grafito (118575).

- Para aplicaciones de servocontrol con paros/arranques rigurosos.- Motor de reducidas dimensiones y poco peso.- Bajo coste, comparado con los motores de escobilla y colectores de metal

precioso (para aplicaciones semejantes).

DATO DESCRIPCIÓN VALORPN Potencia nominal asignada (W) 1.5 WVN Voltaje de alimentación nominal (V) 7.2 VωωωωMAX Velocidad máxima permitida (rpm) 16000 rpm - 1675.52 rad/sIMAX Intensidad máxima continua (A) 267 mAIINI Intensidad de arranque máxima (A) 633 mATMAX Par máximo continua permitido (m·Nm) 1.32 m NmPINI Par de arranque máximo (m·Nm) 3.13 m NmKb Constante de la fuerza contraelectromotriz (V·s/rad) 4.948·10-3 V·s/radKi Constante del par (Nm/A) 4.95·10-3 Nm/ARa Resistencia de la armadura (Ω) 11.4 ΩLa Inductancia de la armadura (H) 0.240·10-3 HBm Coeficiente de fricción viscosa (Nm/ras/s) 2.155·10-6 Nm/rad/sJm Inercia del rotor (Kg·m2) 0.190·10-7 Kg·m2

ττττm Constante mecánica temporal del motor (s) 13.5 msTabla 8. Datos característicos de un motor de CC del fabricante Maxon

NOTA: Para obtener mayor información referente al motor (Tabla 8), consultar el Anexode información técnica.


116

Conocidos los datos característicos del motor, se procede a realizar un modelo, tal ycomo se muestra en la Figura 49 (ver apartado 2.7.8.3 Fichero de control Simulink) [2] [3][4]:

Figura 49. Modelado de los motores en el fichero simulink

Todo motor tiene limitaciones físicas, es decir, unos valores máximos defuncionamiento como pueden ser; la intensidad máxima y el par máximo que soporta. Deahí la aplicación de los elementos de saturación, con los datos característicos máximos delmotor (tensión de alimentación, intensidad, par) para hacer un modelo más realista [3] [4].

Una vez modelado y caracterizado el motor, para integrarlo en el sistema motriz delRobot, se ha de diseñar el controlador de éste.


117

2.8.1.2 Diseño del Sistema de Control

Después de definir el proceso-planta a controlar, el usuario del simulador debeescoger un tipo de controlador, que con la selección adecuada de los valores de sus factoressatisfacerá todas las especificaciones del diseño. Realmente se han de tener muchosfactores en consideración a la hora de diseñar un controlador, sobre todo si después sepretende realizarlo en la práctica. Normalmente se escoge el controlador más simple quecumpla con las especificaciones requeridas, ya que en la mayoría de los casos, el máscomplejo, es el más costoso, el menos fiable y el más difícil de diseñar. Se recomiendatener muy claro estos tres pasos para la elección y diseño de un controlador:

a) Determinar qué hace el sistema y cómo hacerlo (especificaciones de diseño).b) Determinar la configuración del compensador o controlador relativa a cómo

está conectado el proceso controlado.c) Determinar los valores de los parámetros del controlador para alcanzar los

objetivos de diseño.

Uno de los controladores más ampliamente empleados para este tipo de sistemas esel controlador PID, el cual aplica una señal al proceso que es una combinaciónproporcional, integral y derivativa de la señal que actúa sobre el proceso-planta, aunquetambién existen otros tipos de controladores como los FUZZY.

Configuración del Controlador (PID)

Un sistema de control típico (Figura 50) suele ser aquel que compara la salida delproceso con la entrada (consigna), para según la diferencia existente corregir la salida (lazocerrado, realimentación).

1s-

+ω (s)REFmω (s)

θ (s)mCONTROLADOR

PLANTAPROCESO

(MOTORES)

ε(s) C(s)

PID

REALIMENTACIÓN

LAZO CERRADO

CONTROLADOR

PIDREF(s)ω PROCESO mθ(MOTORES) s

PLANTA(s)C mω (s) 1 (s)

LAZO ABIERTO

Figura 50. Sistemas de control, con realimentción y sin realimentción

En este caso se utiliza el sistema llamado de control realimentado o control en lazocerrado, aunque seria un buen ejercicio el estudiar la respuesta del sistema utilizando elcontrol en lazo abierto. De los dos sistemas, el lazo cerrado es muy popular debido a susbuenos resultados y a la imprecisión en ciertas condiciones del sistema en lazo abierto.


118

Realimentación

Características de un Sistema Retroalimentado

a) El error respecto a la consigna de entrada, es sustancialmente menos sensible alas variaciones de parámetros del sistema que un controlador de lazo abierto, sin necesidadde conocer de forma precisa las características del sistema para lograr un control preciso[3] [4] [5].

b) El error es menos sensible a las perturbaciones exteriores que en un sistema enlazo abierto [3] [4] [5].

c) Un controlador de lazo abierto no tiene efecto sobre la dinámica del sistema enrespuesta a perturbaciones exteriores, por eso los sistemas retroalimentados, que si afectana la dinámica, son más lentos de respuesta y de estabilidad, aunque más fiables [3] [4] [5].

La realimentación formada por la suma de un factor proporcional, más un integral ymás un diferencial se conoce como control PID. En la transformada de Laplace se define(Figura 51):

1s-

+ω (s)REFmω (s)

θ (s)mCONTROLADOR

PLANTAPROCESO

(MOTORES)

E(s)

PID

REALIMENTACIÓN

U(s)

Figura 51. Modelado en bloques de un sistema de control con realimentción

)34()s(E)sTsT

11(K)s(U DI

P ++=

La combinación de los tres modos (P I D) es utilizada, para proveer un gradoaceptable de reducción del error, simultáneamente a una estabilidad y una amortiguaciónaceptables.

Seguidamente se procede a sintonizar las constantes KP, TI y TD para este ejemplopráctico de controlador de un motor del fabricante MAXON Motor, utilizando el simulador.

Asignación de los Parámetros del PID

Están disponibles métodos de diseño sofisticados, para desarrollar un controladorque satisfaga las especificaciones en estado estacionario y transitorio, para rastreos de loscomandos de entrada y rechazo de perturbaciones. Estos métodos, requieren modeloscompletos de la dinámica del proceso que se va controlar en forma de ecuaciones demovimiento o de funciones de transferencia.

Para el ajuste de las constantes del controlador PID, utilizamos un métodoempírico, basado en las reglas de Ziegler y Nichols [3] [4]. Estos reconocieron que larespuesta de escalón de la mayoría de los sistemas de control de procesos tiene la curva enforma de S (ver figura anterior), denominada curva de reacción del proceso y se puede


119

generar experimentalmente o a partir de una simulación dinámica de la planta. La forma dela curva es característica de los sistemas de orden superior, y el comportamiento entrada-salida de la planta se puede aproximar por:

Figura 52. Curva de reacción de un proceso según Ziegler&Nichols [3] [4]

)35(1s

eK)s(U)s(Y)s(G

ds

+τ==

τ

que es simplemente un sistema de primer orden más un retraso de transporte. Lasconstantes de la ecuación anterior pueden determinarse a partir de la respuesta del escalónunitario del proceso [3] [4].

Ziegler&Nichols dieron dos métodos para sintonizar el controlador. El utilizado eneste ejemplo de simulación consiste en la evaluación del sistema en el límite de estabilidad.La ganancia proporcional se incrementa hasta que se observan oscilaciones continuas, osea, hasta que el sistema llega a ser marginalmente estable. La ganancia correspondienteKCR (también llamada última ganancia) y el periodo de oscilación TCR (también llamadoúltimo periodo).

1s-

+ω (s)R EFmω (s)

θ (s)mPLA N TA

PRO CESO

r ey

K CR

Figura 52. Método de cálculo de los parámetros del PID Ziegler&Nichols [3] [4]

Este periodo debería medirse cuando la amplitud de la oscilación es bastantepequeña. Entonces se retrocede desde esta ganancia, como se muestra en esta tabla.

Tipo de controlador Ganancia óptimaKP = 0.6·KCR

TI = (1/2)·TCRPIDTD = (1/8)·TCR

Tabla 9. Parámetros del PID aplicando Ziegler&Nichols [3] [4]

La experiencia ha mostrado que los ajustes del controlador de acuerdo con lasreglas de Ziegler&Nichols a menudo proporcionan una buena respuesta en lazo cerradopara muchos sistemas. La sintonía final del controlador puede realizarse manualmente porel usuario para obtener el mejor control posible adaptado a cada aplicación, aunque con


120

el método de Ziegler&Nichols se consigue un buen control, no siempre nos puede interesarel tipo de respuesta obtenida (porque es poco amortiguada o demasiado rápida en el inicio).

2.8.1.3 Cálculo de la Consigna

Como ya se ha comentado en los apartados anteriores, el control y el cálculo de laconsigna se realiza de forma separada del control de los motores. La consigna de posicióndel robot (señalada en coordenadas [x, y] por el setpoint asociado al Robot) es uno de losparámetros de entrada del fichero simulink de control, expresada de la siguiente forma:

d_rela_rel d_rel [cm] distancia entre el Robot y el setpoint asociadoa_rel [rad] ángulo entre el Robot y el setpoint asociado

A partir de la distancia lineal existente entre el Robot y su setpoint (consigna deposición) podemos calcular la velocidad lineal deseada en función de esta distancia, deforma que (ver fórmula 35):

Para una máxima distancia d_relMAX ≈ 160 cm., la máxima velocidad lineal será:

)36(VVVVsi2

VVV MAX2,1MMAXLINMAX1MMAX2M

MAX1MMAX2MMAXLIN =→=

+=

entonces:

Máxima distancia d_relMAX → VLIN MAXDistancia d_rel → VLIN

)37(rel_d

VKrel_d·K

rel_dVrel_d

VMAX

MAXLIN11

MAX

MAXLINLIN =→==

En el programa simulink:

Figura 53. Cálculo de la consigna lineal representada en el fichero simulink

VLIN = velocidad lineal del Robot [cm/s] drel = consigna de distancia lineal [cm]


121

Del mismo modo, para la velocidad angular:

Máximo espacio angular a_relMAX = ±π, tendremos la máxima velocidad angular.

)38(D

V0Vsi

DVV MAX2,1M

MAXANGMAX2,1MMAX1MMAX2M

MAXANG =ω→=−

=ω

entonces:Máximo ángulo a_relMAX π → ωANG MAXDistancia a_rel → ωANG

)39(rel_a

Krel_a·Krel_a

rel_a

MAX

MAXANG22

MAX

MAXANGANG

ω=→=

ω=ω

En el programa simulink:

Figura 54. Cálculo de la consigna angular representada en el fichero simulink

ωANG = velocidad angular del Robot [rad/s] arel = consigna de espacio angular [rad]

D = distancia entre ruedas del Robot [cm]

Una vez que se obtienen las velocidades lineal y angular proporcionales a ladistancia lineal y al ángulo respectivamente, se separan las consignas de control develocidad para cada motor, a partir de los siguientes cálculos:

)44(RV)43(

RV

)42(2DV2VVdespejando)41(

2DV2V

VaislarpodemosahoraVV2VDV

iablesvarlasdeunadespejando)40(D

VV2

VVV

2

2M2M

1

1M1M

ANGLIN2M2M

ANGLIN1M

1M1MLIN1MANG2M

1M2MANG

1M2MLIN

=ω=ω

ω+=→ω−=

−=+ω=

−=ω+=

VM1 = velocidad lineal del motor1 [cm/s] VM2 = velocidad lineal del motor2 [cm/s]

R1,2 = radio de las ruedas[cm] ωM1 = velocidad angular del motor1 [rad/s]

ωM2 = velocidad angular del motor2 [rad/s]


122

Del cálculo anterior, representado en el fichero simulink como se muestra en lafigura, se obtienen las consignas de velocidad para cada motor (ver Figura 55).

Figura 55. Cálculo de las consignas lineal y angular representada en el fichero simulink

Hay que tener en cuenta, que para conseguir un control de posición aceptable (errorfinal ≈ 0), la variable de distancia lineal d_rel, puede llegar a tener valores muy pequeños,llegando a no ser suficientemente grande para excitar la entrada de los motores (consignade velocidad lineal ≈ 0). Por eso, es recomendable multiplicar d_rel por un factor, paraaumentar la consigna, pero evitando la sobreexcitación del sistema.

2.8.1.4 Estudio de los Resultados

Realizado el modelado de todo el sistema de control de los motores del Robot, seprocede a la simulación para la obtención de los resultados y su posterior estudio. Elmodelo del sistema de control (fichero simulink) queda de la siguiente forma (Figura 56):

Figura 56. Modelado del sistema de control en el fichero simulink

Para este primer ejemplo se utiliza el movimiento en la opción other, para estudiarla respuesta del sistema, principalmente en el control de la trayectoria y la precisión en laposición final del Robot.


123

El primer paso, es el ajuste de los parámetros del PID, utilizando el métodoanteriormente comentado (Ziegler&Nichols) [3] [4]. Esto conlleva la ejecución demúltiples simulaciones hasta que se obtiene la respuesta en el control que se cree másconveniente para este tipo de aplicación, esto significa que no siempre los parámetroscalculados utilizando el método de Ziegler&Nichols son los más apropiados, y se acabanajustando manualmente. Finalmente, se obtiene una respuesta, como la que se presenta enlas siguientes gráficas, pero con un controlador PI (ver Figuras 57, 58, 59) [2].

XINICIAL ROBOT = 45YINICIAL ROBOT = 45ÁnguloINICIAL ROBOT = 180ºGrain time = 5 ms.Kp = 16e-4Ki = 175e-3Total grain time = 3,65 s.CPU time = 3,69 s.

Figura 57. Representación de los resultados de velocidad del primer ejemplo


124

Figura 58. Representación de la trayectoria del Robot en el primer ejemplo

Figura 59. Resultados de velocidad, intensidad y par de cada motor

x = 45, y = 45 x = 64.5, y = 45.05


125

Como se observa, la respuesta es muy satisfactoria, porque se consigue unatrayectoria minimizada, y un error final reducido. Pero en la última gráfica se aprecian,tanto en la intensidad de cada motor como en el par de trabajo, irregularidades y picosdebido a una pequeña inestabilidad del sistema de control. Sería apropiado conseguirreducir y amortiguar estos picos para estabilizar la respuesta. Esto se consigue con laadición del término derivativo, que tiene las propiedades de aumentar la amortiguación y lade incrementar la estabilidad del sistema.

La respuesta siguiente se obtiene con un controlador PID.

XINICIAL ROBOT = 45YINICIAL ROBOT = 45ÁnguloINICIAL ROBOT = 180ºGrain time = 5 ms.Kp = 16e-4Ki = 175e-3Kd = 0.25e-4Total grain time = 1,38 s.CPU time = 1,34 s.

Figura 60. Representación de los resultados de velocidad del segundo ejemplo


126

Figura 61. Resultados de velocidad, intensidad y par de cada motor (segundo ejemplo)

La respuesta se ha amortiguado a la vez que aumenta la estabilidad, pero, seprovoca un aumento del error en el régimen estacionario (reducción de la ganancia), quelleva a que la posición final del Robot [x = 61,53, y = 46,62] se aleje de la consigna deposición [x = 65, y = 45]. Para corregir este efecto de la adición del término derivativo seaumenta el valor del parámetro integral ya que:

- El tiempo integral, que es el tiempo de restablecimiento, modifica la velocidadde respuesta del sistema [4] [5] [11].

- El término integral, reduce o elimina los errores de estado estacionario, pero acosta de la estabilidad [4] [5] [11].

Por otro lado, el error se puede disminuir también aumentando el términoproporcional, y por tanto, aumentando la fuerza de la respuesta y la velocidad para llegar alerror 0 (inestabilidad ↑). Para la mayoría de los sistemas hay un limite superior en laganancia de realimentación para lograr una respuesta bien amortiguada y estable. Larealimentación proporcional, incrementa la rapidez con que se lleva al error a cero y enalgunos casos es esencial para la eliminación del error. Aunque hay que insistir en queestamos limitados por la estabilidad del sistema [4] [5].


127


Figura 62. Resultados de velocidad del motor controlador con un PID

Esta última respuesta obtenida es bastante estable y amortiguada, consiguiendo unerror reducido, que lleva a la posición final del Robot a [x = 64,53, y = 45,25]. Realizandomás simulaciones y ajustando los parámetros del PID, como por ejemplo aumentar losvalores proporcional e integral se puede obtener un error menor y más próximo a 0, pero acosta de la estabilidad del sistema. Si quisiéramos corregir esta estabilidad aumentando elvalor derivativo volveríamos a aumentar el error. Es decir, que para mejorar la respuesta enun sentido, la empeoramos en otro. En la gráfica de la Figura 63, se muestra como haaumentado considerablemente el sobreimpulso debido al elevado valor proporcional. Esdifícil conseguir un error 0 sin afectar a otros factores de la respuesta.


128

Figura 63. Resultados de velocidad del motor controlador con un PID

Una vez estudiadas las diversas respuestas, y conocido como afecta cada parámetrodel PID en el control del Robot utilizando el simulador, se realizan diferentesmodificaciones en el modelado del sistema de control para estudiar el diseño realizado.

¿Qué funciones realiza la realimentación? Para contestar esta pregunta se realizandos nuevas simulaciones, pero ahora utilizando el movimiento grasping y examinando larespuesta con realimentación y sin realimentación cuando se transporta el objeto (par decarga aumenta).

Es necesario compensar la carga del objeto (par de perturbación) aumentando lafuerza del PID, para KP↑ y KI↑.

Con realimentación para el control de los motores (Figuras 64, 65).

Grain time = 5 ms.Kp = 50e-4Ki = 100e-3Kd = 0.1e-5Total grain time = 2,3 s.CPU time = 5,5 s.

Sobreimpulso elevado, demasiadavelocidad en el arranque


129

Figura 64. Respuesta con realimentación (velocidad, intensidad y par de cada motor)

Figura 65. Respuesta del Robot con realimentación (velocidad lineal y angular)


130

Observando las dos respuestas, con realimentación (Figuras 64, 65) y sinrealimentación (Figuras 66, 67), vemos como afecta el par de carga por el transporte delobjeto grasping (carga del objeto supone en este caso un par de carga, perturbación de 10-5

mNm). Incluso con un par de carga pequeño, cuando el objeto es cargado por el Robot, sinrealimentación, la perturbación exterior afecta a la respuesta, cosa que no sucede con larealimentación, ya que [4] [5]:

- La realimentación reduce la sensibilidad a las variaciones.- La realimentación reduce la inestabilidad y el error provocado por las

perturbaciones exteriores.

Sin realimentación para el control de los motores (Figuras 66, 67).

Grain time = 5 ms.Kp = 50e-4Ki = 100e-3Kd = 0.1e-5Total grain time = 2,3 s.CPU time = 8,67 s.

Figura 66. Respuesta del Robot sin realimentación (velocidad lineal y angular)


131

Figura 67. Respuesta sin realimentación (velocidad, intensidad y par de cada motor)

Sin realimentación, tal y como se aprecia en las gráficas, el sistema tiene valores depico mayores y más inestabilidad, además, de verse afectado en mayor grado el par y elconsumo de intensidad por la perturbación de carga (objeto grasping). En cambio, graciasa la realimentación el sistema es más estable y amortiguado, reduciendo la sensibilidad auna perturbación exterior (aumenta el control del PID) [4] [5].

Ahora que ya conocemos la utilidad de la realimentación, se estudia el método desimulación del programa, variando el tiempo de duración de los intervalos a partir delgrain time del programa. En el siguiente ejemplo, se aumenta la duración del intervalo demuestreo (grain time ↑).



132

Figura 68. Respuesta de sistema de contol variando el grain time (velocidad lineal y angular)

Figura 69. Respuesta de sistema de contol variando el grain time (velocidad lineal y angular)


133

Cuando se modifica el intervalo de simulación, aumentando el tiempo de muestreo(grain time ↑), el control y la respuesta del sistema se ve afectado. Al producirse muestreosmás separados temporalmente, el Robot, para una misma consigna de velocidad-posiciónresponde con una mayor inestabilidad, que para un tiempo de muestreo más elevadoconlleva más espacio recorrido e imprecisión en los movimientos del Robot. Además,aumentando el tiempo de muestreo se provoca mayor diferencia entre los resultados decada intervalo de simulación.

Un sistema con un tiempo de respuesta τSIS, debe ser muestreado y controlado enintervalos de como mínimo τMUES < 2τSIS, para evitar el efecto de aliasing en lareconstrucción y control de la respuesta. Además, un mayor intervalo de muestreo,comporta más problemas de control del sistema, porque cuando queramos corregir el errorde posición-velocidad, este error puede ser demasiado grande para minimizarlo [11].

El grain time es principalmente la variable que controla la precisión en lasimulación, es decir; la precisión en la evolución de los movimientos, la precisión en elcontrol de las velocidades, la precisión en el control de las intensidades, etc.

2.8.1.5 Conclusiones

a) El método de muestreo-simulación utilizado en el programa, recalcula laconsigna de posición-velocidad cada grain time segundos. Con intervalos de muestreospequeños, se consigue un control satisfactorio, obteniendo respuestas bastanterepresentativas del funcionamiento del sistema. Pero si el grain time es demasiado grande,las respuestas obtenidas no serán fiables, debido a la falta de muestreos.

b) Toda simulación esta limitada a las aproximaciones y modelados realizados, esdecir, cuando más completo y real sea el modelado del sistema, las respuestas obtenidasserán más coherentes con la realidad de un montaje práctico, aunque siempre se estarálimitado en la posibilidad de tener en cuenta todos los factores que afectan en la práctica,como por ejemplo; tracción de las ruedas, roces, vibraciones, fricción, desajustes en elmontaje, masa del Robot, forma del Robot, etc.

c) Los factores nombrados anteriormente, afectan a los resultados de lasvelocidades, intensidades y par en un montaje práctico, por eso, y al no tenerlos en cuentaen la simulación, las respuestas gráficas obtenidas no son del todo aplicables a la realidad.Pero si pueden ayudar a conocer una tendencia en la respuesta del sistema. Por ejemplo:

- Observar como afecta un par de carga al consumo (intensidad) del motor.- Estudiar el pico de intensidad en el arranque y en la evolución de ésta, nos

ayuda a ajustar el control para tener el menor consumo posible.- Un estudio de todas las respuestas; velocidad, intensidad y par, ayuda a

deducir sí un motor es apropiado para un tipo de aplicación en concreto.- Estudiar como afecta el control realizado en la trayectoria del Robot.- Estudiar el tipo de controlador más apropiado (PI, PID, FUZZY u otros),

y el ajuste de sus parámetros para el control del sistema. Además deobservar como afecta cada parámetro por separado a la respuesta delsistema (KP, TI y TD).


134

- Realizar un control muy amortiguado para evitar el patinaje de las ruedasdel Robot en la práctica.

- Etc.

d) Destacar que los resultados obtenidos siempre estarán limitados al método demuestreo-simulación realizado en el programa, ya que nunca sabemos como responde elsistema entre muestra y muestra.

e) Conocer y entender el método de muestreo-simulación realizado en el programaayuda a saber estudiar los resultados obtenidos, evitando estudios o deducciones erróneas.

f) El programa sirve para estudiar de forma didáctica los sistemas de control y elmodelado de éstos en un entorno atractivo y divertido, mostrando un funcionamientoaproximado a la realidad.

2.8.2 Programación de la Máquina de Estados

Los ejemplos que se exponen a continuación, consisten en la programación de lamáquina de estados para el control de la dirección del Robot, utilizando un modelo delsistema de control (fichero simulink) muy parecido al del ejemplo anterior. En amboscasos, la programación de la máquina de estados depende del tipo de aplicación para la quese requiera, teniendo muy en cuenta, las diferencias existentes entre los resultados de estamáquina de estados ideal y los resultados de un montaje práctico, donde por ejemplo, lossensores responden de forma muy distinta:

a) Distancia y ángulo de detección siempre es el mismo sin afectarle por ejemplo,la perdida o disminución de tensión en la batería del Robot.

b) Detección de obstáculos ideal, sin afectar las condiciones ambientales, color nila forma del obstáculo (sensores de proximidad de infrarrojos).

c) Distancia y ángulo de detección exacto y preciso en todas las distancias.

Considerando los anteriores factores, el programa puede ser utilizado por el usuariopara la programación, el desarrollo y el estudio de una máquina de estados que controle lasacciones y dirija el “sentido” de los movimientos del Robot móvil, es decir, el control paraevitar el choque contra los obstáculos detectados, ya sean obstáculos fijos o móviles (otrosrobots).


135

2.8.2.1 Ejemplo del Laberinto

El primer ejemplo de programación de la máquina de estados consiste en el controlde un Robot que sea capaz de salir de un laberinto, basándose sólo en la informaciónaportada por cinco sensores de proximidad distribuidos en la estructura del Robot de lasiguiente forma (ver Figura 70 y Tabla 10).

Sensor Distancia detección Ángulo detección Ángulo rotación

S1 5 cm. 40º 0ºS2 4 cm. 40º -35ºS3 7 cm. 40º 0ºS4 7 cm. 40º 0ºS5 4 cm. 40º 35º

El radio del Robot es de 3 cm.

Tabla 10. Datos característicos de los sensoresFigura 70. Distribución de los sensores

El número de sensores y las características de estos, posición, distancia dedetección, ángulo de detección y ángulo de rotación, afectan al correcto funcionamiento dela máquina de estados, es decir, no sólo se depende de la acción programada por cadaestado sino que según las características de los sensores, se obtienen mejores o peoresresultados con una misma máquina de estados (para una misma máquina de estados,variando las características de los sensores, se adapta el control del Robot para diferentesaplicaciones).

Una vez conocidos el número de sensores y las características de éstos, se realiza elanálisis de todos los estados posibles, asignando una “acción” por cada estado. El númerode estados posibles es de 2nº sensores = 25 = 32.

El siguiente paso consiste en la programación de la máquina de estados finitosposibles y las acciones por cada estado. La respuesta de la máquina de estados sesimplifica por cada movimiento gracias a las siguientes fórmulas:

Movimiento en línea recta (ver Figura 71 )

( )[ ] )45(SSSSSSSSSSRECTO 2345343251 ++=

% Ir en línea rectaRECTO = [(selfa(1)+20*cos(selfa(3))) (selfa(2)+20*sin(selfa(3))) 0];

12

34

5


136

x, y

xdia, ydiaxdia = selfdia(1)ydia = selfdia(2)x = selfa(1)y = selfa(2)theta = selfa(3)

theta

20

Figura 71. Ejemplo de programación del movimiento en línea recta

% Movimiento en línea recta al ángulo del Robot

if ESTADO_SENSORES(1) == 0

if (ESTADO_SENSORES(5) == 0 & ESTADO_SENSORES(2) == 0) & (ESTADO_SENSORES(3)== 0 | (ESTADO_SENSORES(4) == 1 & ESTADO_SENSORES(3) == 1))

selfdia = RECTO; % Movimiento en línea recta

elseif (ESTADO_SENSORES(5) == 1 & ESTADO_SENSORES(4) == 0 & ESTADO_SENSORES(3)== 0 & ESTADO_SENSORES(2) == 1)

selfdia = RECTO; % Movimiento en línea recta

end;end;

Giro a la izquierda de 90º (ver Figura 72)

)46(SSSSS90GIIZ 12345=

function [selfdia] =siguepar(selfa,selfdia,ESTADO_SENSORES,ESTADO_ANTERIOR_SENSORES,a)... ... ...% Giro a la izquierda del Robot 90ºGI_IZ_90 = [(selfa(1)+10*cos(selfa(3)+(pi/2))) (selfa(2)+10*sin(selfa(3)+(pi/2)))0];

x, y

xdia, ydia

xdia = selfdia(1)ydia = selfdia(2)x = selfa(1)y = selfa(2)theta = selfa(3)

10

90ºtheta

Figura 72. Ejemplo de programación del movimiento de giro a la izquierda de 90º

% Giro a la izquierda de 90º

if (ESTADO_SENSORES(5) == 0 & ESTADO_SENSORES(4) == 0 & ESTADO_SENSORES(3) == 0 &ESTADO_SENSORES(2) == 1 & ESTADO_SENSORES(1) == 0) selfdia = GI_IZ_90; % Giro a la izquierda de 90ºend;


137

Giro a la derecha de 90º (ver Figura 73)

)47(SSSSS90GIDE 12345=

function [selfdia] =siguepar(selfa,selfdia,ESTADO_SENSORES,ESTADO_ANTERIOR_SENSORES,a)... ... ...

% Giro a la derecha del Robot 90ºGI_DE_90 = [(selfa(1)+10*cos(selfa(3)-(pi/2))) (selfa(2)+10*sin(selfa(3)-(pi/2))) 0];

x, y

xdia, ydia


10

-90º

theta

Figura 73. Ejemplo de programación del movimiento de giro a la derecha de 90º

% Giro a la derecha de 90º

if (ESTADO_SENSORES(5) == 1 & ESTADO_SENSORES(4) == 0 & ESTADO_SENSORES(3) == 0 &ESTADO_SENSORES(2) == 0 & ESTADO_SENSORES(1) == 0) selfdia = GI_DE_90; % Giro a la derecha de 90ºend;

Giro a la izquierda de 135º (ver Figura 74)

( ) ( ) ( ) )48(SSSSSSSSSSSSSSSSSS135GIIZ 123451324512351345 ++++++=


% Giro a la izquierda del Robot 135º

GI_IZ_135 = [(selfa(1)+10*cos(selfa(3)+( 3*pi/4)))(selfa(2)+10*sin(selfa(3)+(3*pi/4))) 0];

x, yxdia, ydia


10

135ºtheta

Figura 74. Ejemplo de programación del movimiento de giro a la izquierda de 135º


138

% Giro a la izquierda de 135º

if (ESTADO_SENSORES(5) == 0 & ESTADO_SENSORES(4) == 0) & (ESTADO_SENSORES(3) == 1| ESTADO_SENSORES(1) == 1) selfdia = GI_IZ_135; % Giro a la izquierda de 135º

elseif (ESTADO_SENSORES(5) == 1 & ESTADO_SENSORES(3) == 1) & (ESTADO_SENSORES(2)== 1 | ESTADO_SENSORES(1) == 1) selfdia = GI_ IZ _135; % Giro a la izquierda de 135º

elseif (ESTADO_SENSORES(5) == 0 & ESTADO_SENSORES(4) == 1) & (ESTADO_SENSORES(2)== 1 | (ESTADO_SENSORES(3) == 1 & ESTADO_SENSORES(1) == 1)) selfdia = GI_ IZ _135; % Giro a la izquierda de 135º

elseif (ESTADO_SENSORES(5) == 1 & ESTADO_SENSORES(4) == 0 & ESTADO_SENSORES(3) ==0 & ESTADO_SENSORES(2) == 1 & ESTADO_SENSORES(1) == 1) selfdia = GI_ IZ _135; % Giro a la izquierda de 135ºend;

Giro a la derecha de 135º (ver Figura 75)

( )[ ] )49(SSSSSSSSSSSS135GIDE 123414231235 +++=

% Giro a la derecha del Robot 135ºGI_DE_135 = [(selfa(1)+10*cos(selfa(3)-( 3*pi/4))) (selfa(2)+10*sin(selfa(3)-(3*pi/4))) 0];

x, y

xdia, ydia


10

-135º

theta

Figura 75. Ejemplo de programación del movimiento de giro a la derecha de 135º


% Giro a la derecha de 135ºif ESTADO_SENSORES(5) == 1 if (ESTADO_SENSORES(3) == 1 & ESTADO_SENSORES(2) == 0 & ESTADO_SENSORES(1) ==1) selfdia = GI_DE_135; % Giro a la derecha de 135º

elseif (ESTADO_SENSORES(3) == 0 & ESTADO_SENSORES(2) == 1) &(ESTADO_SENSORES(4) == 1 | ESTADO_SENSORES(1) == 1) selfdia = GI_DE_135; % Giro a la derecha de 135ºend;end;

if (ESTADO_SENSORES(4) == 1 & ESTADO_SENSORES(3) == 0 & ESTADO_SENSORES(2) == 0 &ESTADO_SENSORES(1) == 1) selfdia = GI_DE_135; % Giro a la derecha de 135ºend;


139

Estas fórmulas simplificadas, designan cada movimiento (“acción”) a cada posibleestado de los sensores. Recordar que para indicar la nueva función de control de lamáquina de estados se debe introducir el nombre de ésta en:

NOTA: En el Anexo de programación se muestra la función siguepar.m con laestructura utilizada para realizar el control de los estados de los sensores.

Inicializando el sistema (obstáculos, datos, etc.) y colocado en su posición inicial elRobot (coordenadas x = 5 y = 5 ángulo rotación = 0º) se puede dar comienzo a lasimulación. El resultado de la simulación se presenta en la Figura 76 (ventana desimulación). Se aprovechan las características del movimiento forward del pro-grama, ya que interesa que el Robot por defecto, vaya en línea recta en el interior dellaberinto. Sólo se corrige la trayectoria del Robot en el caso de encontrar un obstáculo(paredes del laberinto).

Figura 76. Ventana de simulación con la trayectoria del Robot en el interior del laberinto

En la primera simulación realizada se observa que el Robot tiene unos movimientosde giro muy bruscos, y se decide reducir el valor de la variable KPA = 0’5, disminuyendoasí la consigna de velocidad angular, obteniendo movimientos de giro más suaves.

En la Figura 76 se observa que el Robot es capaz de recorrer todo el laberinto ysalir de él sin mayor problema, aunque cuando debe realizar giros completos (cambio desentido) esta limitado por la poca amplitud de los pasillos (choque contra las paredes, parde perturbación↑).


140

Conclusiones

Este ejemplo de simulación, sirve al usuario para el estudio de la programación,reducción y funcionamiento de una máquina de estados que controla los movimientos y larespuesta de un Robot móvil, pero teniendo muy en cuenta las siguientes características yfactores que afectan a esta máquina de estados y a la respuesta del sistema de control.

a) El modelado y el sistema de control utilizado afecta a la respuesta del Robot,aunque parezca que para la programación de la máquina de estados no afectedirectamente, sí afecta a los movimientos del Robot y por tanto al control (serecomienda una velocidad reducida).

b) Si los movimientos del Robot son lentos y suaves, el control y elfuncionamiento de la máquina de estados obtendrá mejores resultados.

c) Los sensores de la simulación no son del todo perfectos, ya que los obstáculoscuadrados presentan problemas para una total detección en todos los ángulosposibles.

d) Un sensor de proximidad en un montaje práctico se ve afectado por múltiplesfactores, como: color del obstáculo (en sensores infrarrojos), condicionesambientales, posición de los obstáculos, etc. Estos factores no se tienen encuenta en la simulación.

e) Los movimientos en la simulación no son del todo exactos a los movimientosque se tendrían en un montaje práctico (patinaje de las ruedas, tracción, roces,choques con los obstáculos, etc.)

f) En un montaje práctico los movimientos del Robot serán más continuos, y nocomo en la simulación que parecen discontinuos debido al método de muestreodel programa y al control realizado.

Todos los factores anteriores, ayudan a conocer la simulación y la implementaciónde la máquina de estados. Pero a pesar de estos factores, es de gran utilidad didáctica laprogramación y el estudio de las máquinas de estados que se diseñan con este programa desimulación, ya no sólo por la aplicación práctica que se pueda realizar posteriormente (quenunca será aplicable al 100%), sino por el aprendizaje del funcionamiento y simplificaciónde una máquina de estados.


141

2.8.2.2 Ejemplo de las Columnas

El segundo ejemplo de simulación para la programación de una máquina de estadosse basa en el control de tres sensores, pero con una característica especial, la utilización delestado anterior de estos sensores. La máquina de estados programada debe evitar el choquecon los obstáculos, cualquiera que sea el entorno de esos obstáculos. Recordando laslimitaciones y características anteriormente comentadas en el caso anterior, se programa lamáquina de estados de tres sensores colocados tal y como se muestra en la Figura 77 yTabla 11.

Sensor Distancia detección Ángulo detección Ángulo rotación

S1 10 cm. 45º 0ºS2 6 cm. 45º -55ºS3 6 cm. 45º 55º

El radio del Robot es de 3,5 cm.Tabla 11. Datos característicos de los sensores

Figura 77. Distribución de los sensores

El tener un número mayor de sensores no mejorará la respuesta y el control sobre elRobot, sólo puede llevar a complicar de forma excesiva la máquina de estados,consiguiendo con un número menor de sensores idénticos o incluso mejores resultados.Como ya se ha comentado, para este nuevo ejemplo se utilizan tres sensores que sedistribuyen simétricamente por el contorno del Robot para obtener una mejor respuesta delsistema de control. Las características de estos sensores afectan a la programación y a larespuesta del sistema de control, ya que al variar la distancia y el ángulo de detección, elcontrol responde antes y en mayores ocasiones. No se debe reducir el número de sensoresaumentando la distancia y ángulo de detección, porque esto sólo llevaría a una máquina deestados inestable y sin sentido en el funcionamiento, por la poca exactitud de lainformación aportada por los sensores.

La programación de la máquina de estados y los movimientos en cada estado seresume con las siguientes fórmulas:

Movimiento en línea recta

[ ][ ] [ ] )50(SSSSSSSSSRECTO 123^1

^2

^3123 +++=

Giro a la izquierda de 45º

[ ] [ ][ ] )51(SSSSSSSSSS45GIIZ ^3

^1

^2123

^3123 ++=

1

23


142

Giro a la derecha de 45º

[ ] [ ][ ] )52(SSSSSSSSSS45GIDE ^2

^1

^3123

^2123 ++=

Giro a la izquierda de 90º

[ ] [ ] [ ] [ ] [ ] )53(SSSSSSSSSSSSSSSSSSS90GIIZ ^2

^3123

^3123

^2

^1

^3123

^2123 ++++=

Giro a la derecha de 90º

[ ] [ ] [ ] [ ] [ ][ ] )54(SSSSSSSSSSSSSSSSSSS90GIDE ^3

^2123

^2123

^3

^1

^2123

^2123 +++++=

Sn para n = 1,2,3: estado actual del sensor (intervalo i de simulación)

Sn^ para n = 1,2,3: estado anterior del sensor (intervalo i-1 de simulación)

function [selfdia] =maes2(selfa,selfdia,ESTADO_SENSORES,ESTADO_ANTERIOR_SENSORES,a)... ... ...% Ir en línea rectaRECTO = [(selfa(1)+40*cos(selfa(3)))(selfa(2)+40*sin(selfa(3))) 0];

% Giro a la derecha del Robot 45ºGI_DE_45 = [(selfa(1)+40*cos(selfa(3)-(pi/4)))(selfa(2)+40*sin(selfa(3)-(pi/4))) 0];

% Giro a la derecha del Robot 90ºGI_DE_90 = [(selfa(1)+40*cos(selfa(3)-(pi/2)))(selfa(2)+40*sin(selfa(3)-(pi/2))) 0];

% Giro a la izquierda del Robot 45ºGI_IZ_45 = [(selfa(1)+40*cos(selfa(3)+(pi/4)))(selfa(2)+40*sin(selfa(3)+(pi/4))) 0];

% Giro a la izquierda del Robot 90ºGI_IZ_90 = [(selfa(1)+40*cos(selfa(3)+(pi/2)))(selfa(2)+40*sin(selfa(3)+(pi/2))) 0];

% ------------------------------------------------------% Inicio programación de la Máquina de Estados del Robot% ------------------------------------------------------

if ESTADO_SENSORES(3) == 0 & ESTADO_SENSORES(2) == 0 & ESTADO_SENSORES(1)== 0 if ESTADO_ANTERIOR_SENSORES(3) == 1 | ESTADO_ANTERIOR_SENSORES(2) == 1| ESTADO_ANTERIOR_SENSORES(2) == 1 selfdia = RECTO; % Movimiento en línea recta al ángulo del Robot end;end;

if ESTADO_SENSORES(3) == 0 & ESTADO_SENSORES(2) == 0 & ESTADO_SENSORES(1)== 1 aleatorio = rand; % Número aleatorio


143

if ESTADO_ANTERIOR_SENSORES(3) == 0 & ESTADO_ANTERIOR_SENSORES(2) == 0 &ESTADO_ANTERIOR_SENSORES(1) == 1 selfdia = MOVI_ANTERIOR; elseif ESTADO_ANTERIOR_SENSORES(2) == 0 & aleatorio > 0.5 selfdia = GI_DE_90; % Giro a la derecha +90º MOVI_ANTERIOR = GI_DE_90; elseif aleatorio <= 0.5 selfdia = GI_IZ_90; % Giro a la izquierda -90º MOVI_ANTERIOR = GI_IZ_90; end;

end;

if ESTADO_SENSORES(3) == 0 & ESTADO_SENSORES(2) == 1 & ESTADO_SENSORES(1)== 0 if ESTADO_ANTERIOR_SENSORES(3) == 0 & (ESTADO_ANTERIOR_SENSORES(1) ==1 | ESTADO_ANTERIOR_SENSORES(2) == 1) selfdia = GI_IZ_90; % Giro a la izquierda -90º end; if (ESTADO_ANTERIOR_SENSORES(1) == 0 & ESTADO_ANTERIOR_SENSORES(2) ==0) | ESTADO_ANTERIOR_SENSORES(3) == 1 selfdia = GI_IZ_45; % Giro a la izquierda -45º end;end;

if ESTADO_SENSORES(3) == 0 & ESTADO_SENSORES(2) == 1 & ESTADO_SENSORES(1)== 1 if ESTADO_ANTERIOR_SENSORES(3) == 0 selfdia = GI_IZ_90; % Giro a la izquierda +45º else selfdia = GI_IZ_45; % Giro a la izquierda -45º end;end;

if ESTADO_SENSORES(3) == 1 & ESTADO_SENSORES(2) == 0 & ESTADO_SENSORES(1)== 0 if ESTADO_ANTERIOR_SENSORES(2) == 0 & (ESTADO_ANTERIOR_SENSORES(1) ==1 | ESTADO_ANTERIOR_SENSORES(3) == 1) selfdia = GI_DE_90; % Giro a la derecha +90º end; if (ESTADO_ANTERIOR_SENSORES(3) == 0 & ESTADO_ANTERIOR_SENSORES(1) ==0) | ESTADO_ANTERIOR_SENSORES(2) == 1 selfdia = GI_DE_45; % Giro a la derecha +45º end;end;

if ESTADO_SENSORES(3) == 1 & ESTADO_SENSORES(2) == 0 & ESTADO_SENSORES(1)== 1 if ESTADO_ANTERIOR_SENSORES(2) == 0 selfdia = GI_DE_90; % Giro a la derecha +90º else selfdia = GI_DE_45; % Giro a la derecha +45º end;end;

if ESTADO_SENSORES(3) == 1 & ESTADO_SENSORES(2) == 1 & ESTADO_SENSORES(1)== 0 selfdia = RECTO; % Movimiento en línea recta al ángulo del Robotend;


144

if ESTADO_SENSORES(3) == 1 & ESTADO_SENSORES(2) == 1 & ESTADO_SENSORES(1)== 1 if ESTADO_ANTERIOR_SENSORES(2) == 0 | ESTADO_ANTERIOR_SENSORES(3) == 1 selfdia = GI_DE_90; % Giro a la derecha +90º else selfdia = GI_IZ_90; % Giro a la izquierda -90º end;end;

% ---------------------------------------------------% Fin programación de la Máquina de Estados del Robot% ---------------------------------------------------

El código de programa que se muestra, define la máquina de estados representadaanteriormente con las fórmulas. Se puede apreciar como en algunos casos la máquina deestados se vale de la probabilidad (función de aplicación de MATLAB rand [1]) paraasignar un giro a la derecha o a la izquierda. Este método sólo se aplica en el caso deencontrar un obstáculo justo enfrente del Robot teniendo ambos lados libres, asignandoaleatoriamente el movimiento de giro en ese estado (derecha o izquierda), evitando unaprogramación repetitiva que introduzca el Robot en un bucle de movimientos sin salida porculpa del entorno de obstáculos. Este método se ha revelado, como un método aceptable,aunque no ideal para realizar el trayecto más corto hacia el objetivo.

NOTA: Recordar introducir el nombre de la nueva función de control de lamáquina de estados a la hora de realizar la simulación.

El entorno de obstáculos escogido, se asemeja a la disposición de las columnas enuna nave industrial. Combinado con el movimiento grasping , donde el Robot sedesplaza a coger objeto y transportarlo a las coordenadas de destino x = 115 y = 75, seobtiene la respuesta de la Figura 78.

Figura 78. Ventana de simulación con la trayectoria del Robot entre las columnas

xiniROBOT = 65yiniROBOT = 45

x GRASPING2 = 15yGRASPING2 = 75





145

El objeto grasping, cada vez que es transportado a las coordenadas de destino seposiciona en diferentes coordenadas de forma manual para observar diferentes recorridosdel Robot. La programación de la máquina de estados realizada parece idónea en estaaplicación ya que el principal objetivo que es el de evitar el choque con los obstáculos, seconsigue (Figura 78), aunque podría interesar realizar recorridos y trayectorias más cortas(no siempre es posible).

Conclusiones

Las conclusiones de este segundo ejemplo de programación de la máquina deestados son muy parecidas a las conclusiones del primer ejemplo. Eso sí, en este ejemplo elmétodo de programación es bastante diferente, al utilizar el estado anterior de los sensoresademás de un sistema aleatorio para la asignación de algunos movimientos de giro.

En los siguientes puntos se recogen diferentes factores y conclusiones sobre elfuncionamiento de la máquina de estados programada.

a) El método aleatorio de asignación de algunas consignas de giro, no siemprepuede ser el más idóneo para realizar el recorrido más corto. Aunque sí es muyútil para evitar que el Robot entre en un bucle (callejón) sin salida.

b) La utilización del estado anterior de los sensores no es del todo necesario paraeste entorno de obstáculos (columnas), ya que con el estado actual de lossensores se obtiene la suficiente información para la programación de lamáquina de estados. Entre el intervalo de simulación “i” y el intervalo “i-1” ladistancia recorrida por el Robot es mínima y por eso la información del estadoanterior de los sensores no es muy útil.

c) El entorno de obstáculos utilizado facilita el control de los movimientos delRobot y el funcionamiento de la máquina de estados por la gran separaciónentre los obstáculos.

d) La precisión y características de los sensores afecta directamente a la trayectoriadel Robot.

e) Con un número de sensores reducido, pero bien posicionados y ajustados a laaplicación se obtienen buenos resultados, comparables a los de una máquina deestados con mayor número de sensores.

f) Los sensores de la simulación no son del todo perfectos, ya que los obstáculoscuadrados presentan problemas para una total detección en todos los ángulosposibles.

g) Este segundo ejemplo de simulación presenta las mismas diferencias ylimitaciones que el anterior ejemplo, respecto a un posible montaje práctico.


146

Como conclusión final, cabe recordar la utilidad de la programación de máquinasde estados y de sistemas de control para el estudio y aprendizaje de una forma vistosa ydivertida dirigido a los estudiantes, tal y como ya se ha realizado en la asignatura optativade Robótica Móvil (5º Curso / 2º Cuatrimestre) de la carrera de Ingeniería en Automática yElectrónica Industrial de la Universidad Rovira i Virgili.

2.9 Conclusiones

El simulador realizado de robots móviles en MATLAB/Simulink cumple con losobjetivos marcados en el inicio. Ahora bien, el cumplimiento de estos objetivos no eximeal programa de las limitaciones en el funcionamiento y en la respuesta, derivadas de todasimulación y estudio teórico-simulado.

Los resultados derivados de las limitaciones o características del programa nodeben confundirse con las aproximaciones o modelos que realiza el usuario en el ficherosimulink de control. Los resultados obtenidos serán más o menos exactos y coherentes conla realidad según la complejidad y los factores que tenga en cuenta el usuario en el diseñodel modelado del sistema de control (fichero simulink).

Como factores propios del programa que limitan y afectan a los resultados delsimulador se destacan:

a) El método de ejecución del simulador, donde se realizan pequeñas simulacionesen intervalos de duración de dt segundos (muestreo) convierte la respuesta delsistema una respuesta en tiempo discreto. Siendo recomendable realizarmodelos discretos en el sistema motriz del robot (fichero simulink).

b) Entre muestra y muestra, no se conoce la evolución del sistema, aunque cuandose obtienen los resultados gráficos, se aproxime la respuesta entre los intervalosde las muestras, ya que se unen los resultados de cada muestreo directamente(realizar interpolación?). Además de recordar que toda simulación tiene unlímite en su precisión ya que no deja de ser un estudio teórico (precisión de loscálculos).

c) En la ejecución del programa, no se realizan simulaciones o modeloscomplejos, realizando sencillas aproximaciones en los casos de por ejemplo:

- Choque entre Robots.- Choque de los Robots contra los obstáculos.- Programación de la máquina de estados.- Sistema de detección de los sensores para los obstáculos, robots.

Aunque en el caso de los choques contra los obstáculos o los robots, si sesimula el aumento del par de carga en los motores (perturbación ↑).

d) El desplazamiento del robot se realiza calculando a cada intervalo las nuevascoordenadas de posición del robot en función de:

- velocidad lineal y angular (obtenidas del modelo realizado por el usuarioen el fichero simulink).

- tiempo de muestreo (grain time).


147

e) La correcta respuesta del sistema depende del tiempo entre muestreos (graintime). La precisión de los movimientos del robot se ve afectado directamentepor éste dato. Si se aumenta la separación temporal entre las muestras, elespacio recorrido por el robot será mayor.

Recordar que el grain time debe ser menor que el tiempo de respuesta delsistema, ya que si lo aumentamos, la respuesta del sistema es más difícil decontrolar, porque al ser menor el tiempo de respuesta del sistema (motor de CC)responde antes y más rápido que el sistema de control.

f) Los giros, choques contra los obstáculos, etc. del robot corresponden a los deuna simulación, y por tanto no se debe trasladar esta respuesta a la realidad deforma exacta, por muy bien que haya sido modelado el sistema de choque o degiro.

g) Sólo se pueden simular robots móviles con una estructura determinada, es decir,no se dispone de las herramientas ni los cálculos necesarios para:

- Cambiar la posición y el número de las ruedas motrices.- Cambiar la posición y el número de las ruedas “locas”.- Cambiar la forma, estructura del robot, etc. para la simulación del peso y

la aerodinámica del robot (carretilla, silla de ruedas, bicicleta) [8].

Otros factores a tener en cuenta que no se derivan de la programación sino queinvolucra más al modelo del sistema realizado por el usuario serían:

a) Un modelo más complejo, donde se tenga en cuenta el menor número deaproximaciones (por ejemplo, para modelar los motores), realizando modelados(fichero simulink) lo más parecidos a un caso práctico, conlleva una respuestamás coherente con la realidad de un montaje. Factores que se podrían tener encuenta en el modelado y que afectan a los resultados son :

- Tracción de las ruedas (roces).- Peso del robot.- Velocidad-posición de los motores utilizando encoders.- Tipo de actuador (PWM).- Etc.

b) La respuesta del sistema y los resultados obtenidos deben ser estudiados en suconjunto y de forma global, ya que en caso contrario, estudiando por ejemplo elresultado de un muestreo en concreto, la conclusión será incorrecta por la faltade información.

Es importante saber estudiar los resultados obtenidos teniendo en cuenta losfactores explicados anteriormente para que las conclusiones y soluciones adoptadas seancoherentes con el funcionamiento del simulador.

Todos los factores anteriores, resaltan las limitaciones e imprecisiones delprograma de simulación realizado, pero al mismo tiempo, todo aquello que se puede mejoren el programa, y que en posteriores Proyectos podría corregirse o mejorarse.


148

Otras herramientas del programa, como es el diseño de sensores de proximidad y laprogramación de la máquina de estados, también tienen sus limitaciones y factores queinfluyen en la respuesta del simulador.

Como ya se ha comentado en los ejemplos de simulación, donde se ponía a pruebala programación de la máquina de estados y el diseño de sensores, existen diversos factoresa tener en cuenta en el estudio de los resultados y para una posible traslación a un casopráctico:

a) Los sensores de proximidad simulados no tienen en cuenta factores queinfluyen en su normal funcionamiento en una aplicación real:

- Los sensores de la simulación no se ven afectados por las condicionesambientales como pasaría en un caso práctico (luz, temperatura, radiaciónsolar, etc.)

- El rebote de la señal (infrarrojos, ultrasonidos) es de mejor o peor calidaddependiendo del color del obstáculo, su forma o posición.

- Pueden darse, en el caso práctico, interferencias en la señal queprovoquen falsas detecciones.

- Los sensores de la simulación funcionan gracias a aproximaciones en laforma del obstáculo, que no siempre son del todo exactas.

b) La zona de detección de los sensores de proximidad del simulador es muyparecido a un triángulo isósceles perfecto en la detección, cuando en la prácticalos sensores tienen otra respuesta en la forma de radiación de la zona dedetección (más ovalada). Además está zona depende de las características decada sensor, y no es igual para todos.

c) Los movimientos del robot y el control de éstos, cuando se programa lamáquina de estados, se ven afectados por diversos factores (anteriores párrafos),por eso la máquina de estados realizada y la respuesta obtenida en el control, nodebe ser estudiada buscando una gran precisión y resolución en losmovimientos, sino de forma global. Por ejemplo, el robot puede ser que gire a laderecha porque detecta un obstáculo, cuando en un montaje práctico no lodetectaría.

Una vez definidos los factores que afectan al funcionamiento del simulador, seredactan ahora las virtudes del programa realizado que se resumen en la consecución de losobjetivos que se plantearon en este Proyecto.

a) Simulador con un entorno gráfico de fácil uso.

b) Herramientas para el modelado de un Robot: tamaño, sensores de proximidad,modelado de los motores, sistema de control, etc.

c) Modelado del sistema motriz utilizando el dinámico y práctico entorno desimulink.


149

d) Diseño de sensores de proximidad, con una función independiente creada por elusuario para programar la máquina de estados que controle la trayectoria delrobot.

e) Creación de un entorno con herramientas para el diseño de obstáculos queponga a prueba la máquina de estados implementada por el usuario, osimplemente, comprobar el choque del robot contra este obstáculo.

f) Posibilidad de configurar los parámetros del programa para ajustarlos a cadaaplicación.

g) Herramientas para el trabajo con el robot: setmouse, trail, etc.

h) Movimientos programados en el programa para la simulación de aplicacionesprácticas: grasping, tracking, forward, other.

i) Herramientas para guardar la configuración del robot, el entorno de obstáculoso los resultados de la simulación (guardados en un fichero de texto).

j) Representación gráfica de los resultados para su posterior (intensidad, velocidady par de los motores).

k) El modelado en un fichero simulink del sistema motriz del robot permite,siguiendo un mínimo formato, una gran versatibilidad en el diseño realizado,sin afectar al funcionamiento interno del programa (el fichero simulink es unacaja negra para el resto del programa, de donde se obtiene la velocidad lineal yangular, intensidades, etc.).

La pretensión a la hora de realizar este Proyecto, era crear un simulador de fácil usoy de una utilidad, principalmente didáctica. Ni mucho menos se esperaba llegar a unsimulador de grandes y complejas aplicaciones, donde pudieran aplicarse sus resultados acasos reales y montajes prácticos de gran envergadura. Además gracias a este Proyecto seabre una puerta para posteriores estudios desarrollados en otros Proyectos que mejoren estesimulador de robots móviles.

Una prueba de la utilidad didáctica de este simulador en MATLAB de RobotsMóviles es su utilización con éxito en la asignatura optativa de Robótica Móvil (5ºCurso/2º Cuatrimestre) de la carrera de Ingeniería en Automática y Electrónica Industrialde la Universidad Rovira i Virgili, donde los alumnos (usuarios) pusieron en práctica laprogramación de la máquina de estados. Precisamente, en esta asignatura los alumnoscriticaron y realizaron apreciaciones de forma constructiva para corregir y mejorar lasherramientas y resultados del simulador. Además de probar nuevas aplicaciones (simuladormuy versátil) como por ejemplo, utilizar la posibilidad de comunicar dos ordenadores através de MATLAB, simulando en el primero un robot en un entorno de obstáculos,mientras en el segundo se reconstruye este entorno de obstáculos gracias a la informaciónaportada por los sensores de proximidad que se están simulando en el primer ordenador.


150

2.10 Presupuesto

El Presupuesto de ejecución asciende a OCHO MIL CUATROCIENTOSTREINTA Y TRES EUROS Y OCHEINTA Y TRES CÉNTIMOS (8433,83 €, 1.403.272Ptas.). Este precio incluye la instalación y puesta en marcha del programa, así como laasesoría técnica sobre el funcionamiento del mismo.

Tarragona, 15 de junio de 2001Firmado: Santos Barquero CáceresIngeniero Técnico Industrial en Electrónica Industrial


Memoria de Cálculo

3. Memoria de Cálculo

152


3.1 Distancia y Ángulo entre dos Puntos

Durante la simulación se realiza el cálculo de la distancia y el ángulo entre dospuntos con las siguientes operaciones:

[x , y ]

[x , y ]2 2

r

ϕ1 1

ϕ

1

2

Figura 79. Cálculo de la distancia y ángulo entre dos puntos

Distancia entre puntos:

( ) ( ) )55(yyxxr 221

221 −+−=

Ángulo entre puntos:( )( ) )56(

xxyyarctg

21

211 −

−=ϕ

r : distancia lineal en [cm] si (x1[cm] y1[cm]), (x2[cm] y2[cm]).ϕ1: ángulo en rad (función atan2 de MATLAB [1]).

Utilizando la función de aplicación atan2 de MATLAB [1], obtendremos el ángulode forma que 0º < |ϕ| < 180º. Entonces, para trabajar con un ángulo siempre positivo y enlos cuatro cuadrantes se ha de establecer la condición de:

if ϕ1 < 0º ϕ1 = 360 + ϕ1

end;


153

3.2 Cálculo de la Velocidad Lineal y Angular del Robot

Para el modelado del sistema de control del Robot, en el fichero simulink, secalcula la velocidad lineal y la angular utilizando las siguientes operaciones (Figura 80)[6] [10]:

x , y

theta

2R r

lm2V

Vlm1

lRVωaR

theta

inc(x)

inc(y)

1 1

00x , y

1

0

Figura 80. Cálculo de la velocidad lineal y angular

(60)R2

VV

(59)2

VVV

(58)r·V

(57)r·V

lm2lm1aR

lm2lm1lR

am2lm2

am1lm1

−=ω

+=

ω=

ω=

donde, Vlm : Velocidad lineal del motor [cm/s]ωam : Velocidad angular del motor [rad/s]VlR : Velocidad lineal del Robot [cm/s]ωaR : Velocidad angular del Robot [rad/s]2R : Distancia entre ruedas (diámetro del Robot) [cm]r : Radio de las ruedas del Robot [cm]


154

3.3 Cálculo del Espacio Lineal y Angular Recorrido por el Robot

Para realizar el cálculo de la distancia recorrida por el Robot en un intervalo desimulación se parte de la fórmula básica (Figura 80) [6] [10]:

espacio = velocidad · tiempo

Aplicando el cálculo al espacio angular:

θωR = ωaR · dt espacio angular recorrido por el Robot (61)theta1 = theta0 + θωR (62)

En el caso de la simulación:

elR = VlR · dt espacio lineal recorrido por el Robot (63)inc(x) = elR · cos(theta1) (64)inc(y) = elR · sin(theta1) (65)x1 = x0 + inc(x) (intervalo de tiempo dt) (66)y1 = y0 + inc(y) (intervalo de tiempo dt) (67)

donde, dt : Tiempo de muestreo [s]theta0 :Ángulo de rotación actual del Robot (intervalo i-1) [rad]theta1 :Ángulo de rotación en el siguiente intervalo i [rad]θωR : Espacio angular recorrido por el Robot en un intervalo de simulación [rad]ωaR : Velocidad angular del Robot en el intervalo i [rad/s]inc(x): Incremento lineal de la coordenada x [cm]inc(y): Incremento lineal de la coordenada y [cm]VlR : Velocidad lineal del Robot [cm/s]elR : Espacio lineal recorrido por el Robot en un intervalo de simulación [cm]



Presupuesto

4. Presupuesto

156

4. PresupuestoEl presupuesto que se adjunta en el proyecto, es de obligado cumplimiento, y no

siendo necesaria su ratificación por el titular del proyecto. Toda modificación debenotificarse al Ingeniero Técnico Industrial firmante, para su aprobación. Ahora bien, estepresupuesto es meramente informativo e indicativo del coste de realización del programa,y no puede ser aplicado en caso que se desee distribuir comercialmente el simulador sinprevia revisión y aprobación del Ingeniero Técnico Industrial firmante.

4.1 Precios Unitarios

Presentación de los precios unitarios, tanto de la mano de obra de las personasfísicas que intervienen en la realización del simulador, como el precio del materialutilizado.

Programa de simulación de un Robot móvilJustificación de precios unitariosCódigo Mano de obra Precio en € Precio en Ptas.1000 Ingeniero Técnico Industrial en

Electrónica29,63 €/h 4.931,00 Ptas./h

1001 Ingeniero Superior (revisión técnica) 36,74 €/h 6.114,00 Ptas./h1002 Filólogo en lengua castellana e inglesa

(revisión técnica y lingüística)22,62 €/h 3.764,00 Ptas./h

Programa de simulación de un Robot móvilJustificación de precios unitariosCódigo Material Precio en € Precio en Ptas.2000 Ordenador 0,61 €/h 102,00 Ptas./h2001 Consulta de libros y documentación 5,75 €/h 957,00 Ptas./h2002 Material diverso de oficina 1,43 €/h 238,00 Ptas./h

4. Presupuesto

157

4.2 Precios Descompuestos de las Subpartidas

Justificación de precios de las subpartidasNº. Código Uds. Descripción Rd Precio

1 101 Planteamiento de los objetivos e ideas básicas delprograma de simulación

1 93,91 €

Uds. Precio Parcial Importe

Mano deobra

1000 h Ingeniero Técnico Industrialen Electrónica

3,00 X 4.931,00 = 14.793,00 Ptas. 14.793,00 Ptas.

Subtotal… 14.793,00 Ptas. 14.793,00 Ptas.


Materiales 2001 h Consulta de libros ydocumentación

0,50 X 957,00 = 478,50 Ptas. 478,50 Ptas.

2002 h Material diverso de oficina 0,20 X 238,00 = 47,60 Ptas. 47,60 Ptas.

Subtotal… 526,10 Ptas. 526,10 Ptas.

Gastos auxiliares 0,00 % 0,00 Ptas.

Coste directo 15.319,10 Ptas.Gastos indirectos 2,00 % 306,38 Ptas.

Coste ejecución 93,91 € 15.625,48 Ptas.


1 102 Desarrollo de los objetivos y finalidades del programade simulación

1 75,92 €


Mano deobra


2,50 X 4.931,00 = 12.327,50 Ptas. 12.327,50 Ptas.



Materiales 2000 h Ordenador 0,10 X 102,00 = 10,20 Ptas. 10,20 Ptas.2002 h Material diverso de oficina 0,20 X 238,00 = 47,60 Ptas. 47,60 Ptas.





4. Presupuesto

158


1 103 Esquematización del programa de simulación 1 60,60 €


Mano deobra


2,00 X 4.931,00 = 9.862,00 Ptas. 9.862,00 Ptas.



Materiales 2000 h Ordenador 0,10 X 102,00 = 10,20 Ptas. 10,20 Ptas.2002 h Material diverso de oficina 0,10 X 238,00 = 23,80 Ptas. 23,80 Ptas.






1 104 Estudio del lenguaje de programación (MATLAB) 1 269,78 €


Mano de obra 1000 h Ingeniero Técnico Industrialen Electrónica

8,00 X 4.931,00 = 39.448,00 Ptas. 39.448,00 Ptas.



Materiales 2000 h Ordenador 6,00 X 102,00 = 612,00 Ptas. 612,00 Ptas.2001 h Consulta de libros y

documentación4,00 X 957,00 = 3.828,00 Ptas. 3.828,00 Ptas.






4. Presupuesto

159


1 201 Inicio de la programación 1 135,92 €


Mano de obra 1000 h Ingeniero TécnicoIndustrial en Electrónica

4,00 X 4.931,00 = 19.724,00 Ptas. 19.724,00 Ptas.











1 202 Desarrollo y diseño del programa de simulación 1 3890,39 €



124,00 X 4.931,00 = 611.444,00Ptas.

611.444,00 Ptas.

Subtotal… 611.444,00Ptas.

611.444,00 Ptas.


Materiales 2000 h Ordenador 124,00 X 102,00 = 12.648,00 Ptas. 12.648,00 Ptas.2001 h Consulta de libros y





Coste directo 634.614,00 Ptas.Gastos indirectos 2,00 % 12.692,28 Ptas.


4. Presupuesto

160


1 301 Finalización y depuración del programa desimulación

1 160,86 €



5,00 X 4.931,00 = 24.655,00 Ptas. 24.655,00 Ptas.




documentación1,00 X 957,00 = 957,00 Ptas. 957,00 Ptas.







1 302 Fase de pruebas y conclusiones del programa.Corrección de errores

1 365,29 €



8,00 X 4.931,00 = 39.448,00 Ptas. 39.448,00 Ptas.

1001 h Ingeniero Superior(revisión técnica)

3,00 X 6.114,00 = 18.342,00 Ptas. 18.342,00 Ptas.




documentación1,00 X 957,00 = 957,00 Ptas. 957,00 Ptas.






4. Presupuesto

161


1 303 Revisión-corrección técnica 1 374,81 €


Mano de obra 1001 h Ingeniero Superior(revisión técnica)

10,00 X 6.114,00 = 61.140,00 Ptas. 61.140,00 Ptas.






1 304 Revisión-corrección lingüística 1 184,60 €


Mano de obra 1003 h Filólogo en lenguacastellana e inglesa(revisión técnica ylingüística)

8,00 X 3.764,00 = 30.112,00 Ptas. 30.112,00 Ptas.





4. Presupuesto

162


1 401 Documentación técnica del programa desimulación.

1 497,62 €



10,00 X 4.931,00 = 49.310,00 Ptas. 49.310,00 Ptas.

1001 h Ingeniero Superior(revisión técnica)

3,00 X 6.114,00 = 18.342,00 Ptas. 18.342,00 Ptas.

1002 h Filólogo en lenguacastellana e inglesa(revisión técnica ylingüística)

1,50 X 3.764,00 = 5.646,00 Ptas. 5.646,00 Ptas.



Materiales 2000 h Ordenador 10,00 X 102,00 = 1.020,00 Ptas. 1.020,00 Ptas.2001 h Consulta de libros y


2002 h Material diverso de oficina 4,50 X 238,00 = 1.071,00 Ptas. 1.071,00 Ptas.Material de presentación del programa (CD's) 521,00 Ptas. 521,00 Ptas.





4. Presupuesto

163

4.3 Precios Compuestos de las Subpartidas

Programa de simulación de un Robot móvilDesignación de las subpartidasCódigo Descripción Precio en € Precio en Ptas.101 Planteamiento de los objetivos e ideas básicas del programa de

simulación93,91 € 15.625,48 Ptas.

102 Desarrollo de los objetivos y finalidades del programa desimulación

75,92 € 12.633,01 Ptas.

103 Esquematización del programa de simulación 60,60 € 10.083,52 Ptas.104 Estudio del lenguaje de programación (MATLAB V4.2.1.c) 269,78 € 44.887,14 Ptas.

201 Inicio de la programación 135,92 € 22.615,24 Ptas.202 Desarrollo y diseño del programa de simulación 3890,39 € 647.306,28 Ptas.

301 Finalización y depuración del programa de simulación 160,86 € 26.765,82 Ptas.302 Fase de pruebas y conclusiones del programa. Corrección de

errores365,29 € 60.778,54 Ptas.

303 Revisión-corrección técnica 374,81 € 62.362,80 Ptas.304 Revisión-corrección lingüística 184,60 € 30.714,24 Ptas.

401 Documentación técnica del programa de simulación. 497,62 € 82.265,55 Ptas.

4. Presupuesto

164

4.4 Resumen del Presupuesto

RESUMEN DEL PRESUPUESTOSimulador en MATLAB de Robots móvilesPartida Subpartida Importe en € Importe en Ptas.01 Fase de inicio 101 Planteamiento de los objetivos e ideas

básicas del programa de simulación93,91 € 15.625,48 Ptas.

102 Desarrollo de los objetivos yfinalidades del programa de simulación

75,92 € 12.633,01 Ptas.

103 Esquematización del programa desimulación

60,60 € 10.083,52 Ptas.

104 Estudio del lenguaje de programación(MATLAB V4.2.1.c)

269,78 € 44.887,14 Ptas.

02 Desarrollo delprograma

201 Inicio de la programación 135,92 € 22.615,24 Ptas.

202 Desarrollo y diseño del programa desimulación

3890,39 € 647.306,28 Ptas.

03 Pruebas, correccionesy revisiones

301 Finalización y depuración del programade simulación

160,86 € 26.765,82 Ptas.

302 Fase de pruebas y conclusiones delprograma. Corrección de errores

365,29 € 60.778,54 Ptas.

303 Revisión-corrección técnica 374,81 € 62.362,80 Ptas.304 Revisión-corrección lingüística 184,60 € 30.714,24 Ptas.

04 Documentación 401 Documentación técnica del programade simulación.

497,62 € 82.796,97 Ptas.

PRESUPUESTO DE EJECUCIÓN 6109,70 € 1.016.569,03 Ptas.GASTOS GENERALES 13,00 % 794,26 € 132.153,97 Ptas.BENEFICIO INDUSTRIAL 6,00 % 366,58 € 60.994,14 Ptas.PRESUPUESTO DE EJECUCIÓN POR CONTRATA 7270,55 € 1.209.717,14 Ptas.

IVA 16,00 % 1163,29 € 193.554,74 Ptas.El presupuesto asciende a la cantidad de: 8433,83 € 1.403.272 Ptas.

El Presupuesto de ejecución asciende a OCHO MIL CUATROCIENTOSTREINTA Y TRES EUROS Y OCHEINTA Y TRES CÉNTIMOS (8433,83 €, 1.403.272Ptas.). Este precio incluye la instalación y puesta en marcha del programa, así como laasesoría técnica sobre el funcionamiento del mismo.



Pliego de Condiciones

5. Pliego de Condiciones

166


5.1 Objetivo

El objetivo de este Pliego de Condiciones es definir y establecer con el usuario finaldel programa y la Universidad Rovira i Virgili, las limitaciones y condiciones sobre ladistribución, utilización y responsabilidades concernientes a este simulador en MATLABde Robots móviles.

5.2 Materiales

Este Proyecto consiste en un programa de simulación de Robots móviles en elentorno de MATLAB V4.2.1c, formado por 72 funciones realizadas en lenguaje M-MATLAB V4.2.1c. El paquete en el que se presenta el programa de simulación consta de:

- Documentación Técnica Memoria DescriptivaMemoria de CálculoPresupuestoPliego de CondicionesAnexos (Programa, Presentación de las pantallas,

Información técnica)Manual de Usuario

- CD-ROM donde se presenta todo el programa, con todas las funciones necesariasy elementos para su funcionamiento, así como la recopilación de toda la documentacióntécnica anteriormente referida.

5.3 Condiciones Generales

Conjuntamente con el CD donde se recopila todo el programa de simulación ytodos los ficheros informáticos referentes a la completa realización y ejecución del mismo,se hace entrega de toda la documentación técnica del programa. La entrega de estadocumentación a la Universidad Rovira i Virgili no ha comportado ningún tipo deretribución económica para el Ingeniero Técnico firmante del Proyecto.

No está permitida la reproducción total o parcial de este Proyecto (incluyendo elprograma y la documentación técnica adjuntada), ni su tratamiento informático, ni latransmisión de ninguna forma o por cualquier medio, ya sea electrónico, mecánico, porfotocopia, por registro u otros métodos, sin el permiso previo y por escrito del IngenieroTécnico firmante del Proyecto.

5.4 Condiciones Económicas

Por la realización de este proyecto y la entrega de la documentación adjunta, no hanexistido retribuciones económicas al Ingeniero Técnico firmante del mismo, y por tanto laUniversidad Rovira i Virgili no puede obtener beneficios económicos por la venta odistribución del programa de simulación, o de cualquier documento o información que deéste se derive.


167

5.5 Condiciones de SeguridadEn el momento de la entrega del programa y de la información adjunta en el CD, se

comprueba con el programa Panda Antivirus V6.0 (revisión de 1-1-2001) la no existenciade virus en el mismo, no haciéndose responsable el Ingeniero Técnico firmante demanipulaciones, copias u otros modos de reproducción no aprobadas por éste, queperjudiquen y afecten al usuario del simulador o al usuario de la información adjuntada.

5.6 Limitación de Responsabilidad

La instalación y funcionamiento del programa de simulación debe realizarse tal ycomo se especifica en el Manual de Usuario que se adjunta en la documentación técnica,sin hacerse responsable el Ingeniero Técnico firmante de la pérdida o daños en el hardwareo software del ordenador donde se instale y ejecute el programa o cualquier fichero deinformación técnica que le acompaña, provocados por el uso incorrecto del simulador,tanto en su instalación o en su posterior utilización.

Tarragona, 15 de junio de 2001Firmado: Santos Barquero CáceresIngeniero Técnico en Electrónica Industrial


Anexo APrograma

Anexo A. Programa

170

A.1 Funciones de Inicio del Programa

function [] = comensa()

% -----------------------------------------------------------------------% COMENSA()%% Función de inicio.% Inicialización de las variables globales del programa (variables de% configuración).% Llamada a la Pantalla Principal de Simulación (idsply).% -----------------------------------------------------------------------

close all; % Cerrar todas las pantallas-gráficas actuales.clear all; % Vacía el espacio de trabajo MATLAB V4.0.clear functions; % Vacía de la memoria las funcionesclc; % Limpiar pantalla de trabajo.

invarsim; % Función declaración variables globales Simulink.invargen(1); % Función declaración variables globales generales.idsply; % Display-entorno de la pantalla de Simulación

% (buttons, popups, etc...).

Anexo A. Programa

171

function[]=invargen(op)

% -----------------------------------------------------------------------% INPUT_VARIABLES_GENERALES(op)%% Función de declaración de variables globales utilizadas en el programa.% Variables con información de máximos, mínimos, etc...% -----------------------------------------------------------------------

global DISPLAY ... % Figura principal. Pantalla de simulaciónAGENTS ... % Vector de los Robots creados, Agents that this Matlab

% process simulates or controlsMAX_AGENTS ... % Variable de máximo numero de Robots a crearDEFAULT_AGENT_RADIUS ... % Variable del radio de los robots

% por defectodt ... % Diferencial de tiempo, tiempo de simulaciónPOSITIONS ... % Vector de positions a crearDEFAULT_POSITION_RADIUS ...OBSTACLES ... % Objects in the simulated worldMAX_OBSTACLES ... % Número máximo de obstáculosDEFAULT_OBSTACLE_WIDTH ... % Radio, tamaño del obstáculo por

% defectoMAX_SIZE_OBSTACLES ... % Máximo tamaño obstáculoTRAILDIANA ... % Variable para el patch del recorrido de la

% dianaTRAIL ... % Variable para el patch del recorrido de los

% robotsOBJECT_GRAS... % Vector de índices de los objetos de graspingMAX_SENSORS ... % Número máximo de sensores por RobotMAX_ANG_SEN ... % Máximo angulo de un sensorMIN_ANG_SEN ... % Mínimo angulo de un sensorMAX_DIS_SEN ... % Máxima distancia de detección permitida para un

% sensorMIN_DIS_SEN ... % Mínima distancia de detección permitida para un

% sensorMAX_SIZE ... % Máximo radio-tamaño permitido para un RobotMIN_SIZE ... % Mínimo radio-tamaño permitido para un RobotMAX_MIN_ROT ... % Máximo y mínimo angulo de rotacióncolor ... % Vector de colores, color =

% ['y' 'c' 'b' 'r' 'm' 'w' 'g' 'w'];diana ... % Opción de visualizar dianavital ... % Opción de visualizar espacio vital robotveloSimula ... % Variable de referencia para marcar la velocidad

% de simulación deseada (1=Alta;2=Media;3=Baja)evitar % Número de movimientos para evitar obstáculo

% -----------------------------------------------------------------% Inicio de la lectura del fichero con los valores de las variables% globales% -----------------------------------------------------------------% Lectura fichero de valores de configuración.fid = fopen('varcfg.m','r'); % Abrir fichero para lectura


while ~(feof(fid)) & lectura~='' % Leer mientras no final if strcmp(lectura,'diana') % Visualizar diana diana = fgetl(fid); end;

Anexo A. Programa

172

if strcmp(lectura,'vital') % Visualizar espacio vital robot vital = fgetl(fid); end; if strcmp(lectura,'MAX_AGENTS') MAX_AGENTS = str2num(fgetl(fid)); end;

if strcmp(lectura,'DEFAGERAD') DEFAULT_AGENT_RADIUS = str2num(fgetl(fid)); end;

if strcmp(lectura,'MAX_SIZE') % Máximo radio-tamaño permitido para un Robot en cm MAX_SIZE = str2num(fgetl(fid)); end;

if strcmp(lectura,'MIN_SIZE') % Mínimo radio-tamaño permitido para un Robot en cm MIN_SIZE = str2num(fgetl(fid)); end;

if strcmp(lectura,'dt') % Tiempo granulado simulación (fichero simulink) dt = str2num(fgetl(fid)); end;

if strcmp(lectura,'MAX_SENSORS') % Número máximo de sensores por Robot MAX_SENSORS = str2num(fgetl(fid)); end;

if strcmp(lectura,'MAX_DIS_SEN') % Máxima distancia de detección permitida para un sensor MAX_DIS_SEN = str2num(fgetl(fid)); end;

if strcmp(lectura,'MIN_DIS_SEN') % Mínima distancia de detección permitida para un sensor MIN_DIS_SEN = str2num(fgetl(fid)); end;

if strcmp(lectura,'MAX_ANG_SEN') % Máximo angulo de un sensor MAX_ANG_SEN = str2num(fgetl(fid)); end;

if strcmp(lectura,'MIN_ANG_SEN') % Mínimo angulo de un sensor MIN_ANG_SEN = str2num(fgetl(fid)); end;

if strcmp(lectura,'MAX_MIN_ROT') % Máximo y mínimo angulo de rotación en grados MAX_MIN_ROT = str2num(fgetl(fid)); end;

Anexo A. Programa

173

if strcmp(lectura,'MAXOBSTA') % Número máximo de obstáculos MAX_OBSTACLES = str2num(fgetl(fid)); end;

if strcmp(lectura,'DEFOBSTWIDTH') % Radio del obstáculo por defecto DEFAULT_OBSTACLE_WIDTH = str2num(fgetl(fid)); end;

if strcmp(lectura,'MAXSIOBST') % Máximo tamaño obstáculo en cm MAX_SIZE_OBSTACLES = str2num(fgetl(fid)); end;

if strcmp(lectura,'veloSimula') % Velocidad de Simulación veloSimula = str2num(fgetl(fid)); end;



fclose(fid); % Cerrar fichero para Load. Acabada lectura de los% valores para las variables de configuración

if op == 0 % Inicializar todas las variables globales ??? return; % op = 0 ---NO---end;

evitar = 15; % Número de movimientos para evitar obstáculo

AGENTS = []; % Vector de los Robots creadosDEFAULT_AGENT_RADIUS = 4; % Variable del radio de los Robots pordefecto

DEFAULT_POSITION_RADIUS = 1; % Variable del radio de las dianas pordefecto

DEFAULT_OBSTACLE_WIDTH = 3; % Radio del obstáculo por defecto

% Colores por defecto de la Simulación

% Amarillo 'y' Cyan 'c' Azul 'b' Rojo 'r' Magenta 'm'% Verde 'g' Blanco 'w' Negro 'k'

color = ['b' 'c' 'y' 'r' 'm' 'w' 'g' 'k'];

for j=1:MAX_AGENTS % Inicializar matriz de estado de los sensores OBJECT_GRAS(j) = 0; % Vector de índices de los objetos de grasping POSITIONS(j) = 0; % Vector de positions a crearend % end for index1=1:MAX_AGENTS

clear j index2

Anexo A. Programa

174

función varcfg.m (no ejecutable)

dianaonvitalonMAX_AGENTS7MAX_SIZE5MIN_SIZE2.5dt0.005MAX_SENSORS10MAX_DIS_SEN20MIN_DIS_SEN5MAX_ANG_SEN75MIN_ANG_SEN15MAX_MIN_ROT60MAXOBSTA20MAXSIOBST7.5veloSimula1

Anexo A. Programa

175

A.2 Funciones de Creación de las Pantallas del Programa

function [] = idsply()

% -------------------------------------------------------------------% Función para la creación de la pantalla de entorno de la simulación% initDisplay().%% Inicializar interface de pantalla%% URV 05-11-2000% Copyright (c) 2000 by URV% and the Regents of the University of Tarragona% -------------------------------------------------------------------

% Declaración de variables de la funciónglobal AGENTS WORLD_AXIS POSITIONS text1_error stop

% --------------------------------------------------------------% Declaración de la figura-pantalla del entorno de la simulación% --------------------------------------------------------------DISPLAY1 = figure('Name', 'Physical RobotSimulator','interruptible','yes', ...

'Units','normalized','Position', [0 0 1 1]);

% ------------------------------------------------------% Creación del texto y del entorno de simulación gráfica% ------------------------------------------------------

stop = 1; % Variable de control de stop del Run : 0-RUN 1-STOP

% División lineas de referencia ejes de la pantalla de simulaciónz1 = 0:10:130;z2 = 10:10:90;

WORLD_AXIS = axes('position', [0.025 .12 .7 .68], 'xlim',[0 130], 'ylim', [0 90],'interruptible', 'yes', 'box', 'on', 'gridlinestyle', ':','linewidth', 0.05,...'units','cent','xgrid','on','ygrid','on','xtick',z1,'xcolor','w',...'ytick',z2,'ycolor','w', 'ytickmode', 'manual','yticklabelmode','manual', 'xticklabels', z1,'yticklabels', z2,'fontsize', 16, 'drawmode', 'fast');

text1_error = text('color', 'y','fontweight', 'normal','units','normalized','color', 'r','position', [0 1.24], 'string', '', 'fontsize',20, 'rotation',0, 'visible', 'on');

% --------------------------------------------------% Main control buttons: Run, Reset, Step, Stop, Quit% --------------------------------------------------

quitText = uicontrol('Style','text','Units','normalized','Position',[.75.95 .11 .03], 'HorizontalAlignment','Center','String', 'End Simulation','BackgroundColor','m', 'Callback','');

quitButton = uicontrol('Style','Push','Units','normalized','HorizontalAlignment','Center', 'Position',[.87 .945 .07.04],'String','Quit','Value',0, ...'Callback','quit1(0);');

Anexo A. Programa

176

resetButton = uicontrol('Style','Push','Units','normalized','Position',[.87 .9 .07 .04], 'HorizontalAlignment', 'Center', 'String','Reset', ...'visible', 'on', 'Callback', 'Reset;');

stopButton = uicontrol('Style','Push','Units', 'normalized','Position',[.75 .9 .07 .04], 'Value',0, 'HorizontalAlignment', ‘Center','interruptible', 'no', 'String','Stop','Callback','stop = 1;');

% stop = 1 Variable de control de stop del Run : 0-RUN 1-STOP

simuText = uicontrol('Style','text','Units','normalized','Position',[.75.85 .08 .03],'HorizontalAlignment', 'Center','String','Simulation','BackgroundColor', 'm', 'Callback','');

stepButton = uicontrol('Style', 'Push','Units','normalized', 'Position',[.84 .845 .07 .04], 'HorizontalAlignment', 'Center','String', 'Step',...'Callback', 'stop=0;paso;stop=1;');

runButton = uicontrol('Style', 'Push','Units', 'normalized','Position',[.915 .845 .07 .04], 'String', 'Run', 'HorizontalAlignment', 'Center','interruptible', 'yes', 'Callback', 'mainloop;');

% -------------------------------------------------------------------% Setup buttons for type movement, Grasping, Forward, Tracking, Other% -------------------------------------------------------------------

global moveText grasping forward tracking other obSetk xobj_Textxobj_coor yobj_Text yobj_coor

moveText = uicontrol('Style','text','Units','normalized','Position',[.75.795 .09 .03],'HorizontalAlignment', 'Center','String','Movement','BackgroundColor','m', 'Callback','');

grasping = uicontrol('Style', 'Radio', 'Visible','on','Units','normalized','Position',[.85 .79 .09 .04],'String','Grasping','value',0,'interruptible', 'yes', 'HorizontalAlignment', 'Center', 'Callback','fungras;');

forward = uicontrol('Style','Radio','Visible','on','Units','normalized','Position', [.75 .74 .09 .04], 'String', 'Forward','value' ,0,'interruptible', 'yes', 'HorizontalAlignment', 'Center','Callback','funforw;');

tracking = uicontrol('Style', 'Radio','Visible', 'on','Units','normalized', 'Position',[.85 .74 .09 .04],'String','Tracking','value',0,'interruptible','yes', 'HorizontalAlignment','Center', ...'Callback','funtrack;');

other = uicontrol('Style', 'Radio', 'Visible','on','Units','normalized','Position',[.75 .69 .09 .04], 'String', 'Other', 'value',1,'interruptible', 'yes','HorizontalAlignment', 'Center','Callback','funother');

obSetk = uicontrol('Visible', 'off','Style','Push','Units', 'normalized','Position', [.85 .69 .09 .04],'HorizontalAlignment', 'center','String','Update', 'Callback', 'sother;');

xobj_Text = uicontrol('Visible','on', 'Style','text','Units','normalized', 'Position', [.75 .55 .16 .03],

Anexo A. Programa

177

HorizontalAlignment','left','String','','BackgroundColor','y','Callback','');

xobj_coor = uicontrol('Visible','on','Style','Edit','Units','normalized','Position', [.91 .55 .05 .03],'String','', 'BackgroundColor', 'w', ...'HorizontalAlignment', 'Center', 'Callback','');

yobj_Text = uicontrol('Visible','on','Style','text','Units','normalized','Position',[.75 .51 .16 .03],'HorizontalAlignment','left','String','','BackgroundColor','y','Callback','');

yobj_coor = uicontrol('Visible','on','Style','Edit','Units','normalized','Position',[.91 .51 .05 .03],'String','','BackgroundColor','w', ...'HorizontalAlignment','Center','Callback','');

funother; % Función de movimiento tipo other

% ----------------------------------------------------------% Setup buttons for AGENTS-ROBOTS Add, Del, SetMouse, etc...% ----------------------------------------------------------

global agAdd agNum agDel agSet agSetk x_coor y_coor ang_ro editro

agText = uicontrol('Style','text','Units','normalized', ...'Position',[.75 .56 .07 .03],'HorizontalAlignment','Center', ...'String','Robot nº','BackgroundColor','m','Callback','');

agAdd = uicontrol('Style','Push','Units', 'normalized', ...'Position', [.75 .51 .09 .04],'HorizontalAlignment', 'center', ...'String', 'Add Robot','Callback', 'agent;');

agNum = uicontrol('Style','Popup','Units','normalized','Position', [.825.56 .04 .04], 'Value', 0,'BackgroundColor','w', 'String','','Callback','chrobot;');

agDel = uicontrol('Style','Push','Units', 'normalized','Position', [.75.46 .09 .04], ...

'HorizontalAlignment','center','String', 'DelRobot','Callback','dagent;');

agSet = uicontrol('Style','Push','Units', 'normalized','Position', [.845.51 .09 .04],'HorizontalAlignment', 'center', 'String', 'Set mouse','Callback', 'sagent;');

agSetk = uicontrol('Style','Push','Units','normalized','Position', [.845.46 .09 .04],

'HorizontalAlignment', 'center','String', 'Setkeyboard','Callback','sagentk;');

editro = uicontrol('Style','Push','Units', 'normalized','Position', [.845.41 .09 .04],'HorizontalAlignment', 'center', 'String', 'Edit Robot','Callback', 'edirobot;');

x_Text = uicontrol('Visible','on','Style', 'text','Units', 'normalized','Position', [.75 .37 .16 .03], 'HorizontalAlignment', 'left','String', 'X Coordinate (<130cm):','BackgroundColor', 'y','Callback','');

x_coor = uicontrol('Visible', 'on','Style', 'Edit','Units', 'normalized','Position', [.91 .37 .05 .03],'String','','BackgroundColor', 'w','HorizontalAlignment', 'Center', 'Callback','');

Anexo A. Programa

178

y_Text = uicontrol('Visible', 'on','Style', 'text','Units', 'normalized','Position', [.75 .33 .16 .03],'HorizontalAlignment', 'left', ...'String', 'Y Coordinate (<90cm):','BackgroundColor', 'y','Callback','');

y_coor = uicontrol('Visible','on','Style', 'Edit','Units', 'normalized','Position', [.91 .33 .05 .03],'String','','BackgroundColor', 'w', ...'HorizontalAlignment', 'Center','Callback','');

ang_Text = uicontrol('Visible', 'on','Style', 'text','Units','normalized', 'Position', [.75 .29 .16 .03],'HorizontalAlignment','left','String', 'Orientation (Degrees):','BackgroundColor','y','Callback','');

ang_ro = uicontrol('Visible', 'on','Style', 'Edit','Units', 'normalized','Position', [.91 .29 .05 .03],'String','','BackgroundColor', 'w', ...'HorizontalAlignment', 'Center','Callback','');

% --------------------------------% Setup buttons for OBSTACLE, Edit% --------------------------------

obsText = uicontrol('Style', 'text','Units', 'normalized','Position',[.75 .235 .08 .03], 'HorizontalAlignment', 'Center','String','Obstacles', 'BackgroundColor', 'm', 'Callback','');

obsButton = uicontrol('Style','Push','Visible','on','Units','normalized', 'Position', [.85 .23 .08.04], 'String', 'Edit', 'value', 0,'interruptible','yes','HorizontalAlignment', 'Center', 'Callback','editobst;');

% ---------------------------------------------------% Setup buttons for Trail, Logger Plot, Clear, etc...% ---------------------------------------------------

global trailButton loggButton

resulText = uicontrol('Style', 'text','Units', 'normalized','Position',[.75 .185 .08 .03], 'HorizontalAlignment', 'Center', 'String','Results','BackgroundColor', 'm', 'Callback','');

loggButton = uicontrol('Style','Radio','Visible','on','Units','normalized', 'Position', [.85 .18 .08 .04],'String', 'Logger','value',1,'interruptible', 'yes','HorizontalAlignment', 'Center', ...'Callback','');

plotButton = uicontrol('Style','Push','Units', 'normalized','Position',[.85 .13 .08 .04], 'HorizontalAlignment', 'center','String', 'Plot data','Callback','plotData;');

clearButton = uicontrol('Style','Push','Units', 'normalized','Position',[.75 .13 .08 .04],'HorizontalAlignment', 'Center','String', 'Clear', ...'Callback','limpiar;');

trailText = uicontrol('Style', 'text','Units', 'normalized','Position',[.75 .085 .08 .03],'HorizontalAlignment','Center','String','Robots','BackgroundColor', 'm', 'Callback','');

Anexo A. Programa

179

trailButton = uicontrol('Style','Radio','Visible','on','Units','normalized', 'Position', [.85 .08 .08 .04],'String', 'Trail', ...'value', 0,'interruptible','yes','HorizontalAlignment', 'Center', ...'Callback','');

% ----------------------------------------------------------------% Creación de los buttons y entorno SAVE, LOAD and QUIT edit Robot% ----------------------------------------------------------------

options3 = uimenu('label', 'Options','Accelerator','O');save3 = uimenu(options3,'label','Save','callback','');

uimenu(save3,'label','Save DataSimulator','callback','filedata(1);');load3 = uimenu(options3,'label','Load','callback','');

uimenu(load3,'label','Load DataSimulator','callback','filedata(2);');config = uimenu(options3,'label','Change DataConfig','callback','chcfg;');

% -----------------------------------------% Creación de los text tiempo de Simulación% -----------------------------------------

global minutos segundos minu segun dt_edit dt_text dt %dt_total

minu = 0;segun = 0;

tiem_Text = uicontrol('Visible', 'on','Position',[.03 .87 .09 .03], ...'HorizontalAlignment',

'Left','Style','text','Units','normalized', ...'String','CPU Time = ','BackgroundColor', 'y','Callback','');

minutos = uicontrol('Visible', 'on','Position',[.115 .87 .07 .03], ...'HorizontalAlignment','Left','Style','text',...'Units','normalized','String','0 min', ...'BackgroundColor','w','Callback','');

segundos = uicontrol('Visible', 'on','Position', [.20 .87 .07 .03], ...'HorizontalAlignment','Left','Style','text','Units',

'normalized',...'String','0 s','BackgroundColor', 'w','Callback','');

dt_text = uicontrol('Visible', 'on','Position', [.03 .83 .245 .03],...'HorizontalAlignment', 'Left','Style','text','Units', 'normalized', ...'String',['Total Grain Time (GT = ' num2str(dt*1000) ' ms) = '] , ...'BackgroundColor', 'y','Callback','');

dt_edit = uicontrol('Visible', 'on','Position', [.2745 .83 .07 .03], ...'HorizontalAlignment', 'Left','Style','text',...'Units', 'normalized','String','0 s' , ...'BackgroundColor', 'w','Callback','');

Anexo A. Programa

180

% ---------------------------% Creación de PATCH OBSTACLES% ---------------------------

global OBSTACLES MAX_OBSTACLES

% ------------------------------------------------------------------% Crear los patch's que definen el obstáculo (gráficos), UserData =state, añadiendo el manejador% de este obstáculo en el vector OBSTACLES% ------------------------------------------------------------------

for i = 1:MAX_OBSTACLES

% Crear patch gráfico para Obstáculo a =patch('XData',0,'YData',0,'EraseMode','xor','FaceColor','w','EdgeColor','w');

% selfobs = [coordenada"x" coordenada"y" theta radius shape] selfobs = [0 0 0 0 0]; % Vector de estado inicializado a cero

state = [0 selfobs]; % Vector de datos del obstáculo(inicializar índice = 0)

set(a,'UserData',state); % Dato "UserData" del manejador delobstáculo OBSTACLES(i) = a; % Guardar manejador del obstáculo

end; % end for i = 1:MAX_OBSTACLESclear state selfobs a i

Anexo A. Programa

181

function[] = edirobot(a)

% ------------------------------------------------------------------% EDITROBOT(a)% EDITROBOT()%% Función para crear la Pantalla con el entorno de Diseño del Robot.% El índice del Robot se lee del popup de la pantalla principal de% simulación, o se obtiene como parámetro de entrada.% ------------------------------------------------------------------% SEE ALSO: editobst(), idsply()

% Declaración de variables de la función

global WORLD_AXIS2 DISPLAY2 AGENTS MAX_SENSORS AGENT_AMPLI agNumagAdd text1_error text2_error text_num_sen ampli y0_ampli x0_ampli

text_num_sen = [];set(text1_error,'visible','off');

if nargin == 0 % Leer índice del Robot popup de la pantalla a = popup(AGENTS,agNum,1,text1_error,'Warning: Not exist Robot');

if a == 0 % Robot no existente ? set(agAdd, 'Value', 0); return; end;end; % end if nargin == 0

% Creación de la pantalla para el diseño del Robot. Ejes, circulos y% texto de referencia

DISPLAY2 = figure('Name','Robot Design','interruptible','yes', ...'Units','normalized','Position', [0 0 1 1]);

ampli = 1.95;z = -50:5:50;

WORLD_AXIS2 = axes('position', [.05 .09 .58 .79],'xlim', [-50 50],'ylim', [-50 50], 'box', 'on', 'gridlinestyle', '-','linewidth', 0.5,'units', 'cent', 'xgrid', 'off', 'ygrid', 'off','xtick', z*ampli,'ycolor', 'w', 'xcolor','w','ytick', z*ampli, 'ytickmode', 'manual','yticklabelmode','manual','xticklabels', z,'yticklabels',z, 'fontsize',16, 'drawmode', 'fast');

text_world = text('color', 'm', 'fontweight', 'bold','units','normalized', 'position', [.5 1.05], 'string', 'Robot Design','fontsize',30, 'HorizontalAlignment', 'center');

text2_error = text('color','w', 'fontweight', 'normal','units','normalized', 'position', [0.005 1.11],'string', '', 'fontsize',20,'color', 'r', 'rotation', 0,'visible','off');

axes(WORLD_AXIS2);hold on;

x0_ampli = 0; % Coordenada "x" inicial para el Robot de la pantalla% de edición

Anexo A. Programa

182

y0_ampli = 0; % Coordenada "y" inicial para el Robot de la pantalla% de edición

s1 = [0 0];s2 = [50 -50];plot(s1,s2,'w');plot(s2,s1,'w');

radius = 0;

% Circulos de referencia de la pantalla de ediciónfor j=1:7 radius = radius + 5; z = linspace(0,2*pi,100); s1 = x0_ampli + cos(z) * radius * ampli; s2 = y0_ampli + sin(z) * radius * ampli; plot(s1,s2,'w:','linewidth',0.1);end;

% Función recuperar la estructura de datos del Robot[state i self goal confina mast redon graf_sen dades_sen] = strucrob(a);

% Dibuja el Robot con un factor de amplificación de "factor=ampli"% Crear un Robot AGENT_AMPLI

col = get(AGENTS(a),'FaceColor'); % Color del Robot de la pantalla dediseño

AGENT_AMPLI = patch('XData', 0, 'YData', 0,'EraseMode', 'xor','FaceColor',col, 'EdgeColor',col);

for j=1:MAX_SENSORS dades_sen(j,4) = dades_sen(j,4)*ampli; dades_sen(j,1) = dades_sen(j,1)*ampli; graf_sen(j)=line('EraseMode','xor','XData',0,'YData',0,'color','g','UserData',dades_sen(j,:));

end;

% Inicializar objeto tipo "line", circulo de espacio vitalredon = line('EraseMode','xor','XData',0,'YData',0,'Color','r');

state = [redon graf_sen];

set(AGENT_AMPLI, 'UserData', state);

[xydata redon graf_sen] = dibu_a(x0_ampli, y0_ampli, pi/2, ampli*self(4),redon, graf_sen, dades_sen);

Anexo A. Programa

183

% --------------------------% Numeración de los sensores% --------------------------

for j=1:MAX_SENSORS

text_num_sen(j) =text(x0_ampli,y0_ampli,'','visible','off','color','r','HorizontalAlignment','center');

if dades_sen(j,1) ~= 0';

x_ini = x0_ampli + dades_sen(j,4) * cos(dades_sen(j,5) + pi/2); y_ini = y0_ampli + dades_sen(j,4) * sin(dades_sen(j,5) + pi/2);

posx = x_ini + cos(dades_sen(j,3) + pi/2) * dades_sen(j,1) * (0.9); posy = y_ini + sin(dades_sen(j,3) + pi/2) * dades_sen(j,1) * (0.9);

set(text_num_sen(j),'position', [posxposy],'string',num2str(j,1),'visible','on');

end;end;clear x_ini y_ini posy posx

% Guardar datos del Robot en la pantalla de diseño

set(AGENT_AMPLI,'XData',xydata(:,1),'YData',xydata(:,2));

state = [redon graf_sen];

set(AGENT_AMPLI,'UserData',state);

% ----------------------------------------------------------% Creación de los buttons y entorno de datos para SIZE Robot% ----------------------------------------------------------

global MAX_SIZE MIN_SIZE ch_si_ro size_ro num_ro

roText = uicontrol('Visible', 'on','HorizontalAlignment', 'left', ...'Style', 'text','Units', 'normalized','Position', [.65 .73

.08 .03], ...'HorizontalAlignment', 'Center','String',['Robot nº '

num2str(i)], ...'BackgroundColor', 'm','Callback','');

num_ro = i; % Guardar el índice del Robot

ch_si_ro = uicontrol('Visible', 'on','Style', 'Push',...'Units', 'normalized','Position', [.77 .73 .10 .03], ...'String', 'Change size','HorizontalAlignment', 'Center', ...'Callback','chsize;');

size_Text = uicontrol('Visible', 'on','Style', 'text','Units','normalized', 'Position', [.65 .69 .24 .03],'HorizontalAlignment','left', 'String', ['Radius Robot MAX/MIN ' num2str(MAX_SIZE) ' / '...num2str(MIN_SIZE) ' cm:'], 'BackgroundColor', 'y', 'Callback','');

size_ro = uicontrol('Visible','on','Style','Edit','Units','normalized','Position', [.889 .69 .05 .03],'String', self(4), ...'BackgroundColor', 'w', 'HorizontalAlignment', 'Center', 'Callback','')

Anexo A. Programa

184

% -------------------------------------------------------------% Creación de los buttons y entorno de datos ADD, DELETE sensor% -------------------------------------------------------------

global MAX_DIS_SEN MIN_DIS_SEN MAX_ANG_SEN MIN_ANG_SEN MAX_MIN_ROTnum_sen add_sen del_sen edit_sen dis_sen ang_sen rot_sen maes_fi

sen_Text = uicontrol('Visible', 'on','HorizontalAlignment','left','Style', 'text',...'Units', 'normalized','Position', [.65 .62 .08 .03], ...'HorizontalAlignment', 'Center','String', 'Sensor nº', ...'BackgroundColor', 'm','Callback','');

num_sen = uicontrol('Visible', 'on','Style','Popup','BackgroundColor','w',...'Units', 'normalized','Position', [.735 .62 .04 .04], ...'String','','HorizontalAlignment', 'Center','Callback','chcolsen;');

add_sen = uicontrol('Visible', 'on','Style', 'Push',...'Units', 'normalized','Position', [.65 .58 .1 .03], ...'String', 'Add Sensor','HorizontalAlignment','Center','Callback','asensor;');

edit_sen = uicontrol('Visible', 'on','Style', 'Push',...'Units', 'normalized','Position', [.77 .58 .1 .03], ...'String', 'Update','HorizontalAlignment', 'Center', ...'interruptible', 'yes','Callback','updasen;');

dis_Text = uicontrol('Visible', 'on','Style', 'text','Units','normalized', 'Position', [.65 .54 .26 .03],'HorizontalAlignment','left', 'String', ['Distance MAX/MIN ' num2str(MAX_DIS_SEN) ' / ' ...num2str(MIN_DIS_SEN) ' cm:'],'BackgroundColor', 'y','Callback','');

dis_sen = uicontrol('Visible', 'on','Style', 'Edit','Units','normalized', 'Position', [.909 .54 .05 .03],'String','','BackgroundColor', 'w', 'HorizontalAlignment', 'Center','Callback','');

ang_Text = uicontrol('Visible', 'on','Style', 'text','Units','normalized', 'Position', [.65 .5 .26 .03], 'HorizontalAlignment','left', 'String', ['Directivity MAX/MIN ' num2str(MAX_ANG_SEN) ' / ' ...num2str(MIN_ANG_SEN) ' Degrees:'],'BackgroundColor', 'y','Callback','');

ang_sen = uicontrol('Visible', 'on','Style', 'Edit','Units','normalized', 'Position', [.909 .5 .05 .03], 'String','','BackgroundColor', 'w', 'HorizontalAlignment', 'Center','Callback','');

rot_Text = uicontrol('Visible', 'on','Style', 'text','Units','normalized','Position', [.65 .46 .26 .03],'HorizontalAlignment', 'left','String', ['Rotate sensor MAX/MIN ±' num2str(MAX_MIN_ROT) ' Degrees:'],'BackgroundColor', 'y','Callback','');

rot_sen = uicontrol('Visible', 'on','Style', 'Edit','Units','normalized', 'Position', [.909 .46 .05 .03],'String','', ...'BackgroundColor', 'w','HorizontalAlignment', 'Center','Callback','');

del_sen = uicontrol('Visible', 'on','Style','Push','Units','normalized','Position',[.65 .42 .1 .03],'String','Del Sensor','HorizontalAlignment','Center','interruptible','yes','Callback','dsensor;');

Anexo A. Programa

185

maes_Text = uicontrol('Visible', 'on','Style', 'text','Units','normalized','Position', [.65 .38 .25 .03],'HorizontalAlignment', 'left','String', 'Programation sensors in the function:','BackgroundColor', 'y','Callback','');

maes_fi = uicontrol('Visible', 'on','Style', 'Edit','Units','normalized', 'Position', [.90 .38 .08 .03],'String',get(mast,'UserData'),'BackgroundColor', 'w','HorizontalAlignment','Center','Callback','');

% --------------------------------------------------------------% Creación de los buttons y entorno de datos File Control System% --------------------------------------------------------------

global edit_mot ra_fi_con1 nafi_con

con_Text = uicontrol('Visible','on','HorizontalAlignment','left', ...'Style','text','Units','normalized','Position',[.65 .31 .145 .03], ...'HorizontalAlignment','Center','String','Simulink Control File ', ...'BackgroundColor', 'm','Callback','');

edit_con = uicontrol('Visible', 'on','Style','Push','Units','normalized',.'Position',[.805 .31 .1 .03],'String','Open File','HorizontalAlignment','Center', 'Callback','opensimu;');

nafi_text1 = uicontrol('Visible', 'on','Style', 'text','Units','normalized', 'Position', [.65 .27 .085 .03],'HorizontalAlignment','left', 'String', 'Name File:','BackgroundColor', 'y','Callback','');

nafi_con = uicontrol('Visible', 'on','Style', 'Edit','Units','normalized', 'Position', [.73 .27 .08 .03],'String',get(confina,'UserData'), 'BackgroundColor', 'w','HorizontalAlignment','Center','Callback','');

ra_fi_con1 = uicontrol('Style', 'Radio','Visible','on','Units','normalized', 'Position', [.65 .23 .225 .03],'String', ' "O" Only Motors/ "·" All System', 'value',get(confina,'Xdata'), ... % 'Xdata' -- 0Only Motors -- 1 All System 'interruptible', 'yes','HorizontalAlignment', 'Center', 'Callback','');

% -----------------------------------------------------% Creación de los buttons y entorno SAVE, LOAD and QUIT% Barra de herramientas de la pantalla de diseño% -----------------------------------------------------

global save load quit

options = uimenu('label', 'Options','Accelerator','O');save = uimenu(options,'label','Save Config Robot','callback','');

uimenu(save,'label','Data Only','callback','filecfg(1);');uimenu(save,'label','All Config','callback','filecfg(2);');

load = uimenu(options,'label','Load Config Robot','callback','');uimenu(load,'label','Data Only','callback','filecfg(3);');uimenu(load,'label','All Config','callback','filecfg(4);');

quit = uimenu(options,'label','Quit','callback','quit1(1);');

% Función renovar índices de los Robots en el popup de la pantallapopup(dades_sen(:,1),num_sen,0);

clear z s1 s2 s

Anexo A. Programa

186

function[] = editobst()

% --------------------------------------------------------------% EDITOBSTACLE()%% Función para crear la pantalla con el entorno de diseño de los% obstáculos.% --------------------------------------------------------------% SEE ALSO: editrobot(), idsply()

% Declaración de variables de la funciónglobal DISPLAY3 WORLD_AXIS2 text1_error text2_error text_num_sen

set(text1_error,'visible','off'); % Mensaje de error, no existe ficherode Simulink

% --------------------------------------------------% Figura-pantalla del entorno de Creación Obstáculos% --------------------------------------------------

DISPLAY3 = figure('Name','Obstacles Design','interruptible','yes', ...'Units','normalized','Position', [0 0 1 1],'color','k');

% ------------------------------------------------------% Creación del texto y del entorno de simulación gráfica% ------------------------------------------------------

% División lineas de referencia ejes de la pantalla de simulaciónz1 = 0:10:130;z2 = 10:10:90;

WORLD_AXIS2 = axes('position', [0.035 .17 .7 .69],'xlim', [0 130],'ylim', [0 90], 'interruptible','yes','xcolor','w','ycolor','w',...'box', 'on', 'gridlinestyle', ':','linewidth', 0.05, 'units', 'cent', ...'xgrid', 'on', 'ygrid', 'on','xtick',z1,'ytick',z2, ...'ytickmode','manual','yticklabelmode','manual',...'xticklabels',z1,'yticklabels',z2,'fontsize',16,'drawmode','fast');

text_world = text('color', 'm', 'fontweight', 'bold','units','normalized','position', [.5 1.05],'string', 'Obstacles Design','fontsize',30, ‘HorizontalAlignment', 'center');

text2_error = text('color', 'y', 'fontweight', 'normal','color','r', ...'units', 'normalized', 'position', [0 1.13], ...'string','','fontsize',20,'rotation', 0,'visible','off');

% --------------------------% Setup buttons for OBSTACLE% --------------------------

global obsNum obsSet obsSetk obsAdd obsDel

obsText = uicontrol('Style', 'text','Units', 'normalized','Position',[.75 .67 .09 .03], 'HorizontalAlignment', 'Center','String', 'Obstaclenº','BackgroundColor', 'm', 'Callback','');

obsNum = uicontrol('Style', 'Popup','Units', 'normalized','Position',[.845 .67 .04 .04], 'BackgroundColor','w','Value', 0,'String','','Callback','chcolobs;');

Anexo A. Programa

187

obsAdd = uicontrol('Style', 'Push','Units', 'normalized','Position', [.75.61 .09 .04], 'HorizontalAlignment', 'center','String', 'Add Obsta.','Callback','obsta;');

obsDel = uicontrol('Style', 'Push','Units', 'normalized','Position', [.75.56 .09 .04], 'HorizontalAlignment', 'center','String', 'Del Obsta.','Callback','dobsta;');

obsSet = uicontrol('Style', 'Push','Units', 'normalized', ...'Position', [.845 .61 .09 .04], 'HorizontalAlignment', 'center', ...'String', 'Set mouse','Callback','sobsta;');

obsSetk = uicontrol('Style', 'Push','Units', 'normalized', ...'Position', [.845 .56 .09 .04],'HorizontalAlignment', 'center', ...'String', 'Update','Callback','sobstak;');

% --------------------------------------% Setup buttons for Coordinates OBSTACLE% --------------------------------------

global MAX_SIZE_OBSTACLES xob_coor yob_coor ang_obs_ro rad_obs_ro

xob_Text = uicontrol('Visible', 'on','Style', 'text','Units','normalized', 'Position', [.75 .52 .16 .03],'HorizontalAlignment','left', 'String', 'X Coordinate (<130cm):','BackgroundColor','y','Callback','');

xob_coor = uicontrol('Visible', 'on','Style', 'Edit','Units','normalized', 'Position', [.91 .52 .05 .03],'String','','BackgroundColor', 'w', ...'HorizontalAlignment', 'Center','Callback','');

yob_Text = uicontrol('Visible', 'on','Style', 'text','Units','normalized', 'Position', [.75 .48 .16 .03],'HorizontalAlignment','left','String', 'Y Coordinate (<90cm):','BackgroundColor','y','Callback','');

yob_coor = uicontrol('Visible', 'on','Style', 'Edit','Units','normalized', 'Position', [.91 .48 .05 .03],'String','','BackgroundColor', 'w', 'HorizontalAlignment', 'Center','Callback','');

ang_obs_Text = uicontrol('Visible', 'on','Style', 'text','Units','normalized', 'Position', [.75 .44 .16 .03], 'HorizontalAlignment','left', 'String', 'Orientation (Degrees):','BackgroundColor','y','Callback','');

ang_obs_ro = uicontrol('Visible', 'on','Style', 'Edit','Units','normalized', 'Position', [.91 .44 .05 .03],'String','','BackgroundColor', 'w', 'HorizontalAlignment', 'Center','Callback','');

rad_obs_Text = uicontrol('Visible','on','Style', 'text','Units','normalized', 'Position', [.75 .40 .16 .03], 'HorizontalAlignment','left', 'String', ['Size Obstacle (<' num2str(MAX_SIZE_OBSTACLES) 'cm):'], 'BackgroundColor', 'y','Callback','');

rad_obs_ro = uicontrol('Visible', 'on','Style', 'Edit',...'Units', 'normalized','Position', [.91 .40 .05 .03],'String','', ...'BackgroundColor', 'w', 'HorizontalAlignment', 'Center','Callback','');

Anexo A. Programa

188

% --------------------------------% Setup buttons for SHAPE OBSTACLE% --------------------------------

global shapecua shapecir

sha_obs_Text = uicontrol('Visible','on','Style', 'text','Units','normalized','Position', [.75 .343 .13 .03],'String', 'Shape forObstacle', 'HorizontalAlignment', 'left','BackgroundColor','y','Callback','');

shapecua = uicontrol('Style', 'Radio','Visible','on','Units','normalized', 'Position',[.885 .36 .065 .03],'String', 'Square','value',1, 'interruptible','yes','HorizontalAlignment','Left','Callback','chshape(1);');

shapecir = uicontrol('Style', 'Radio','Visible','on','Units','normalized','Position', [.885 .325 .065 .03],'String','Circular','value', 0, 'interruptible', 'yes','HorizontalAlignment','Left','Callback','chshape(0);');

% -------------------------------------------------------------% Creación de los buttons y entorno SAVE, LOAD and QUIT% Barra de herramientas de la pantalla de edición de obstáculos% -------------------------------------------------------------

global options2 save2 load2 quit2

options2 = uimenu('label', 'Options','Accelerator','O');save2 = uimenu(options2,'label','Save ConfigObstacles','callback','');

uimenu(save2,'label','Data Obstacles','callback','fileobst(1);');load2 = uimenu(options2,'label','Load ConfigObstacles','callback','');

uimenu(load2,'label','Data Obstacles','callback','fileobst(2);');quit2 = uimenu(options2,'label','Quit','callback','quit1(2);');

% --------------------------------------------------------------------% Copy PATCH OBSTACLES, copiar manejadores de la pantalla principal de% simulación (OBSTACLES) a% la pantalla de edición de obstáculos (OBSTACLES2)% --------------------------------------------------------------------

global OBSTACLES OBSTACLES2 MAX_OBSTACLES

for index=1:length(OBSTACLES)

text_num_sen(index) =text(0,0,'','visible','off','color','r','HorizontalAlignment','center');

state = get(OBSTACLES(index), 'UserData'); i = state(1); % Índice del obstáculo % selfo = [coordenada"x" coordenada"y" theta radius shape] selfo = state(2:6);

if i~=0 % Si existe PATCH OBSTACLES

% Función de dibujo del obstáculo [xdata ydata] =dibu_o(selfo(1),selfo(2),selfo(3),selfo(4),selfo(5));

Anexo A. Programa

189

% Crear el patch gráfico del obstáculo (pantalla de edición) a =patch('XData',xdata,'YData',ydata,'EraseMode','xor','FaceColor','w','EdgeColor','w');

state = [i selfo]; % Vector de datos del obstáculo

set(a,'UserData',state); % Dato "UserData" del manejador delobstáculo OBSTACLES2(i) = a; % Guardar manejador del obstáculo pantallade edición

% Numerar obstáculos set(text_num_sen(i),'position', [selfo(1)selfo(2)],'string',num2str(i,2),'visible','on');

end; % end if i~=0end; % end for index=1:length(OBSTACLES)

% Función renovar índices de los obstáculos en el popup de la pantallapopup(OBSTACLES2,obsNum,0);

clear s state a

Anexo A. Programa

190

function [] = chcfg()

% ---------------------------------------------------------------% CHANGECONFIG()%% Función para la creación de la pantalla de configuración de las% variables globales del programa.% ---------------------------------------------------------------% SEE ALSO: idsply(), edirobot()

% Declaración de variables de la funciónglobal DISPLAY4 AGENTS text1_error

set(text1_error,'visible','off');

% Declaración de la figura-pantalla del entorno de la simulación

DISPLAY4 = figure('Name','Config of DataSimulator','color','k','interruptible','yes',...

'Units','normalized','Position', [0.15 0.15 0.7 0.55],'color', 'k');

% ------------------% Setup config Robot% ------------------

global MAX_AGENTS MAX_A

Text1 = uicontrol('Style', 'text','Units', 'normalized','Position', [.03.88 .33 .045], 'HorizontalAlignment', 'left','String', 'VALUE OF THECONFIG ROBOT', 'BackgroundColor', 'm', 'Callback','');

aText = uicontrol('Style', 'text','Units', 'normalized','Position', [.03.81 .25 .045], 'HorizontalAlignment', 'left','String', 'MAX_NUMBER = ',...'BackgroundColor', 'y','Callback','');

MAX_A = uicontrol('Style', 'edit','Units', 'normalized', ...'Position', [.28 .81 .06 .045],'HorizontalAlignment', 'left', ...'String',['' num2str(MAX_AGENTS)],'BackgroundColor', 'W','Callback','');

% -------------------% Setup config Robots% -------------------global MAX_SIZE MIN_SIZE MAX_SI MIN_SI

Text3 = uicontrol('Style', 'text','Units', 'normalized','Position', [.03.74 .25 .045], 'HorizontalAlignment', 'left','String', 'MAX_SIZE_Radius(cm) = ', 'BackgroundColor', 'y','Callback','');

MAX_SI = uicontrol('Style', 'edit','Units', 'normalized','Position', [.28.74 .06 .045], 'HorizontalAlignment', 'left','String',[''num2str(MAX_SIZE)], 'BackgroundColor', 'W','Callback','');

simiText = uicontrol('Style', 'text','Units', 'normalized', ...'Position', [.03 .67 .25 .045],'HorizontalAlignment', 'left', ...'String', 'MIN_SIZE_Radius (cm) = ','BackgroundColor','y','Callback','');

Anexo A. Programa

191

MIN_SI = uicontrol('Style', 'edit','Units', 'normalized','Position', [.28.67 .06 .045], 'HorizontalAlignment', 'left','String',[''num2str(MIN_SIZE)], 'BackgroundColor', 'W','Callback','');

% --------------------% Setup config Sensors% --------------------

global MAX_SENSORS MAX_SE

Text2 = uicontrol('Style', 'text','Units', 'normalized','Position', [.03.56 .36 .045], 'HorizontalAlignment', 'left','String', 'VALUE OF THECONFIG SENSORS','BackgroundColor', 'm','Callback','');

Text3 = uicontrol('Style', 'text','Units', 'normalized','Position', [.03.49 .25 .045], 'HorizontalAlignment', 'left','String', 'NUMBER_MAX =','BackgroundColor', 'y','Callback','');

MAX_SE = uicontrol('Style', 'edit','Units', 'normalized','Position', [.28.49 .06 .045], 'HorizontalAlignment', 'left','String',[''num2str(MAX_SENSORS)], 'BackgroundColor', 'W','Callback','');

% --------------------% Setup config Sensors% --------------------

global MAX_ANG_SEN MIN_ANG_SEN MAX_DIS_SEN MIN_DIS_SEN MAX_MIN_ROTMA_DS MI_DS MA_ANG MI_ANG MA_MI_ROT

Text3 = uicontrol('Style', 'text','Units', 'normalized','Position', [.03.42 .25 .045], 'HorizontalAlignment', 'left','String', 'MAX_DISTANCE (cm)= ', 'BackgroundColor', 'y','Callback','');

MA_DS = uicontrol('Style', 'edit','Units', 'normalized','Position', [.28.42 .06 .045], 'HorizontalAlignment', 'left','String',[''num2str(MAX_DIS_SEN)], 'BackgroundColor', 'W','Callback','');

Text3 = uicontrol('Style', 'text','Units', 'normalized','Position', [.03.35 .25 .045], 'HorizontalAlignment', 'left','String', 'MIN_DISTANCE (cm)= ','BackgroundColor', 'y','Callback','');

MI_DS = uicontrol('Style', 'edit','Units', 'normalized','Position', [.28.35 .06 .045], 'HorizontalAlignment', 'left','String',[''num2str(MIN_DIS_SEN)], 'BackgroundColor', 'W','Callback','');

Text3 = uicontrol('Style', 'text','Units', 'normalized','Position', [.03.28 .25 .045], 'HorizontalAlignment', 'left','String', 'MAX_ANG (Degrees)= ', 'BackgroundColor', 'y','Callback','');

MA_ANG = uicontrol('Style', 'edit','Units', 'normalized','Position', [.28.28 .06 .045], 'HorizontalAlignment', 'left','String',[''num2str(MAX_ANG_SEN)], 'BackgroundColor', 'W','Callback','');

Text3 = uicontrol('Style', 'text','Units', 'normalized','Position', [.03.21 .25 .045], 'HorizontalAlignment', 'left','String', 'MIN_ANG (Degrees)= ', 'BackgroundColor', 'y','Callback','');

MI_ANG = uicontrol('Style', 'edit','Units', 'normalized','Position', [.28.21 .06 .045], 'HorizontalAlignment', 'left','String',[''num2str(MIN_ANG_SEN)], 'BackgroundColor', 'W','Callback','');

Anexo A. Programa

192

Text3 = uicontrol('Style', 'text','Units', 'normalized','Position', [.03.14 .25 .045],'HorizontalAlignment', 'left','String', 'MAX_ANG_ROT(Degr.) =', 'BackgroundColor', 'y','Callback','');

MA_MI_ROT = uicontrol('Style', 'edit','Units', 'normalized','Position',[.28 .14 .06 .045], 'HorizontalAlignment', 'left','String',[''num2str(MAX_MIN_ROT)], 'BackgroundColor', 'W','Callback','');

global MAX_OBSTACLES MAX_SIZE_OBSTACLES MAX_O MAX_SI_O

Text1 = uicontrol('Style', 'text','Units', 'normalized','Position', [.5.88 .38 .045], 'HorizontalAlignment', 'left','String', 'VALUE OF THECONFIG OBSTACLES','BackgroundColor', 'm','Callback','');

oText = uicontrol('Style', 'text','Units', 'normalized','Position', [.5.81 .25 .045], 'HorizontalAlignment', 'left','String','MAX_OBSTACLES = ','BackgroundColor', 'y','Callback','');

MAX_O = uicontrol('Style', 'edit','Units', 'normalized','Position', [.75.81 .06 .045], 'HorizontalAlignment', 'left','String',[''num2str(MAX_OBSTACLES)],'BackgroundColor', 'W','Callback','');

oText = uicontrol('Style','text','Units', 'normalized','Position', [.5.74 .25 .045], 'HorizontalAlignment', 'left','String','MAX_SIZE_OBSTACLE=', 'BackgroundColor', 'y','Callback','');

MAX_SI_O = uicontrol('Style','edit','Units','normalized','Position', [.75.74 .06 .045], 'HorizontalAlignment','left','String',[''num2str(MAX_SIZE_OBSTACLES)], 'BackgroundColor', 'W','Callback','');

% --------------------------------------------% Setup text config data diferencial de tiempo% --------------------------------------------

global dt D_T

Text1 = uicontrol('Style', 'text','Units', 'normalized','Position', [.5.56 .205 .045], 'HorizontalAlignment', 'left','String', 'VALUE GRAINTIME', 'BackgroundColor', 'm','Callback','');

tText = uicontrol('Style', 'text','Units', 'normalized','Position', [.5.49 .18 .045], 'HorizontalAlignment', 'left','String','Grain Time (ms) =', 'BackgroundColor', 'y','Callback','');

D_T = uicontrol('Style', 'edit','Units', 'normalized','Position', [.68.49 .08 .045], 'HorizontalAlignment', 'left','String',[''num2str(dt*1000)], 'BackgroundColor', 'W','Callback','');

% ----------------------------% Setup text config data DIANA% ----------------------------

global DIN VIT diana vital

DIN = uicontrol('Style', 'Radio','Units', 'normalized','Position', [.5.42 .325 .045], 'HorizontalAlignment', 'left','String','Visible Setpoint("·" Yes/ "O" No)', 'value',0,'Callback','');

Anexo A. Programa

193

% ------------------------------------% Setup text config data ESPACIO VITAL% ------------------------------------

VIT = uicontrol('Style','Radio','Units','normalized','Position', [.5 .35.42 .045], 'HorizontalAlignment','left','value',0,'String', 'Visibleliving space Robot ("·" Yes/ "O" No)','Callback','');

if diana(1:2) == 'on' set(DIN,'value',1);end;

if vital(1:2) == 'on' set(VIT,'value',1);end;

% ----------------------------------------------% Setup Popup velocidad de simulación continuada% ----------------------------------------------

global veloRun veloSimula

Text2 = uicontrol('Style','text','Units', 'normalized','Position', [.5.28 .37 .045], 'HorizontalAlignment', 'left','String', 'VALUE VELOCITYSIMULATOR (RUN)','BackgroundColor', 'm','Callback','');

veloRun = uicontrol('Style', 'Popup','Units', 'normalized','Position',[.875 .275 .1 .045],'HorizontalAlignment', 'center', 'String','Fast|Normal|Slow','value',veloSimula);

% ----------------------------------------% Setup button for QUIT screen config data% ----------------------------------------

global quitcfd

quitcfd = uicontrol('Style', 'Push','Units', 'normalized','Position', [.5.14 .14 .07], 'HorizontalAlignment', 'center','String', 'Quit & Save','Callback','quit1(3);');

clear aText tText Text1 oText Text3 simiText

Anexo A. Programa

194

A.3 Funciones Asociadas al Tratamiento de Datos del Robot

function [n] =agent(n,self,filename1,typefile,filename2,graf_sen,dades_sen)

% -----------------------------------------------------------------------% n = AGENT;% n = AGENT(n,self,graf_sen,dades_sen)%% Función para crear un nuevo ROBOT (AGENT).%% Un Robot es un tipo especial de "patch", donde se le define una% estructura de datos de tipo "UserData". El patch no deja de ser un% objeto-dibujo (con un espacio de memoria asignado) bidimensional, con% una estructura muy manejable (manual de MATLAB V4.0).% El patch gráfico, tiene un estado interno que permite moverlo de forma% dinámica por la pantalla modificando sus coordenadas 'Xdata' 'Ydata'.% Estructura de datos patch gráfico (Robot-AGENT).% state(UserData of patch) = [i, state, goal, confina, graf_sen], donde% i -- índice del robot% -- datos propios del Robot% self = [x y theta radius velolineal veloangular pertur]% goal = [r psi libre?] -- coordenadas polares Robot-objetivo% -- name of the file of control% confina = [control_name_file tipo_control]% redon = [gráfico tipo "line" que define el circulo vital del Robot]% graf_sen(i) = [coordenades de dibuix sensor "i"]% dades_sen(i,:) =[distan_detec, ang_detec, ang_rotate_sensor,% position_dis, position_ang]%% El manejador del patch creado, se guarda en la posición "n" del vector% AGENT, para ser recuperado en la ejecución del programa cuando se desee% modificar algún dato del Robot.% -----------------------------------------------------------------------% SEE ALSO; sagent, gagent

% Declaración de variables de la funciónglobal AGENTS MAX_AGENTS DEFAULT_AGENT_RADIUS MAX_SENSORS WORLD_AXIScolor agAdd agNum grasping x_coor y_coor ang_ro text1_error vital wwx wwywx wy R

axes(WORLD_AXIS);set(text1_error,'visible','off'); % Borrar texto de Warning

if nargin < 7

dades_sen = [0 0 0 0 0]; % Inicializar vector de datos referentes a% los sensores

% Inicializar gráfico "line" para dibujar sensores for j = 1:MAX_SENSORS graf_sen(j) =line('EraseMode','xor','Visible','on','XData',0,'YData',0,'color','g','UserData',dades_sen); end;

Anexo A. Programa

195

filename1 = 'defaultc'; typefile = 1; filename2 = 'maes';

if nargin < 2 % Si parámetros de entrada < 2, nuevo índice

blankspots = find(AGENTS == 0); % posiciones igual a cero % El valor "n" contiene el nuevo índice del Robot if blankspots

n = blankspots(1); else

n = length(AGENTS) + 1; end

% Control del número máximo de agentes que se pueden crear if n > MAX_AGENTS

% Número máximo de Robots superadoset(text1_error,'String',['Warning: Agent not created. MAX_AGENTS =

' num2str(MAX_AGENTS)],'visible','on','color','r'); % Mensaje de errorset(agAdd, 'Value', 0);return;

end

% Lectura de los datos (coordenadas, radio, etc) introducidas porteclado if (get(x_coor,'string')=='' | get(y_coor,'string')=='' |get(ang_ro,'string')=='') % Mensaje de error, faltan datos set(text1_error,'String','Warning: Date Input Robot, is notcorrect','visible','on'); return; end

% Lectura de los datos introducidos por teclado % Asignar coordenadas y el radio al nuevo Robot creado x = str2num(get(x_coor,'string')); % Leer coordenada "x" del

% button edit y = str2num(get(y_coor,'string')); % Leer coordenada "y" del

% button edit theta = str2num(get(ang_ro,'string'))*pi/180; % Leer ángulo del

% button edit radius = DEFAULT_AGENT_RADIUS;

% self = [coorde. "x" coorde. "y" theta radius velolineal% veloangular pertur]

self = [x y theta radius 0 0 0];

% ----------------------------------------------------------------- % wwx wwy wx wy R -> variables del fichero de Simulink. % bounds -> fichero simulink % Función de saturación de las coordenadas del Robot xmax = [0+R

% 130-R], ylim = [0+R 90-R]. % -----------------------------------------------------------------

temps = 1e-5; wwx = self(1); wwy = self(2); R = self(4);

Anexo A. Programa

196

% Control límites del terreno de juego del Robot "a" rk45('bounds',temps,[],[1e-6 temps temps 0 0 2]); self(1) = wx; self(2) = wy;

if self(3) > 2*pi self(3) = self(3) - 2*pi; elseif self(3) < 0 self(3) = 2*pi + self(3); end

set(x_coor,'string',num2str(self(1))); % Set en los buttons tipoedit set(y_coor,'string',num2str(self(2))); set(ang_ro,'string',num2str(180*self(3)/pi));

end; % end if nargin < 2end; % end if nargin < 3

% Evitar superposición Robot con otros elementos de la simulaciónerror = noencima(self,n,'r');

if error % Superposición, mensaje de error stop = 1; set(text1_error,'String',['Warning: Superposition Robot nº 'num2str(n) ' Coordinates x = ' num2str(self(1)) ' y = ' num2str(self(2))],... 'visible','on'); % Mensaje de error return;end; % end if error

% Crear un objeto asociado al Robot en opción graspingif get(grasping,'value') error = gras(n); if error return; end; % end if errorend % end if grasping

% goal = [r psi libre?] -- coordenadas polares Robot-objetivogoal = [0 0 0]; % Vector de datos para la consigna de posición(coordenadas polares)

% Inicializar objeto tipo "line", circulo de espacio vitalredon =line('EraseMode','xor','XData',0,'YData',0,'Color','r','Visible',vital);

% Crear un patch para guardar el nombre del fichero Simulink de Control% 'XData' = 0 Only Motors 'XData' = 1 All System% 'defaultc' es el fichero Simulink de control, por defectoconfina =patch('Visible','off','XData',typefile,'YData',0,'UserData',filename1);

% 'maes' es el fichero de control máquina de estados, por defectomast = patch('Visible','off','UserData',filename2);

% Función "dibu_a" para definir las coordenadas de dibujo del Robotif nargin == 7 [xydata redon graf_sen] =dibu_a(self(1),self(2),self(3),self(4),redon,graf_sen,dades_sen,n);

Anexo A. Programa

197

else [xydata redon] = dibu_a(self(1),self(2),self(3),self(4),redon);end % end if nargin == 4

% Crear el patch gráfico que define al Robot.

a = patch('XData', xydata(:,1), 'YData', xydata(:,2),'EraseMode', 'xor','FaceColor', color(n),...

'EdgeColor',color(n));

% Guardar los diferentes patch de datos del Robot en el vector state,definido como dato "UserData"% del patch principal del Robot.state = [n self goal confina mast redon graf_sen];

set(a,'UserData',state);% El manejador del patch se guarda en la posición "n" del vector AGENTS.AGENTS(n) = a;

% Crear un objectivo-destinación asociado al Robot (diana, consigna deposición del Robot)position(n);


% Inicializar variables de representación gráfica (vector de la matriz derepre. gráfica).if nargin < 2

global x_ag ... % Coordenana x del Robot para ploty_ag ... % Coordenana y del Robot para plotlogv_m_d ... % Velocidad del motor derecho Robotlogi_m_d ... % Velocidad del motor izquierdo Robotlogv_m_i ... % Velocidad del motor derecho Robotlogi_m_i ... % Intensidad del motor derecho Robotlogp_m_d ... % Intensidad del motor izquierdo Robotlogp_m_d ... % Par del motor derecho robotlogp_m_i ... % Par del motor izquierdo robotlog_vlin ... % Velocidad lineal del Robotlog_vang ... % Velocidad angular del Robotuxjv uyjv

uxjv(n) = self(1); % Guardar coordenada "x" anterior uyjv(n) = self(2); % Guardar coordenada "y" anterior

x_ag(n,1) = self(1);y_ag(n,1) = self(2); logv_m_d(n,1) = 0;logv_m_i(n,1) = 0; logi_m_d(n,1) = 0;logi_m_i(n,1) = 0; logp_m_d(n,1) = 0;logp_m_i(n,1) = 0; log_vlin(n,1) = 0;log_vang(n,1) = 0;

% Función presentación de la pantalla de diseño del Robot (tamaño,sensores, etc). edirobot(n);

end % end if nargin < 2clear z self s menu xydata s1 s2 a confina mast

Anexo A. Programa

198

function [state,i,self,goal,confina,mast,redon,graf_sen,dades_sen] =strucrob(a,i,self,goal,confina,mast,redon,graf_sen)

% -----------------------------------------------------------------------% [state,i,self,goal,confina,graf_sen,dades_sen] =% = STRUCROB(a,i,self,goal,confina,graf_sen)%% Función para recuperar o guardar la estructura de datos del Robot.%% nargin = 1, un sólo parámetro de entrada que corresponde al índice del% Robot.% La función devuelve como parámetros de salida los datos de la% estructura del Robot.%% nargin = 7, parámetros de entrada = datos de la estructura del Robot.% Guardar los datos de entrada en la estructura del Robot.% Estructura de datos del patch gráfico (Robot-AGENT).% state(UserData of patch) = [i, state, goal, confina, graf_sen], donde% i -- índice del robot% -- datos propios del Robot% self = [x y theta radius v omega pertur]% goal = [r psi libre?] -- coordenadas polares Robot-objetivo% -- name of the file of control% confina = [control_name_file tipo_control]% redon = [gráfico tipo "line" que define el circulo vital del Robot]% graf_sen(i) = [coordenades de dibuix sensor "i"]% dades_sen(i,:) =[distan detec, ang_detec, ang rotate sensor,% position dis, position ang.]% -----------------------------------------------------------------------

global AGENTS MAX_SENSORS

if nargin == 1

% Recuperar "UserData" state, patch del Robot state = get(AGENTS(a), 'UserData'); i = state(1); self = state(2:8); goal = state(9:11); confina = state(12); mast = state(13); redon = state(14); graf_sen = state(15:(15+MAX_SENSORS-1));

for j = 1:MAX_SENSORS dades_sen(j,:) = get(graf_sen(j),'UserData'); end;

elseif nargin == 8

% Guardar estructura de datos del Robot state = [i self goal confina mast redon graf_sen]; set(AGENTS(a), 'UserData', state);

end; % end if nargin == 1

Anexo A. Programa

199

function [xydata, redon, graf_sen] = dibu_a(x, y, theta, radius, redon,graf_sen, dades_sen,a)

% -----------------------------------------------------------------------% [xydata, graf_sen] = DIBUJAR_AGENTE(x, y, theta, radius, graf_sen,% dades_sen,a)%% Función para la representación gráfica del Robot. Calcula las% coordenadas que definen los cuatro puntos del patch del Robot (matriz% xydata). Dibuja el circulo que define el espacio vital del Robot% “redon" y realiza el dibujo de los sensores, representados en forma de% objetos tipo "line".% Los parámetros de entrada corresponden a los datos:% --- Coordenada central del Robot "x-y".% --- Ángulo de orientación "theta".% --- Radio del Robot (tamaño) "radius".% --- Manejador del circulo que define el espació vital del Robot% "redon".% --- Gráfico de los sensores "graf_sen".% --- Matriz de datos de los sensores "dades_sen".% --- Índice del Robot "a".%% La salida de la función corresponde a los puntos de "xydata" para el% patch del Robot, y la estructura "line" (sensores), además de circulo% vital definido en el manejador "redon".% -----------------------------------------------------------------------% SEE ALSO: dibu_o(), agent(), sagent()


global MAX_SENSORS ESTADO_SENSORES

% Número de puntos para la definición del objeto "line" se los circulosMAX_ELE = 100;

% Puntos para dibujar la forma-cuerpo cónico del Robot

xydata(:,1) = [x + radius*cos(theta + (1/4.5)*pi); ...x + radius*cos(theta - (1/4.5)*pi); ...x + radius*cos(theta - (3/4)*pi); ...x + radius*cos(theta - (5/4)*pi);];

xydata(:,2) = [y + radius*sin(theta + (1/4.5)*pi); ...y + radius*sin(theta - (1/4.5)*pi); ...y + radius*sin(theta - (3/4)*pi); ...y + radius*sin(theta - (5/4)*pi);];

% Dibujar círculo del espacio vital del Robotz = linspace(0,2*pi,MAX_ELE);

s1 = x + cos(z) * radius; % Calculo nuevas coordenadas circulo vitaldel Robots2 = y + sin(z) * radius;

set(redon,'XData', s1,'YData', s2); % Set redibujar circulo vital delRobot

Anexo A. Programa

200

% Dibujar los sensores del Robotif nargin > 6

for index=1:MAX_SENSORS % Dibujar sensores

if dades_sen(index,1)~=0 % Existe sensor?

xini = x + dades_sen(index,4) * cos(dades_sen(index,5)+theta);

yini = y + dades_sen(index,4) * sin(dades_sen(index,5)+theta);

% Puntos de la representación gráfica del sensorz = linspace((-dades_sen(index,2)/2),(dades_sen(index,2)/2),

MAX_ELE);

s1 = xini + cos(z+theta+dades_sen(index,3)) * dades_sen(index,1);s2 = yini + sin(z+theta+dades_sen(index,3)) * dades_sen(index,1);

s1(1) = xini;s2(1) = yini;

s1(MAX_ELE) = xini;s2(MAX_ELE) = yini;

% Detección límites de la simulación.if ((max(s1))>=130 | (max(s2))>=90 | (min(s2))<=0 | (min(s1))<=0) &

nargin == 8 col = 'r'; ESTADO_SENSORES(a,index) = 1;else col = get(graf_sen(index),'color');end; % end if s1(MAX_ELE_SENSOR/2)>=130 | ...

% Set de graf_sen 'Xdata' 'Ydata' de lo sensores (dibujar "line")set(graf_sen(index),'XData',s1,'YData',s2,'color',col);% Set vector de datos del sensor ("UserData")set(graf_sen(index),'UserData',dades_sen(index,:));

end; % end dades_sen(index,1)~=0 end; % end for index=1:MAX_SENSORSend; % end if nargin>6

clear MAX_ELE z xini yini s1 s2 colclear dibu_a

Anexo A. Programa

201

function [] = dagent(i)

% -----------------------------------------------------------------------% DELETEAGENT()% DELETEAGENT(i)%% Función para borrar el Robot "i".% El índice del Robot "i" se lee del popup de la Pantalla Principal de% Simulación o se obtiene como parámetro de entrada.% -----------------------------------------------------------------------% SEE ALSO: dsensor(), dobsta(), delpos(), agent(), sagent(), sagentk()

% Declaración de variables de la funciónglobal AGENTS OBJECT_GRAS text1_error agNum grasping agDel x_coory_coor ang_ro color other xobj_coor yobj_coor


% Leer índice del Robot en el popupif nargin == 0 i = popup(AGENTS,agNum,1,text1_error,'Warning: Not exist Robot');

if i == 0 set(agDel,'value',0); return; end;end; % end if nargin == 0

if AGENTS(i) ~= 0

% Función recuperar la estructura de datos del Robot [state i self goal confina mast redon graf_sen dades_sen] =strucrob(i);

clear dades_sen; % Borrar matriz de datos de los sensores %ESTADO_SENSORES(i,:) = 0; % Borrar vector de estado de los sensores

% Borrar estructura del Robot delete(redon); % Delete circulo espacio vital de Robot delete(confina); % Delete control file name delete(mast); % Delete machine states file delete(graf_sen); % Delete sensores

if get(grasping,'value') % Si existe objeto grasping, borrarlo MAR_FINAL = ggras(i,'MAR_FINAL'); delete(MAR_FINAL); delete(OBJECT_GRAS(i)); OBJECT_GRAS(i) = 0; % Borrar objeto si existe

set(xobj_coor,'string',''); % Borrar coordenadas buttons tipoedit set(yobj_coor,'string',''); end

if get(other,'value') set(xobj_coor,'string',''); % Borrar coordenadas buttons tipoedit set(yobj_coor,'string',''); end

Anexo A. Programa

202

% Borrar el patch del Robot delete(AGENTS(i)); % Delete AGENTS(i) % Borrar el manejador que apunta al patch gráfico del Robot(inicializar a cero) AGENTS(i) = 0; % Borrar manejador patch gráfico del Robot

set(x_coor,'string',''); % Borrar coordenadas buttons tipoedit set(y_coor,'string',''); set(ang_ro,'string','');

end; % end if AGENTS(i) ~= 0

delpos(i); % Borrar la position-diana del Robot "i"

% Modificar el número de Robots en el vector AGENTSwhile AGENTS(length(AGENTS)) == 0 % Mientras último valor igual a cero AGENTS = AGENTS(1:length(AGENTS)-1); % Eliminar la ultima posición delvector

if length(AGENTS) == 0 % Si no existen Robots, salir break; end; % end if length(AGENTS) == 0end % end while AGENTS(length(AGENTS)) == 0

% Función renovar índices de los Robots en el popup de la pantallai = popup(AGENTS,agNum,0);

if i == 0 set(agDel,'value',0); % Cambiar color de fondo popup a blanco set(agNum,'BackgroundColor','w'); return;end;

% Cambiar color de fondo popupset(agNum,'BackgroundColor',color(i));

clear s menu i o

Anexo A. Programa

203

function [value] = sagent(i, propertyname, value, selfobj)

% -----------------------------------------------------------------------% SAGENT(i, propertyname, value)% SAGENT()%% Esta función se encarga de guardar el dato referido en el parámetro de% entrada como "propertyname" en la estructura del Robot con el nuevovalor "value".% Si no existen parámetros de entrada, se realiza el set de las% coordenadas del Robot con el mouse.% "i" representa el número de Robot.% "propertyname" referencia del dato tipo "UserData" para realizar el% set.% "value" nuevo valor a guardar en la estructura del Robot.%% Cuando se realiza el set de las coordenadas (self) es necesario% redibujar el Robot (draw = 1).% -----------------------------------------------------------------------% SEE ALSO: gagent, agent, sagentk

global AGENTS WORLD_AXIS TRAIL agNum agSet text1_error x_coor y_coorang_ro R stop wwx wwy...

wx wy trailButton grasping

if nargin == 0 % Leer índice del Robot i = popup(AGENTS,agNum,1,text1_error,'Warning: Not exist Robot');

if i == 0 return; end;end; % end if nargin == 0

% Función recuperar la estructura de datos del Robot[state i self goal confina mast redon graf_sen dades_sen] = strucrob(i);

if nargin == 0

set(text1_error,'visible','off'); % Borrar texto de Warning % Si no hay parámetros de entrada, entonces esperar entrada del ratón pts = ginput(2); % coordenadas del ratón self(1) = pts(1,1); % x position click ratón self(2) = pts(1,2); % y position click ratón

if ~diff(pts) % NO hay diferiencia entre primer click y segundo (rotación Robot =0) self(3) = 0; else % SI hay diferiencia de ángulo entre primer click y segundo self(3) = atan2(pts(2,2) - pts(1,2),pts(2,1) - pts(1,1)); % Ángulo end

% Evitar superposición Robot con otros elementos de la simulación error = noencima(self,i,'r');

if error % Superposición, mensaje de error stop = 1; set(text1_error,'String',['Warning: Superposition Robot nº '

Anexo A. Programa

204

num2str(i) ' Coordinates x = ' num2str(self(1)) ' y = ' num2str(self(2))],'visible','on'); % Mensaje de error return; end; % end if error

draw = 1; libre = 0;

if get(grasping,'value') % Comprobar que no existe remolque en casode grasping selfobj = ggras(i,'selfobj');

if selfobj(5) == 1 % Set objeto juntamente con el Robot encaso de remolque

selfobj(1) = self(1);selfobj(2) = self(2);

sgras(i,'Self',selfobj,self); end % end if selfobj(5) == 1 end % end if get(grasping,'value')

elseif nargin > 2 propertyname = lower(propertyname);

if propertyname(1:2) == 'se' self = value; % self = [x y theta radius v omega pertur] -- vectorde estado del Robot draw = 1; elseif propertyname(1:2) == 'co' confina = value; % files = [control_name_file] elseif propertyname(1:2) == 'ma' mast = value; % files = [machine_states_file] elseif propertyname(1:2) == 're' redon = value; % redon = [gráfico tipo "line" que define elcirculo vital del Robot] elseif propertyname(1:2) == 'go' % goal = [r psi libre?] -- coordenadas polares Robot-objetivo goal = value; end; % end if propertyname(1:2) == 'se'end; % end if nargin == 0

% Dibujar el Robot en la nueva posición "x = self(1)" e "Y = self(2)" ycon el ángulo% "theta = self(3)"

if draw axes(WORLD_AXIS);

% -------------------------------------------------------------------- % wwx wwy wx wy R -> variables del fichero de Simulink. % bounds -> fichero simulink % Función de saturación de las coordenadas del Robot xmax = [0+R 130- % R], ylim = [0+R 90-R]. % --------------------------------------------------------------------

temps = 1e-5; wwx = self(1); wwy = self(2); R = self(4);

% Control límites del terreno de juego del Robot "a"

Anexo A. Programa

205

rk45('bounds',temps,[],[1e-6 temps temps 0 0 2]); self(1) = wx; self(2) = wy;

if self(3) > 2*pi self(3) = self(3) - 2*pi; elseif self(3) < 0 self(3) = 2*pi + self(3); end % Calculo de las coordenadas que definen el Robot en la nueva posición [xydata redon graf_sen] =dibu_a(self(1),self(2),self(3),self(4),redon,graf_sen,dades_sen,i);

% Guardar datos modificados del Robot

set(AGENTS(i),'XData',xydata(:,1),'YData',xydata(:,2));

% Control set coordenadas para la pantalla de simulación

menu = find(AGENTS); j = menu(get(agNum,'Value')); % Índice del Robot seleccionado en elpopup

if j == i % Set coordenadas del Robot seleccionado en el popup enlos buttons tipo edit set(x_coor,'string',num2str(self(1))); set(y_coor,'string',num2str(self(2))); set(ang_ro,'string',num2str(self(3)*180/pi)); end; % end if j = i

% Set TRAIL (rastro del Robot). Marcador de coordenadas del Robot. if get(trailButton,'Value') == 1 & stop == 0

p = patch('XData',[self(1)+.2 self(1)+.2 self(1)-.2 self(1)-.2],...'YData',[self(2)-.2 self(2)+.2 self(2)+.2 self(2)-.2], ...'EraseMode','xor','FaceColor',get(AGENTS(i),'FaceColor'),...'EdgeColor','none');

TRAIL = [TRAIL p]; end; % end if (get(trailButton, 'Value') == 1...

value = self; % Parámetro de la salidaend; % end if draw

% Función guardar la estructura de datos del Robotstrucrob(i,i,self,goal,confina,mast,redon,graf_sen);

% Vaciar el espacio de trabajo utilizado en esta funciónclear s1 s2 i z draw redon state i confina mast

Anexo A. Programa

206

function [] = sagentk()

% -----------------------------------------------------------------------% SAGENTK()%% Esta función se encarga de realizar el set de las coordenadas del Robot% a través de los valores x, y e theta introducidos por teclado (self(1)% = str2num(get(x_coor...)).% sagent(i,'Self',self) --- Set vector de estado Robot "i"% El índice del Robot se obtiene del popup de la pantalla.% -----------------------------------------------------------------------% SEE ALSO: gagent, agent, sagent


global AGENTS agNum agSetk text1_error x_coor y_coor ang_ro grasping


% Índice del Roboti = popup(AGENTS,agNum,1,text1_error,'Warning: Not exist Robot');

if i == 0 set(agSetk,'value',0); return;end;

% Comprobar las nuevas coordenadas introducidas por tecladoif (get(x_coor,'string')=='' | get(y_coor,'string')=='' |get(ang_ro,'string')=='') set(text1_error,'String','Warning: Date Input, is not correct',...

'visible','on','color','r'); % Mensaje de error, faltan datos return;end % end if (get(x_coor,'string')=='' | ...

% Función recuperar la estructura de datos del Robot[self] = gagent(i,'Self');

% Leer de los buttons tipo edits los datos de coordenadas, en forma destring (transformar de% string a número con la función de aplicación str2num).

self(1) = str2num(get(x_coor,'string')); % Leer coordenada "x" delbutton editself(2) = str2num(get(y_coor,'string')); % Leer coordenada "y" delbutton edittheta = str2num(get(ang_ro,'string')); % Leer ángulo del button editself(3) = theta*pi/180; % Convertir en radianes

% Evitar superposición objeto "grasping" con otros elementos de lasimulaciónerror = noencima(self,i,'r');

if error % Superposición, mensaje de error set(text1_error,'String',['Warning: Superposition Robot nº 'num2str(i) ' Coordinates x = ' num2str(self(1)) ' y = ' num2str(self(2))],...

'visible','on'); % Mensaje de error return;end; % end if error

Anexo A. Programa

207

if get(grasping,'value') % Comprobar remolcado en caso de grasping selfobj = ggras(i,'selfobj');

if selfobj(5) == 1 selfobj(1) = self(1); selfobj(2) = self(2); sgras(i,'Self',selfobj,self); end % end if selfobj(5) == 1end % end if get(grasping,'value')

sagent(i,'Self',self); % Llamada a la función de set en la estructura% del Robot

clear s1 s2 i z self theta selfobjclear sagentk

function [] = chrobot()

% ---------------------------------------------------------------------% CHANGEROBOT()%% Función asociada al popup de la pantalla de simulación (popup índices% Robots).% Esta función cambia los datos asociados al Robot en los buttons tipo% edit de la Pantalla de Simulación.% ---------------------------------------------------------------------% SEE ALSO: chcolsen(), chrobot(), chsize()

global AGENTS agNum grasping other text1_error x_coor y_coor ang_roobj_coor yobj_coor color


% Leer índice del Robot del popupi = popup(AGENTS,agNum,1,text1_error,'Warning: Not exist Robot');

if i == 0 return;end;

self = gagent(i,'Self'); % Recuperar vector de estado del Robot

if get(grasping,'value') % Set coordenadas objeto de grasping en los% buttons tipo edit

selfobj = ggras(i,'selfobj'); x_str = num2str(selfobj(1)); y_str = num2str(selfobj(2)); set(xobj_coor,'string',x_str); % Set datos buttons tipo edit

% (grasping) set(yobj_coor,'string',y_str);end; % end if get(grasping,'value')

if get(other,'value') % Set coordenadas del setpoint en los buttons% tipo edit (movi. other)

selfdia = getpos(i,'Location'); % Datos de la diana

Anexo A. Programa

208

x_str = num2str(selfdia(1));% Set de las coordenadas diana en la % pantalla

y_str = num2str(selfdia(2)); set(xobj_coor,'string',x_str); % Set datos buttons tipo edit (other) set(yobj_coor,'string',y_str);end; % end if get(other,'value')

% Set de las coordenadas en los buttons tipo edit de la pantalla desimulaciónset(x_coor,'String',num2str(self(1))); % Coordenada x Robotset(y_coor,'String',num2str(self(2))); % Coordenada y Robotset(ang_ro,'String',num2str(self(3)*180/pi)); % Ángulo Robot

% Cambiar el color de fondo del popup al color del Robot seleccionadoset(agNum,'BackgroundColor',color(i));

% Vaciar el espacio de trabajo utilizado en esta funciónclear chrobotclear self i menu x_str y_str

A.4 Funciones Asociadas al Tratamiento de Datos del Setpoint

function [n] = position(n, x, y, libre)

% -----------------------------------------------------------------------% n = POSITION;% n = POSITION(X, Y, RADIUS)%% Función para crear el setpoint (diana) y añadir el manejador del% setpoint al vector POSITIONS.% El índice de POSITIONS corresponde al mismo índice del Robot.% Una diana (POSITION) es un elemento de "texto", al que se le define una% estructura de datos de tipo "UserData". El texto no deja de ser un% objeto (con un espacio de memoria asignado) bidimensional, con la% posibilidad de dotarlo de una estructura de datos muy manejable (manual% de MATLAB V4.0).% El gráfico de la diana se define a partir de la variable de texto, con% la letra"O", que tiene un estado interno que permite moverlo de forma% dinámica por la pantalla modificando sus coordenadas de posición% 'Xdata' 'Ydata'.% Estructura de datos texto.% (manejador que apunta a los datos tipo) = [i, location], donde% i -- índice de la diana% location = [x y radius libre] -- vector de datso de posición% x = coordenada x de la diana% y = coordenada y de la diana% radius = radioscoordenada x de la diana% x = coordenada x de la diana%% El manejador del texto creado, se guarda en la posición "n" del vector% POSITION, para ser recuperado en la ejecución del programa cuando se% desee modificar algún dato de la diana.%% Si no existen parámetros de entrada, se inicializa el setpoint con las% variables establecidas por defecto.% -----------------------------------------------------------------------% SEE ALSO: setpos, getpos, delpos

Anexo A. Programa

209

global POSITIONS diana color other %DEFAULT_POSITION_RADIUS

% Inicializar vector de estado (x, y, ángulo) de position

if nargin < 4 %radius = DEFAULT_POSITION_RADIUS; libre = 0;

if nargin < 3 x = 65; y = 45; end; % end if nargin < 3end; % end if nargin < 4

loc = [x y libre]; % Vector de estado de la diana (coordenadas deposición, etc.)

% Texto que define el grafico de la diana "O"txt =text(x,y,'O','color',color(n),'visible',diana,'VerticalAlignment','middle',...

'HorizontalAlignment','center','EraseMode','xor','UserData',[nloc]);

POSITIONS(n) = txt; % Guardar el manejador del texto (gráfico de ladiana letra 'O')

% Inicializar coordenadas de la diana en opción otherif get(other,'value') sother(n,x,y);end % end if grasping

clear loc x y libre txtclear position

function [x,y,libre] = setpos(i,x,y,libre)

% -----------------------------------------------------------------------% SETPOSITIONS()% SETPOSITIONS(i)%% Esta función realiza el set (guardar) de las coordenadas del setpoint(diana).% i índice de la diana% x, y parámetros de entrada en la función, coordenadas de la diana% libre variable de libertad de movimiento del Robot (evitarobstáculo).% wx,wy parámetros de salida en la función, coordenadas de la diana% -----------------------------------------------------------------------% SEE ALSO: getpos, position

global POSITIONS R wx wy wwx wwy

state = get(POSITIONS(i),'UserData'); % Recuperar del manejador losdatos tipo Userdatai = state(1);loc = state(2:4);

Anexo A. Programa

210

% -----------------------------------------------------------------------% wwx wwy wx wy R -> variables del fichero de Simulink.% bounds -> fichero simulink% Función de saturación de las coordenadas de la diana xmax = [0+R 130-% R], ylim = [0+R 90-R].% -----------------------------------------------------------------------

temps = 1e-5;wwx = x;wwy = y;R = 0;

% Control límites del terreno de simulación de la diana "i"rk45('bounds',temps,[],[1e-6 temps temps 0 0 2]);

x = wx;y = wy;

loc(1) = x;loc(2) = y;loc(3) = libre;

% Set de la nueva posición x-y de la diana (loc(1) = x loc(2) = y)set(POSITIONS(i),'Position',loc(1:2));

% Guardar datos "UserData" de la diana "i"set(POSITIONS(i),'UserData',[i loc]);

% Vaciar el espacio de trabajo utilizado en esta funciónclear state loc iclear setpos

function [value] = getpos(i,propertyname)

% -----------------------------------------------------------------------% [value] = GETPOSITION(i,'PropertyName')%% Función para el "get" de los datos de posición del setpoint (diana),% desde cualquier otra función.% Esta función realiza el get (coger) de las coordenadas-datos de la% diana, pasando como% parámetros de entrada el índice de la diana "i" (que coincide con el% del Robot) y el% "propertyname" del dato deseado.% propertyname = 'state' propertyname = 'location' (coordenada% x-y, variable libre)% [value] = parámetro de salida.% -----------------------------------------------------------------------% SEE ALSO: position(), setpos(), delpos(), ggras(), gagent()

% Declaración de variables de la funciónglobal POSITIONS

state = get(POSITIONS(i), 'UserData'); % Recuperar del manejador losdatos tipo Userdataloc = state(2:4); % Vector de estado de la diana (coordenadas x-y,variable libre)

Anexo A. Programa

211

propertyname = lower(propertyname);

% Comprobar y pasar el value según el propertyname de entradaif propertyname(1:2) == 'lo' value = loc;elseif propertyname(1:2) == 'st' value = state;end

clear getposclear loc state i propertyname

function [] = delpos(i)

% ----------------------------------------------------------------------% DELETEPOSITION()% DELETEPOSITION(i)%% Borrar el setpoint (diana) de POSITIONS(i), con el índice "i" obtenido% como parámetro de entrada.% ----------------------------------------------------------------------% SEE ALSO: dsensor(), dagent(), dobsta(), position(), setpos(), getpos()


global POSITIONS

% Borrar el patch de datos de la diana "position"if POSITIONS(i)~= 0; state = get(POSITIONS(i), 'UserData'); delete(POSITIONS(i)); % Borrar patch de la diana POSITIONS(i) = 0; % Inicializar manejador del patchend % end if POSITIONS(i)~= 0;

return;

Anexo A. Programa

212

A.5 Funciones Asociadas al Diseño del Robot (Sensores)

function[] = chsize()

% -----------------------------------------------------------------------% CHANGESIZE()%% Función asociada al push button de cambio de tamaño del Robot (pantalla% de diseño).% En esta función se lee el valor introducido en el button tipo edit de% change size, donde se% indica el nuevo radio del Robot.% Debido al cambio de tamaño, es necesario recalcular todas las% distancias de posición de los% sensores.% -----------------------------------------------------------------------% SEE ALSO: chrobot(), chcolobs(), asensor(), dsensor(), chcolsen()


global AGENTS MAX_SENSORS MAX_SIZE MIN_SIZE AGENT_AMPLI num_rosize_ro ch_si_ro ampli x0_ampli...

y0_ampli text_num_sen

% Control del número de Robot que se esta editando num_ro = índice delRobot que se esta editando

a = num_ro; % Asignar el número del Robot

% Función leer la estructura de datos del Robot

[state i self goal confina mast redon graf_sen dades_sen] = strucrob(a);

% Comprobar datos correctos de tamaño

if MAX_SIZE < str2num(get(size_ro,'string')) set(size_ro,'string',MAX_SIZE);

elseif MIN_SIZE > str2num(get(size_ro,'string')) set(size_ro,'string',MIN_SIZE);end;

radius_nuevo = str2num(get(size_ro,'string'));% self(4) guarda el radiusviejo

for j = 1:MAX_SENSORS % Recalcular position distancia de los sensores % Modificar distancia desde el origen hasta el punto inicial de lossensores ya que se ha % modificado el Radius del Robot % dades_sen(i,:) =[distan detec, ang_detec, ang rotate sensor,position dis, position ang.] dades_sen(j,4) = radius_nuevo*(dades_sen(j,4)/self(4));end;

self(4) = radius_nuevo; % Asignar nuevo radio al vector de estado delRobot

Anexo A. Programa

213

% Función dibujo de las nuevas coordenadas del Robot[xydata redon graf_sen] = dibu_a(self(1), self(2), self(3), self(4),redon, graf_sen, dades_sen);

set(AGENTS(a),'XData',xydata(:,1),'YData',xydata(:,2));

% Guardar la estructura de datos del Robotstrucrob(a,i,self,goal,confina,mast,redon,graf_sen);

% Recuperar datos del Robot de la pantalla de diseñostate = get(AGENT_AMPLI, 'UserData');redon = state(1);graf_sen = state(2:MAX_SENSORS+1);

for o=1:MAX_SENSORS dades_sen(o,4) = dades_sen(o,4)*ampli; dades_sen(o,1) = dades_sen(o,1)*ampli;

if dades_sen(o,1) ~= 0 % -------------------------- % Numeración de los sensores % --------------------------

x_ini = x0_ampli + dades_sen(o,4) * cos(dades_sen(o,5) + pi/2); y_ini = y0_ampli + dades_sen(o,4) * sin(dades_sen(o,5) + pi/2);

posx = x_ini + cos(dades_sen(o,3) + pi/2) * dades_sen(o,1) * (0.9); posy = y_ini + sin(dades_sen(o,3) + pi/2) * dades_sen(o,1) * (0.9);

set(text_num_sen(o),'position', [posxposy],'string',num2str(o,1),'visible','on');

end;end;clear x_ini y_ini posy posx

[xydata redon graf_sen] = dibu_a(x0_ampli, y0_ampli, pi/2, ampli*self(4),redon, graf_sen, dades_sen);

set(redon,'visible','on');set(AGENT_AMPLI,'XData',xydata(:,1),'YData',xydata(:,2));

% Guardar datos modificados del Robot de la pantalla de diseño

state = [redon graf_sen];set(AGENT_AMPLI,'UserData',state);

clear global z s1 s2 menu state i self goal confina mast graf_sendades_sen o radius_nuevo

Anexo A. Programa

214

function[] = asensor(n)

% -----------------------------------------------------------------------% AddSENSOR()% AddSENSOR(n)%% Función para crear un nuevo sensor.% Si no existe parámetro de entrada, el índice del sensor se lee del% popup de la pantalla.% Si no se recibe parámetro de entrada, significa que hay crear un nuevo% sensor.% A través del ratón se selecciona el punto del contorno del Robot para% la colocación del% sensor. Dependiendo del punto seleccionado, se realizan una serie de% calculos, para obtener;% la distancia desde la coordenada central del Robot hasta el punto de% origen del sensor, además% de calcular el ángulo existente entre ambas coordenadas. A partir de% estos dos valores,% distancia y ángulo, y con la coordenada central del Robot se puede% calcular facilmente la% coordenada de origen del sensor, para su reresentación gráfica durante% la simulación.%% Si se recibe parámetro de entrada en la función, no se crea un nuevo% sensor, ya que este índice% de entrada, pertenece a un sensor del cual se realiza un Update de% datos.% -----------------------------------------------------------------------% SEE ALSO: dsensor(),editsen(), chcolsen()

global AGENTS AGENT_AMPLI MAX_SENSORS MAX_DIS_SEN MAX_ANG_SEN...MIN_DIS_SEN MIN_ANG_SEN MAX_MIN_ROT num_ro add_sen dis_sen...ang_sen rot_sen size_ro num_sen ampli x0_ampli y0_ampli text2_error...x0_ampli y0_ampli text_num_sen


% Índice del Robot que se esta editandoa = num_ro;

% Función recuperar la estructura de datos del Robot[state i self goal confina mast redon graf_sen dades_sen] = strucrob(a);

% Lectura de los datos introducidas por teclado, correctos ??if (get(dis_sen,'string')=='' | get(rot_sen,'string')=='' |get(ang_sen,'string')=='') % Mensaje de error, faltan datos set(text2_error,'String','Warning: Date Input, is notcorrect','visible','on'); return;end

% Comprobar datos de distancia-ángulos de detección

if MAX_ANG_SEN<str2num(get(ang_sen,'string')) set(ang_sen,'string',MAX_ANG_SEN);

elseif MIN_ANG_SEN>str2num(get(ang_sen,'string')) set(ang_sen,'string',MIN_ANG_SEN);

Anexo A. Programa

215

end;

if MAX_DIS_SEN<str2num(get(dis_sen,'string')) set(dis_sen,'string',MAX_DIS_SEN);

elseif MIN_DIS_SEN>str2num(get(dis_sen,'string')) set(dis_sen,'string',MIN_DIS_SEN);end;

if MAX_MIN_ROT<str2num(get(rot_sen,'string')) set(rot_sen,'string',MAX_MIN_ROT);

elseif -MAX_MIN_ROT>str2num(get(rot_sen,'string')) set(rot_sen,'string',-MAX_MIN_ROT);end;

if nargin == 0 % Si no existe entrada nargin == 0

% Nuevo índice para el nuevo sensor blankspots = find(dades_sen == 0); % Posiciones igual a cero if blankspots n = blankspots(1); else n = length(dades_sen) + 1; end % end if blankspots

% Mensaje de error en caso de superar el número máximo de sensores if n > MAX_SENSORS % Mensaje de error set(text2_error,'String',['Warning: Sensor not created. MAX_SENSORS= ' num2str(MAX_SENSORS)],...

'visible','on'); set(add_sen, 'Value', 0); return; end % end if n > MAX_SENSORS

% Set mouse sensor pts = ginput(1); % Coordenadas, con clic de mouse

% Calcular la distancia y el ángulo respecto a la coordenada central % del Robot de la pantalla de diseño

% Distancia distance = sqrt((x0_ampli-pts(1,1))^2+(y0_ampli-pts(1,2))^2); angle = atan2((pts(1,2)-y0_ampli),(pts(1,1)-x0_ampli)); % Ángulo

% Comprobar que la situación establecida del sensor es correcta

if (distance>(self(4)*1.05*ampli)) | (distance<(self(4)*0.3*ampli)) set(text2_error,'String','Warning: Point may be on the Robot´s

contour','visible','on'); dades_sen(n,1)=0; dades_sen(n,2)=0; dades_sen(n,3)=0; return; end; % end if (distance>(self(4)*1.1*ampli))...

Anexo A. Programa

216

% Al ángulo calculado anteriormente se le resta el ángulo de 90 grados % que corresponde a la orientación del Robot de la ventana de diseño angle = angle - (pi/2);

if angle < 0 angle = (2*pi) + angle; end;

% Controlar el valor del ángulo calculado, adaptandolo a un valor % proporcional a 5º, para conservar una cierta simetria (solo parte % delantera y trasera del Robot)

for j = 1:72 if (j*(pi/36)) >= angle

j = j - 1;angle = (pi/36) * j;break;

end;end;

% Cálculo del punto según el valor del ángulo corregido a un valor % proporcional a 5º

% Parte delantera del Robot if (angle<=(pi/4.5)) | (angle>=((2*pi)-(pi/4.5))) distance = self(4)*cos(pi/4.5)/cos(angle); dades_sen(n,3) = 0;

elseif (angle>=(3*pi/4)) & (angle<=(5*pi/4))% Parte trasera del Robot distance = self(4)*cos(3*pi/4)/cos(angle); dades_sen(n,3) = pi;

else % Sensor colocado en los laterales del Robot

% Recuperar puntos del gráfico Robot de la pantalla de edición xdata = get(AGENT_AMPLI,'XData'); ydata = get(AGENT_AMPLI,'YData');

% Cálculo de la pendiente de la recta y = mx + n (lateral del% Robot).

m = (ydata(4) - ydata(1)) / (xdata(4) - xdata(1));

y1 = pts(1,2); % Coordenada "y" del mouse

if (angle>(pi/4.5)) & (angle<(3*pi/4))

x1 = (m*xdata(1) + y1 - ydata(1))/m;dades_sen(n,3) = pi/2;

elseif (angle > (5*pi/4)) & (angle < ((2*pi)-pi/4.5))m = -1 * m;x1 = (m * xdata(2) + y1 - ydata(2)) / m;dades_sen(n,3) = (3 * pi / 2);

end; % end if (angle > (pi/4.5)) & (angle < (3*pi/4))

% Distancia distance = sqrt((x0_ampli - x1)^2+(y0_ampli - y1)^2)/ampli;

angle = atan2((y1 - y0_ampli),(x1 - x0_ampli));% Ángulo

Anexo A. Programa

217

% Al ángulo calculado anteriormente se le resta el ángulo de 90% grados que corresponde a la orientación del Robot de la ventana% de diseño

angle = angle-(pi/2);

if angle < 0angle = (2*pi) + angle;

end;end;

% Guardar distancia y ángulo de posición en la matriz de datos del % Robot dades_sen(n,4) = distance; dades_sen(n,5) = angle;

else % else (if nargin == 0) Datos para Update del sensor

dades_sen(n,3) = 0;

if (dades_sen(n,5) >= (3*pi/4)) & (dades_sen(n,5) <= (5*pi/4)) dades_sen(n,3) = pi;

elseif (dades_sen(n,5) > (pi/4.5)) & (dades_sen(n,5) < (3*pi/4)) dades_sen(n,3) = pi/2;

elseif (dades_sen(n,5) > (5*pi/4)) & (dades_sen(n,5) < ((2*pi)-pi/4.5))

dades_sen(n,3) = (3*pi/2);

end; % end if (dades_sen(n,5)> = (3*pi/4)) ...

end; % end if nargin == 0

% Lectura button edit distancia deteccióndades_sen(n,1) = str2num(get(dis_sen,'string'));% Lectura button edit ángulo deteccióndades_sen(n,2) = str2num(get(ang_sen,'string'))*pi/180;dades_sen(n,3) = dades_sen(n,3) + str2num(get(rot_sen,'string')) *pi/180; % Lectura button edit ángulo rotación

% Dibujar Robot y sensor creado. Renovar dibujo del Robot en pantalla[xydata redon graf_sen] = dibu_a(self(1), self(2), self(3), self(4),redon,graf_sen, dades_sen);


% Función escribir la estructura de datos del Robot

strucrob(a,i,self,goal,confina,mast,redon,graf_sen);

% ------------------------------------------------------% Dibujar Robot y sensor creado en la pantalla de diseño% ------------------------------------------------------

for j=1:MAX_SENSORS dades_sen(j,4) = dades_sen(j,4) * ampli; % Amplificación de lasdistancias para los datos dades_sen(j,1) = dades_sen(j,1) * ampli; % del Robot de lapantalla de diseñoend;

Anexo A. Programa

218

state = get(AGENT_AMPLI, 'UserData');redon = state(1);graf_sen = state(2:MAX_SENSORS+1);

[xydata redon graf_sen] = dibu_a(x0_ampli,y0_ampli,(pi/2),ampli *self(4),redon,graf_sen,dades_sen);

% --------------------------% Numeración de los sensores% --------------------------

x_ini = x0_ampli + dades_sen(n,4) * cos(dades_sen(n,5) + pi/2);y_ini = y0_ampli + dades_sen(n,4) * sin(dades_sen(n,5) + pi/2);

posx = x_ini + cos(dades_sen(n,3) + pi/2) * dades_sen(n,1) * (0.9);posy = y_ini + sin(dades_sen(n,3) + pi/2) * dades_sen(n,1) * (0.9);

set(text_num_sen(n),'position', [posxposy],'string',num2str(n,1),'visible','on');

clear x_ini y_ini posy posx

set(redon,'visible','on'); % Circulo espacio vital Robot pantalla dediseño

set(AGENT_AMPLI,'XData',xydata(:,1),'YData',xydata(:,2));

% Guardar datos del Robot en la pantalla de diseñostate = [redon graf_sen];set(AGENT_AMPLI,'UserData',state);

% Función renovar índices sensores en el popup de la pantalla de diseñopopup(dades_sen(:,1),num_sen,0);

clear menu s x1 y1 z xydata distance angle m state graf_sen a i self goalconfina mast

function[] = chcolsen(i)

% -----------------------------------------------------------------------% CHANGECOLORSENSOR()%% Función asociada al popup de índices de los sensores en la pantalla de% diseño del Robot.% Esta función cambia el color del sensor "i" seleccionado, y además% renueva los parámetros en% los buttons tipo edit de la pantalla de diseño del Robot ("i" sensor% seleccionado en el popup).% -----------------------------------------------------------------------% SEE ALSO: chrobot(), chsize(), chcolobs(), asensor(), dsensor()

global MAX_SENSORS AGENT_AMPLI num_sen rot_sen ampli dis_sen ang_sentext2_error

Anexo A. Programa

219

% Recuperar estructura de datos del Robot de la pantalla de diseñostate = get(AGENT_AMPLI, 'UserData');redon = state(1);graf_sen = state(2:MAX_SENSORS+1);

for j = 1:MAX_SENSORS dades_sen(j,:) = get(graf_sen(j),'UserData');end; % end for j=1:MAX_SENSORS

if nargin == 0 % Si no hay índice de entrada % Índice sensor seleccionado en el popup i = popup(dades_sen(:,1),num_sen,1,text2_error,'Warning: Not exist

sensor'); if i == 0 return;end;end;

% Inicializar el color de todos los sensores a verdefor index = 1:MAX_SENSORS if index == i % Índice seleccionado en el popup

% Cambiar color sensor "i" seleccionado en el popup set(graf_sen(index),'color','m'); else set(graf_sen(index),'color','g'); % Color sensor a verde end; % end if index == iend; % end for index = 1:MAX_SENSORS

% Set de los parámetros del sensor "i" en los buttons tipo edit

clear global index state o menu

% Set distancia de detección del sensor 'i'set(dis_sen,'string',dades_sen(i,1)/ampli);% Set ángulo de detección del sensor 'i'set(ang_sen,'string',dades_sen(i,2)*180/pi);

% Set del ángulo de rotación del sensor "i" en gradosif (dades_sen(i,5)>=(3*pi/4)) & (dades_sen(i,5)<=(5*pi/4)) set(rot_sen,'string',(dades_sen(i,3)-pi)*180/pi); return; % Parte trasera del Robotelseif (dades_sen(i,5)>(pi/4.5)) & (dades_sen(i,5)<(3*pi/4)) set(rot_sen,'string',(dades_sen(i,3)-(pi/2))*180/pi); return; % Lateral del Robotelseif (dades_sen(i,5)>(5*pi/4)) & (dades_sen(i,5)<((2*pi)-pi/4.5)) set(rot_sen,'string',(dades_sen(i,3)-(3*pi/2))*180/pi); return; % Lateral del Robotend; set(rot_sen,'string',dades_sen(i,3)*180/pi); return; % Parte delantera del Robot

Anexo A. Programa

220

function[] = dsensor(i)

% -----------------------------------------------------------------------% DELETESENSOR()% DELETESENSOR(i)%% Función para borrar un determinado sensor del Robot (pantalla de% diseño). El índice del sensor% se lee del popup de la Pantalla de Diseño o se obtiene como parámetro% de entrada.%% Para eliminar el sensor, se inicializa el objeto "line" y se borran los% datos de la matriz de% datos.% dades_sen(i,:) = [0 0 0 0 0] "i" índice del sensor% -----------------------------------------------------------------------% SEE ALSO: dagent(), dobsta(), delpos()

% Declaración de variables de la funciónglobal AGENTS MAX_SENSORS AGENT_AMPLI num_ro num_sen del_sentext2_error text_num_sen


% Índice del Robot que se esta editandoa = num_ro;

% Leer la estructura de datos del Robot[state j self goal confina mast redon graf_sen dades_sen] = strucrob(a);

if nargin == 0

% Leer índice del sensor del popup i = popup(dades_sen(:,1),num_sen,1,text2_error,'Warning: Not existsensor');

if i == 0 return;end;end; % end if nargin == 0

dades_sen(i,:) = [0 0 0 0 0]; % Borrar datos sensor "i"

% Inicializar "line" gráfico sensor "i", y el dato de "UserData"set(graf_sen(i),'XData',0,'YData',0, 'UserData',dades_sen(i,:));

% Guardar la estructura de datos del Robotstrucrob(a,j,self,goal,confina,mast,redon,graf_sen);

% Borrar sensor de la pantalla de diseñostate = get(AGENT_AMPLI,'UserData');redon = state(1);graf_sen = state(2:MAX_SENSORS+1);

set(graf_sen(i),'XData',0,'YData',0,'UserData',dades_sen(i,:));

state = [redon graf_sen];% Guardar la estructura de datos del Robotset(AGENT_AMPLI, 'UserData', state);

Anexo A. Programa

221

if nargin == 0 % Función renovar los índices de los sensores en el popup de la % pantalla popup(dades_sen(:,1),num_sen,0);

end % end if nargin == 0

set(text_num_sen(i),'string','','visible','off');

clear global index s menu a i self goal confina graf_sen

function[] = UpDatesensor()

% ------------------------------------------------------------------% UPDATESENSOR()%% Función asociada al push button Update de datos del sensor (Update% parámetros del sensor).% Se obtiene el índice del sensor y realiza la llamada a la función% asensor(i) para el Update% de los nuevos datos del sensor.% ------------------------------------------------------------------% SEE ALSO: sobstak(), asensor(), dsensor(), chcolsen()

% Declaración de variables de la funciónglobal num_sen edit_sen text2_error WORLD_AXIS2


if get(num_sen,'String')=='' set(edit_sen,'Value', 0); set(text2_error,'String','Warning: Not existSensor','visible','on','color','r'); return;end;

% Leer índice del sensori = str2num(get(num_sen,'String')); % [sen1 sen2 ... sen"n"]i = i(get(num_sen,'Value')); % i = i(n);

% Llamada a la función de crear nuevo sensor, pasando como parámetro de% entrada el índice del sensor "i", para Update de los datos modificados,% sin necesidad de crear un nuevo sensor.

axes(WORLD_AXIS2);asensor(i);

clear global i

Anexo A. Programa

222

A.6 Funciones Asociadas al Tratamiento de los Ficheros

function[file] = filecfg(op)

% -----------------------------------------------------------------------% FILECONFIG(op)%% Función asociada al label de la barra de menu de la pantalla de diseño,% para salvar o recuperar los datos referentes a la configuración del% Robot, tamaño, sensores y fichero simulink.% Dependiendo del parámetro de entrada, se realiza la operación de Save o% de Load.% op = 1-2 --- Save% op = 3-4 --- Load%% El nombre del fichero se obtiene de una caja de dialogo% uigetfile...(Load) o uiputfile...(Save).% Una vez obtenido el nombre del fichero, se abre en el modo de operación% necesario, ya sea escribir para Save o en modo lectura para Load.% El fichero debe de conservar un cierto formato para ser recuperado sin% problemas (observar un% fichero de datos de ejemplo).%% Los ficheros de datos, deben de estar en el mismo directorio raiz que% el programa de simulación.%% Extensión de los ficheros *.cfg% -----------------------------------------------------------------------% SEE ALSO: filedata(), fileobst()

% Declaración de variables de la funciónglobal MAX_SENSORS AGENTS save num_ro nafi_con text2_errorra_fi_con1 maes_fi

index=1;file=0;

if op > 0 & op < 5 % Opción de Save-Load fichero de config

% Índice del Robot que se esta editando a = num_ro; % Asignar el número del Robot

% Función recuperar la estructura de datos del Robot [state i self goal confina mast redon graf_sen dades_sen] =strucrob(a);

if op == 1 | op == 2 % Opción de Save fichero de config

% Convertir en grados para guardar en el fichero for j=1:MAX_SENSORS

dades_sen(j,:) = get(graf_sen(j),'UserData');dades_sen(j,2) = dades_sen(j,2) * (180/pi);dades_sen(j,3) = dades_sen(j,3) * (180/pi);dades_sen(j,5) = dades_sen(j,5) * (180/pi);

end;

% Uiputfile, leer nombre del fichero de cfg para Save [namefile, pathname] = uiputfile('*.cfg','Save Config Robot');

Anexo A. Programa

223

if namefile==0 % Si se cancela o existe error, salirreturn;

end;

pathname = lower(pathname); % Convertir en minúsculas file = lower(namefile);

fid = fopen(file,'w'); % Abrir fichero para escribir

if op>=1 % Save dato de Radius del Robot y datos de los sensoresfprintf(fid,'Radius (Size) of the Robot\n'); % Save dato de

Radius del Robotfprintf(fid,'%g\n',self(4));

fprintf(fid,'MAX_SENSORS\n'); % Save dato de Radius del Robotfprintf(fid,'%g\n',MAX_SENSORS);

fprintf(fid,'dades_sen(:,:)\n');% Save datos de los sensoresfor j=1:MAX_SENSORS fprintf(fid,'%g %g %g %g %g\n',dades_sen(j,:));

end; end; % end if op>=1

if op==2 % Save nombre del fichero para el controlfprintf(fid,'Sensors File Name\n');% Nombre fichero en string de la pantalla de ediciónset(mast,'UserData',get(maes_fi,'string'));fprintf(fid,get(mast, 'UserData'));fprintf(fid,'\n');

fprintf(fid,'Simulink Control File\n');% Nombre fichero en string de la pantalla de ediciónset(confina,'UserData',get(nafi_con,'string'));fprintf(fid,get(confina, 'UserData'));fprintf(fid,'\n');fprintf(fid,'%g',get(ra_fi_con1,'value'));

end % end if op==2


% Mensaje del nombre del fichero leído. Mensaje de Save fichero set(text2_error,'String',['Name of the file for Save is: 'file],'visible','on');

clear global index file

return; % Fin

end % end if op ==1 | ...

if op >= 3 % Opción de Load fichero de config

% Uigetfile, leer nombre del fichero para Load

[namefile, pathname] = uigetfile('*.cfg','Load Config Robot');

if namefile == 0 % Si se cancela o existe error, salirreturn;

end;

pathname = lower(pathname); % Convertir en minúsculas

Anexo A. Programa

224

file = lower(namefile);

fid = fopen(file,'r'); % Abrir fichero para lectura


while ~(feof(fid)) & lectura~=''% Leer mientras no final


if strcmp(lectura,'radius (size) of the robot') & op>=3 % Lectura del Radius self(4) = str2num(fgetl(fid));end;

if strcmp(lectura,'max_sensors') & op>=3 % Lectura del MAX_SENSORS MX_SEN = str2num(fgetl(fid)); % Leer como máximo un total de MAX_SENSORS sensores if MX_SEN > MAX_SENSORS MX_SEN = MAX_SENSORS;end;end;

if strcmp(lectura,'dades_sen(:,:)') & op>=3 % Lectura datos de sensores

for j = 1:MAX_SENSORS % Borrar sensores existentes dsensor(j); dades_sen(j,:) = [0 0 0 0 0]; end;

lectura = fscanf(fid,'%g',[5 MX_SEN]); fprintf('\n\n La matriz de datos de los sensores leída es:\n'); da_sen = lectura' % Lectura perdida para avanzar carro en el fichero lectura = fgetl(fid);end;

if strcmp(lectura,'sensors file name') & op == 4 % Lectura del nombre del fichero de control lectura = fgetl(fid); set(maes_fi,'string',lectura);end;

if strcmp(lectura,'simulink control file') & op == 4 % Lectura del nombre del fichero de control lectura = fgetl(fid); set(nafi_con,'string',lectura); lectura = fgetl(fid); set(ra_fi_con1,'value',str2num(lectura));end;


end; % end while ~feof(fid)


Anexo A. Programa

225

% Convertir en radianes para guardar en la matriz de los sensores for j = 1:MX_SEN

dades_sen(j,:) = da_sen(j,:);dades_sen(j,2) = dades_sen(j,2) * (pi/180);dades_sen(j,3) = dades_sen(j,3) * (pi/180);dades_sen(j,5) = dades_sen(j,5) * (pi/180);

end;

[xydata redon graf_sen] =dibu_a(self(1),self(2),self(3),self(4),redon,graf_sen,dades_sen);


% Función escribir la estructura de datos del Robot strucrob(a,i,self,goal,confina,mast,redon,graf_sen);

error = quit1(1); % Cerrar pantalla de edición del Robot

if ~error % Si no existe error al cerrar la pantallaedirobot(a); % Llamada a la pantalla de Diseño para renovarla

con los datos leídos% Mensaje del nombre del fichero leído. Mensaje de Load ficheroset(text2_error,'String',['Name of the file for Load is: '

file],'visible','on'); end;

end; % end op>=3end; % end if op == 0

function[file] = opensimu()

% -----------------------------------------------------------------------% OPENSIMULINK()%% Función asociada al push-button Edit (Control File) de la pantalla de% diseño.%% Función para abrir el fichero de control simulink en caso de querer% editarlo desde la pantalla% de diseño del Robot. El nombre del fichero se recupera a partir de una% caja de dialogo uigetfile% para después editarlo con la función de aplicación eval('nombre del% archivo').% -----------------------------------------------------------------------% SEE ALSO: filecfg(), fileobst()

% Declaración de variables de la funciónglobal nafi_con text2_error

index=1;file=0;

% Uigetfile, leer nombre del fichero de control[namefile, pathname] = uigetfile('*.m','File Control for Open');

if namefile == 0 % Comprobar lectura correcta file = 'defaultc'; % Por defecto se devuelve el file "defaultc" set(nafi_con,'string',file); return;end;

Anexo A. Programa

226

pathname = lower(pathname); % Convertir en minúsculasnamefile = lower(namefile);

while namefile(index)~='.' % Eliminar extensión file(index) = namefile(index); index = index + 1;end; % end while namefile(index)~='.'

set(nafi_con,'string',file); % Colocar nombre en string de pantallaeval(file); % Llamar-abrir fichero de Simulinkclear global indexreturn;

function [] = filedata(op)

% -----------------------------------------------------------------------% FILEDATA(op)%% Función asociada al label de la barra de menu de la pantalla principal% de simulación, para salvar o recuperar los resultados de la simulación% como velocidad, intensidad, par, etc...% Dependiendo del parámetro de entrada, se realiza la operación de Save o% de Load.% op = 1 --- Save% op = 2 --- Load%% El nombre del fichero se obtiene de una caja de diálogo% uigetfile...(Load) o uiputfile...(Save).%% Una vez obtenido el nombre del fichero, se abre en el modo de operación% requerido, ya sea% escribir para Save o en modo lectura para Load.%% En la recuperación de datos (Load), se realiza el set de las% coordenasdas del Robot recuperadas% del fichero.%% Los ficheros de datos deben de estar en el mismo directorio raiz que el% programa de simulación.%% La extensión de los ficheros de datos es por defecto *.dat% -----------------------------------------------------------------------% SEE ALSO: filecfg(), fileobst()

% Declaración de variables de la funciónglobal AGENTS plotButton agNum dt text1_error x_ag y_ag log_vlinlog_vang logv_m_d logi_m_d grasping tracking forward other logv_m_ilogi_m_i logp_m_d logp_m_i dt_text


% Leer el índice del Robot de la pantallai = popup(AGENTS,agNum,1,text1_error,'Warning: Not exist Robot');

Anexo A. Programa

227

if i == 0 set(plotButton, 'Value', 0); return;end

% Comprobar si hay datos para realizar gráficaif (length(log_vlin(i,:)) == 1 & op~=2) % Mensaje de error, no existen datos a salvar set(text1_error,'String','Warning: Not exist data','visible','on'); return; % En este caso no hay datosend;

if op == 1 % Opción abrir-Save fichero datos-resultados

% Uiputfile, leer nombre del fichero de datos para Save [namefile, pathname] = uiputfile('*.dat','Save Results');

if namefile == 0 return; end;



% Iniciar la escritura de datos en el fichero

fprintf(fid,'Grain Time\n'); % Save dato nº de elementos de cadavector de datos fprintf(fid,'%g\n',dt);

fprintf(fid,'nº_elementos\n');% Save dato nº de elementos de cadavector de datos fprintf(fid,'%g\n',length(x_ag(i,:)));

fprintf(fid,'x_ag\n'); % Save dato de x_ag del Robot fprintf(fid,'%g\n',x_ag(i,:));

fprintf(fid,'y_ag\n'); % Save dato de y_ag del Robot fprintf(fid,'%g\n',y_ag(i,:));

fprintf(fid,'log_vlin\n'); % Save dato de log_vlin del Robot fprintf(fid,'%g\n',log_vlin(i,:));

fprintf(fid,'log_vang\n'); % Save dato de log_vang del Robot fprintf(fid,'%g\n',log_vang(i,:));

fprintf(fid,'logv_m_d\n'); % Save dato de logv_m_d del Robot fprintf(fid,'%g\n',logv_m_d(i,:));

fprintf(fid,'logi_m_d\n'); % Save dato de logi_m_d del Robot fprintf(fid,'%g\n',logi_m_d(i,:));

fprintf(fid,'logv_m_i\n'); % Save dato de logv_m_i del Robot fprintf(fid,'%g\n',logv_m_i(i,:));

fprintf(fid,'logi_m_i\n'); % Save dato de logi_m_i del Robot fprintf(fid,'%g\n',logi_m_i(i,:));

Anexo A. Programa

228

fprintf(fid,'logp_m_d\n'); % Save dato de logp_m_d del Robot fprintf(fid,'%g\n',logp_m_d(i,:));

fprintf(fid,'logp_m_i\n'); % Save dato de logp_m_i del Robot fprintf(fid,'%g\n',logp_m_i(i,:));

fprintf(fid,'Tipo_movimiento\n'); % Salvar tipo de movimiento

if get(grasping,'value') % Movimiento tipo Grasping fprintf(fid,'grasping\n'); fprintf(fid,'selfobj\n');

% selfobj = [xobj yobj theta radius remocaldo] selfobj = ggras(i,'Self'); fprintf(fid,'%g\n',selfobj);

elseif get(tracking,'value') % Movimiento tipo Tracking fprintf(fid,'tracking\n');

elseif get(forward,'value') % Movimiento tipo Forward fprintf(fid,'forward\n');

elseif get(other,'value') % Movimiento tipo Other fprintf(fid,'other\n');

end;


fprintf(fid,'Self\n'); % Save vector de estado del Robot fprintf(fid,'%g\n',self); fprintf(fid,'\n');



return; % Salir de la función Save Data

elseif op == 2 % Opción abrir-Load fichero datos-resultados


% Uigetfile, leer nombre del fichero de datos para Load [namefile, pathname] = uigetfile('*.dat','Load Results');


% Mensaje de Load fichero, mensaje de espera set(text1_error,'String','Loading File ! Wait a moment', 'visible','on');

pathname = lower(pathname); % Convertir en minúsculas

Anexo A. Programa

229

file = lower(namefile); % Inicializar-limpiar variables antes del LOAD. Borrar datos % almacenados hasta el momento limpiar(i);


lectura = fgetl(fid); while ~(feof(fid)) & lectura~='' % Leer mientras no final


if strcmp(lectura,'grain time') % Lectura Grain Timedt = str2num(fgetl(fid));

end; % end if strcmp(lectura,'nº_elementos')

% Lectura nº elementos de los vectores if strcmp(lectura,'nº_elementos')

n_ele = str2num(fgetl(fid)); end; % end if strcmp(lectura,'nº_elementos')

if strcmp(lectura,'x_ag') % Lectura de coordenada x Robotlectura = fscanf(fid,'%g',[n_ele 1]);x_ag(i,1:n_ele) = lectura';% Lectura perdida para avanzar en el ficherolectura = fgetl(fid);

end; % end if strcmp(lectura,'x_ag')

if strcmp(lectura,'y_ag') % Lectura de coordenada y Robotlectura = fscanf(fid,'%g',[n_ele 1]);y_ag(i,1:n_ele) = lectura';% Lectura perdida para avanzar en el ficherolectura = fgetl(fid);

end; % end if strcmp(lectura,'y_ag')

if strcmp(lectura,'log_vlin') % Lectura Veloci. lineal Robotlectura = fscanf(fid,'%g',[n_ele 1]);log_vlin(i,1:n_ele) = lectura';% Lectura perdida para avanzar en el ficherolectura = fgetl(fid);

end; % end if strcmp(lectura,'log_vlin')

if strcmp(lectura,'log_vang') % Lectura Veloci. angular Robotlectura = fscanf(fid,'%g',[n_ele 1]);log_vang(i,1:n_ele) = lectura';% Lectura perdida para avanzar en el ficherolectura = fgetl(fid);

end; % end if strcmp(lectura,'log_vang')

if strcmp(lectura,'logv_m_d') % Lectura Veloci. motor derecha Robotlectura = fscanf(fid,'%g',[n_ele 1]);logv_m_d(i,1:n_ele) = lectura';% Lectura perdida para avanzar en el ficherolectura = fgetl(fid);

end; % end if strcmp(lectura,'logv_m_d')

Anexo A. Programa

230

% Lectura Veloci. motor izquierda Robot if strcmp(lectura,'logv_m_i')

lectura = fscanf(fid,'%g',[n_ele 1]);logv_m_i(i,1:n_ele) = lectura';% Lectura perdida para avanzar en el ficherolectura = fgetl(fid);

end; % end if strcmp(lectura,'logv_m_i')

% Lectura intensidad motor derecho Robot if strcmp(lectura,'logi_m_d')

lectura = fscanf(fid,'%g',[n_ele 1]);logi_m_d(i,1:n_ele) = lectura';% Lectura perdida para avanzar en el ficherolectura = fgetl(fid);

end; % end if strcmp(lectura,'logi_m_d')

% Lectura intensidad motor izquierdo Robot if strcmp(lectura,'logi_m_i')

lectura = fscanf(fid,'%g',[n_ele 1]);logi_m_i(i,1:n_ele) = lectura';% Lectura perdida para avanzar en el ficherolectura = fgetl(fid);

end; % end if strcmp(lectura,'logi_m_i')

if strcmp(lectura,'logp_m_d') % Lectura Par motor derecho Robotlectura = fscanf(fid,'%g',[n_ele 1]);logp_m_d(i,1:n_ele) = lectura';% Lectura perdida para avanzar en el ficherolectura = fgetl(fid);

end; % end if strcmp(lectura,'logp_m_d')

if strcmp(lectura,'logp_m_i') % Lectura Par motor izquierdo Robotlectura = fscanf(fid,'%g',[n_ele 1]);logp_m_i(i,1:n_ele) = lectura';% Lectura perdida para avanzar en el ficherolectura = fgetl(fid);

end; % end if strcmp(lectura,'logp_m_i')

if strcmp(lectura,'tipo_movimiento') % Lectura Tipo movimientolectura = fgetl(fid);lectura2 = lower(lectura); % Convertir en minúsculas

% Lectura vector estado objeto grasping (inicializar)if strcmp(lectura2,'grasping') lectura = fgetl(fid);

% selfobj = [xobj yobj theta radius remocaldo] lectura=fscanf(fid,'%g',[5 1]); selfobj = lectura';

% Lectura perdida para avanzar en el fichero lectura = fgetl(fid); lectura = lower(lectura); % Convertir en minúsculasend; % end if strcmp(lectura2,'grasping')

end;

% Lectura datos característicos del Robot if strcmp(lectura,'self')

% self = [coordenada"x" coordenada"y" theta radius velolineal

Anexo A. Programa

231

% veloangular pertur]lectura=fscanf(fid,'%g',[7 1]);self = lectura';

end; % end if strcmp(lectura,'self')




% Mensaje del nombre del fichero leído. Mensaje de Load fichero set(text1_error,'String',['Name of the file for Load is: ' file],'visible', 'on');

% Evitar superposición Robot con otros elementos de la simulación error = noencima(self,i,'r');

if error % Superposición, mensaje de error stop = 1; set(text1_error,'String',['Warning: Superposition Robot nº 'num2str(i) ' Coordinates x = ' num2str(self(1)) ' y = ' num2str(self(2))],'visible','on'); % Mensaje de error else

% Set Robot de los datos carac. leídos en el fichero sagent(i,'Self',self); end; % end if error

clear global dt_text

% Set del nuevo grain time leído del fichero (Load) global dt_text dt_edit dt_text = uicontrol('Visible','on','Position', [.03 .83 .245 .03],...'HorizontalAlignment', 'Left', 'Style', 'text', 'Units', 'normalized',...'String',['Total Grain Time (GT = ' num2str(dt*1000,2) ' ms) = '], ...'BackgroundColor','y', 'Callback',''); % Recordar que dt = Grain Time simulación dt_total = n_ele * dt; % Total Grain Time set(dt_edit,'string',[num2str(dt_total,4) ' s']); % Lectura movimiento grasping (inicializar) if strcmp(lectura2,'grasping') fungras; sgras(i,'Self',selfobj); if selfobj(5) == 1 % Si objeto "grasping" transportado

MAR_FINAL = ggras(i,'MAR_FINAL');set(MAR_FINAL,'visible','on'); % visulalizar destino

end; % Lectura movimiento tracking (inicializar) elseif strcmp(lectura2,'tracking') funtrac; % Lectura movimiento other (inicializar) elseif strcmp(lectura2,'other') % Lectura movimiento other (inicializar) funother; % Lectura movimiento forward (inicializar) elseif strcmp(lectura2,'forward') funforw; end;

end; % end if op == 1

Anexo A. Programa

232

function[file] = fileobst(op)

% -----------------------------------------------------------------------% [file] = FILEOBSTÁCULO(op)%% Función asociada al label de la barra de menu de la pantalla de diseño% de obstáculos, para% salvar o recuperar los obstáculos.% Dependiendo del parámetro de entrada, se realiza la operación de Save o% de Load.% op = 1 --- Save% op = 2,3,4,5 --- Load%% El nombre del fichero se obtiene de una caja de diálogo% uigetfile...(Load) o uiputfile...(Save).% Una vez obtenido el nombre del fichero, se abre en el modo de operación% requerido, ya sea% escribir para Save o en modo lectura para Load.% Los ficheros de datos deben de estar en el mismo directorio raiz que el% programa de simulación.%% Extensión de los ficheros de datos referentes a los obstáculos *.obs.% -----------------------------------------------------------------------% SEE ALSO: filedata(), filecfg()

% Declaración de variables de la funciónglobal OBSTACLES OBSTACLES2 MAX_OBSTACLES obsNum save2 load2 quit2AGENTS text2_error


file = 0;

if op == 1 % Opción de Save fichero de Obstáculos

% Uiputfile, leer nombre del fichero de obs para Save [namefile, pathname] = uiputfile('*.obs','Save Config Obstacles');




% Iniciar la escritura de datos en el fichero for index = 1:MAX_OBSTACLES state = get(OBSTACLES(index), 'UserData'); i = state(1); % selfobs = [coordenada"x" coordenada"y" theta radius shape] selfobs = state(2:6);

if i~=0 % Existe patch OBSTACLES ?

fprintf(fid,'Indice Obstáculo \n'); % Save dato de Indice-númerodel Obstáculo

fprintf(fid,'%g\n',i);

Anexo A. Programa

233

fprintf(fid,'Vector de datos Obstáculo \n'); % Save Vector dedatos Obstáculo

% selfobs = [coordenada"x" coordenada"y" theta radius shape]fprintf(fid,'%g %g %g %g

%g\n',selfobs(1),selfobs(2),selfobs(3)*180/pi,selfobs(4),...selfobs(5));

end; % end if i~=0 end; % end for index=1:MAX_OBSTACLES



clear global index state i fid selfobs xdata ydata return; % Fin

elseif op == 2 % Opción de Load fichero de Obstáculos

global namefile pathname

% Uigetfile, leer nombre del fichero para Load [namefile, pathname] = uigetfile('*.obs','Load Config Obstacles');

if namefile==0 return; end;

if length(OBSTACLES2) ~= 0 % Si existen obstáculo, preguntar !!reemplazar ? avislo('Do you want replace current obstacles?','Yes','No','Cancel'); else % No existen obstáculos, recuperar fichero directamente fileobst(5); end;

elseif op > 2 % Opción de Load fichero de Obstáculos

global namefile pathname aviso

if op ~= 5 % Existen obstáculos (presentada pantalla dereemplazar) close(aviso); % Cerrar pantalla de reemplazar if op == 6 % Pulsado botón de Cancel (pantalla reemplazar ?) return; % Cancelar operación de Load end;end;


clear global namefile pathname



salir = 0;

Anexo A. Programa

234

while ~(feof(fid)) & lectura~='' & salir == 0 % Leer mientras nofinal

lectura=lower(lectura); % Convertir en minúsculas

if lectura(1:2)=='in' % Lectura del Indice-número del Obstáculo i = str2num(fgetl(fid)); lectura = fgetl(fid); if i > MAX_OBSTACLES salir = 1; end; lectura = lower(lectura); % Convertir en minúsculas

if lectura(1:2)=='ve' & salir == 0 % Lectura datos de losobstáculos

lectura = fscanf(fid,'%g',[5 1]); selfo = lectura'; % selfo = [coordenada"x" coordenada"y" theta radius shape] selfo(3) = selfo(3)*pi/180; % De grados a radianes

[xdata ydata] = dibu_o(selfo(1), selfo(2), selfo(3),selfo(4), selfo(5));

if op == 4 % Si no reemplazar obstáculos. Mirar obstáculos noexistentes

for index=1:length(OBSTACLES) state = get(OBSTACLES(index), 'UserData'); i = state(1); % Índice del obstáculo if i == 0 % NO existe PATCH OBSTACLES i = index; % Índice no ocupado break;end;end;if i == MAX_OBSTACLES salir = 1;

end;end; % end if op == 4

% Guardar datos del Obstáculo set(OBSTACLES(i),'XData',xdata,'YData',ydata); state = [i selfo]; set(OBSTACLES(i),'UserData',state);

% Evitar superposición obstáculo con otros elementos de lasimulación

[error j]= noencima(selfo,i,'o');

if error % Superposición, mensaje de error

if op == 4 % Si no reemplazar obstáculos. set(OBSTACLES(i),'UserData',[0 [0 0 0 0 0]]);% UserData

inicializado. set(OBSTACLES(i),'XData',0,'YData',0); % Inicializar patch,

pero no borrarlo!!! else dobsta(j); end;

%fclose(fid); % Cerrar fichero para Load. %quit1(2); % Cerrar pantalla de edición de obstáculos. %editobst; % Llamada a la pantalla de edición para

renovarla con los datos leídos.

Anexo A. Programa

235

%dobsta(i);

%set(text2_error,'String',['Warning: SuperpositionObstacle nº ' num2str(i) ' Coordinates x = ' num2str(selfo(1)) ' y = 'num2str(selfo(2)) ],...

%'visible','on'); % Mensaje de error %return;

end; % end if error

lectura = fgetl(fid); % Lectura perdida para avanzar elcarro

end; % end if lectura(1:2)=='ve'end; % end if lectura(1:2)=='in'

lectura = fgetl(fid); end; % end while ~feof(fid)

fclose(fid); % Cerrar fichero para Load.

quit1(2); % Cerrar pantalla de edición de obstáculos.

editobst; % Llamada a la pantalla de edición para renovarla conlos datos leídos.

if salir == 1 set(text2_error,'String',['Warning: Obstacle not created.MAX_OBSTACLES = ' num2str(MAX_OBSTACLES)],...

'visible','on'); % Mensaje de error pause(3); end;

% Mensaje del nombre del fichero leído. Mensaje de Load fichero set(text2_error,'String',['Name of the file for Load is: 'file],'visible','on');

clear index state i fid selfo xdata ydata salir

end; % end if op==1 final de save obstáculos

function [] = avislo(txt1,txt2,txt3,txt4)

% -----------------------------------------------------------------------% [] = AVISLOAD((txt1,txt2,txt3)%% Función de presentación pantalla de aviso para cargar los obstáculos% desde un fichero,% reemplazando los existentes.% txt1 = Mensaje de aviso% txt2 = mensaje push-button 1% txt3 = mensaje push-button 2% txt4 = mensaje push-button 3% -----------------------------------------------------------------------global aviso DelAll butSI butNO

col = [0.5 0.5 0.5];

Anexo A. Programa

236

aviso = figure('name','Load ConfigObstacles','color',col,'menubar','none','NumberTitle','off',...

'Units','normalized','Position', [0.35 0.45 0.30.15],'interruptible','yes');

txt_avis = uicontrol('Style','text','Units','normalized','Position',[00.62 1 0.2],...

'interruptible','yes', 'HorizontalAlignment','Center',...'BackgroundColor',col,'String',txt1, 'visible','on');

butSI = uicontrol('Style','Push','Units','normalized','Position',[0.080.25 0.23 0.2],...

'interruptible','yes','HorizontalAlignment','Center',...'String',txt2,'Callback','fileobst(3);');

butNO = uicontrol('Style','Push','Units','normalized','Position',[0.380.25 0.23 0.2],...

'interruptible','yes','HorizontalAlignment','Center','String',txt3,...

'Callback','fileobst(4);');

butCAN = uicontrol('Style','Push','Units','normalized','Position',[0.680.25 0.23 0.2],...

'interruptible','Yes','HorizontalAlignment','Center','String',txt4,...

'Callback','fileobst(6);');

clear butCAN butNO butSI txt_avis txt1 txt2 txt3 txt4 col

Anexo A. Programa

237

A.8 Funciones Asociadas al Tratamiento de Datos de los Obstáculos

function [n] = obsta(n,x,y,theta,radius,shape)

% -----------------------------------------------------------------------% n = OBSTA;% n = OBSTA(x,y,theta)% n = OBSTA(x,y,theta,radius)%% Función para crear un nuevo Obstáculo.%% Un Obstáculo es un tipo especial de "patch", donde se le define una% estructura de datos en modo% de "UserData". El patch no deja de ser un objeto-dibujo (con un espacio% de memoria asignado)% bidimensional, con una estructura muy manejable (manual de MATLAB% V4.2.1c).% El patch gráfico, tiene un estado interno que permite la representación% gráfica en la pantalla.% Estructura dada al patch (OBSTACLES)% state(UserData of patch) = [i, selfo], donde% i -- indice del obstáculo% selfo = [x y theta radius shape] -- datos propios del obstáculo%% El manejador del patch creado, se guarda en la posición "n" del vector% OBSTACLES, para ser recuperado en la ejecución del programa cuando se% desee modificar algún dato del Robot.%% Observar que los patch de los obstáculos ya están creados antes de% ejecutar esta función, pero sin estar inicializados (ver memoria% descriptiva).% -----------------------------------------------------------------------% SEE ALSO: sobsta, sobstak

% Declaración de variables de la funciónglobal AGENTS grasping OBSTACLES OBSTACLES2 MAX_OBSTACLESMAX_SIZE_OBSTACLES obsNum obsAdd shapecir ...

shapecua text2_error xob_coor yob_coor ang_obs_ro rad_obs_rotext_num_sen


% Nuevo índice del obstáculoblankspots = find(OBSTACLES2 == 0); % Posiciones vacias del vectorOBSTACLES2if blankspots n = blankspots(1);else n = length(OBSTACLES2) + 1;end;

if n > MAX_OBSTACLES % Número máximo de obstáculos superado ? set(obsAdd, 'Value', 0); set(text2_error,'String',['Warning: Obstacle not created.MAX_OBSTACLES = ' num2str(MAX_OBSTACLES)],'visible', 'on');% Mensaje de

% error return;end

Anexo A. Programa

238

% Comprobar los datos (coordenadas, radio, etc) introducidas por tecladoif (get(xob_coor,'string')=='' | get(yob_coor,'string')=='' |get(ang_obs_ro,'string')==''... | get(rad_obs_ro,'string')=='') % Mensaje de error, faltan datos set(text2_error,'String','Warning: Date Input, is not correct','visible', 'on'); return;end

% Lectura de los datos introducidos por tecladox = str2num(get(xob_coor,'string'));y = str2num(get(yob_coor,'string'));theta = str2num(get(ang_obs_ro,'string')) * pi/180;radius = str2num(get(rad_obs_ro,'string'));

% Comprobar radio introducido por teclado. Correcto ?if radius > MAX_SIZE_OBSTACLES radius = MAX_SIZE_OBSTACLES; set(rad_obs_ro,'string',num2str(radius));elseif radius < 2 radius = 2; set(rad_obs_ro,'string',num2str(radius));end; % end if radius > MAX_SIZE_OBSTACLES

% -----------------------------------------------------------------------% wwx wwy wx wy R -> variables del fichero de Simulink% bounds -> fichero simulink% Función de saturación de las coordenadas del obstáculo xmax = [0+R 130-% R], ylim = [0+R 90-R].% -----------------------------------------------------------------------global wwx wwy R wx wy

temps = 1e-6;wwx = x;wwy = y;R = radius;

% Control límites del terreno de juego del Obsta. "n"rk45('bounds',temps,[],[1e-7 temps temps 0 0 2]);

x = wx;y = wy;

set(xob_coor,'string',num2str(x));set(yob_coor,'string',num2str(y));

% Comprobar ángulo de rotación introducido por tecladoif theta > 2*pi theta = theta - 2*pi; set(ang_obs_ro,'string',num2str(theta*180/pi));elseif theta < 0 theta = 2*pi + theta; set(ang_obs_ro,'string',num2str(theta*180/pi));end;

% Asignar coordenadas en el vector de estado del obstáculo% selfo = [coordenada"x" coordenada"y" theta radius shape]selfo = [x y theta radius get(shapecua,'value')];

Anexo A. Programa

239

% Evitar superposición obstáculo con otros elementos de la simulaciónerror = noencima(selfo,n,'o');

if error % Superposición, mensaje de error stop = 1; set(text2_error,'String',['Warning: Superposition Obstacle nº 'num2str(n) ' Coordinates x = ' num2str(selfo(1)) ' y = 'num2str(selfo(2)) ],...


% Función cálculo de coordenadas de dibujo del obstáculo[xdata ydata] = dibu_o(selfo(1),selfo(2),selfo(3),selfo(4),selfo(5));

% Crear el patch que define el obstáculo (gráfico), UserData = state,añadiendo el código de% este obstáculo al vector OBSTACLES (pantalla de edición de obstáculos)a =patch('XData',xdata,'YData',ydata,'EraseMode','xor','FaceColor','w','EdgeColor','w');

% Guardar los diferentes patch de datos del obstáculo en el vector stateque corresponde a% "UserData" del patch principal del obstáculo.state = [n selfo];

set(a,'UserData',state);OBSTACLES2(n) = a; % Guardar manejador del obstá. de la pantalla deedición

% Numerar obstáculosset(text_num_sen(n),'position', [selfo(1)selfo(2)],'string',num2str(n,2),'visible','on','color','r');

set(OBSTACLES(n),'XData',xdata,'YData',ydata);set(OBSTACLES(n),'UserData',state);% Obstáculos de la pantalla principalde simulación

% Función renovar índices de los Obstáculos en el popup de la pantalla deediciónpopup(OBSTACLES2,obsNum,0);

clear a s xdata ydata selfo state shape x y errorclear obsta

Anexo A. Programa

240

function [xdata,ydata] = dibu_o(x,y,theta,radius,shape);

% -----------------------------------------------------------------------% [xdata,ydata] = DIBUJAR_OBSTÁCULOS(x,y,theta,radius,shape)%% Función para el calculo de las coordenadas de representación gráfica de% los obstáculos. Se calculan% las coordenadas que definen la forma de los obstáculos (cuadrado o% circulares, o en caso del objeto% grasping triangular).%% Los parámetros de entrada corresponden a los datos:% --- Coordenada central del obstáculo "x-y" (o del objeto%"grasping").% --- Ángulo de orientación "theta" del objeto.% --- Radio del obstáculo (tamaño) "radius".% --- Forma del obstáculo (o del objeto triangular de grasping).% shape = 0 cuadrado% shape = 1 circular% shape = 2 triangular%% La salida de la función corresponde a las coordenadas "x-ydata" para% definir los patch's-gráficos del obstáculo.%% Esta función también se utiliza para el calculo de las coordenadas de% dibujo del patch para el objeto del movimiento grasping.% -----------------------------------------------------------------------% SEE ALSO: dibu_a()

if shape == 1 % Forma del obstáculo cuadrada. Coordenadas gráficascuadrado. xdata = [x + radius*cos(theta + (1/4)*pi); ...

x + radius*cos(theta + (3/4)*pi); ... x + radius*cos(theta - (3/4)*pi); ... x + radius*cos(theta - (1/4)*pi);];

ydata = [y + radius*sin(theta + (1/4)*pi); ... y + radius*sin(theta + (3/4)*pi); ... y + radius*sin(theta - (3/4)*pi); ... y + radius*sin(theta - (1/4)*pi);];

elseif shape == 0 % Estructura obstáculo circular. Coordenadas gráficascirculo. pts = 0:pi/16:2*pi; xdata = radius*cos(pts) + x; ydata = radius*sin(pts) + y;

elseif shape == 2 % Objeto triangular de graspìng. Coordenadas gráficastriangulo. xdata = [x + radius*cos(theta + (1/2)*pi); ...

x + radius*cos(theta - (1/6)*pi); ... x + radius*cos(theta - (5/6)*pi)];

ydata = [y + radius*sin(theta + (1/2)*pi); ... y + radius*sin(theta - (1/6)*pi); ... y + radius*sin(theta - (5/6)*pi)];

end; % end if cua_cir == 1clear x y theta radius cua_cir

Anexo A. Programa

241

function [] = sobsta(i, propertyname, value)

% -----------------------------------------------------------------------% SETOBSTÁCULO(i, propertyname, value)%% Esta función se encarga de guardar el dato referido en el parámetro de% entrada como "propertyname"% en la estructura del obstáculo con el nuevo valor "value".% Si no existen parámetros de entrada, se realiza el set de las% coordenadas del obstá. con el mouse.% "i" índice del obstáculo.% "propertyname" referencia del dato tipo "UserData" para realizar el% set.% 'self'% "value" nuevo valor para el propertyname. Guardar en la estructura% del obstáculo.% -----------------------------------------------------------------------% SEE ALSO: obsta, sobstak

global OBSTACLES OBSTACLES2 MAX_OBSTACLES dt obsNum obsSettext2_error shapecua xob_coor ...

text_num_sen yob_coor ang_obs_ro rad_obs_ro


if nargin == 0 % Índice del obstáculo. i = popup(OBSTACLES2,obsNum,1,text2_error,'Warning: Not existObstacles');

if i == 0 set(obsSet,'value',0); return; end;

% Recuperar "UserData" state del obstáculo state = get(OBSTACLES2(i), 'UserData'); i = state(1); % selfo = [coordenada"x" coordenada"y" theta radius shape] selfo = state(2:6);

% Renovar datos del vector UserData state, y redibujar el Obsta. enlas nuevas coordenadas

% Si no hay parámetros de entrada, entonces esperar entradacoordenadas del mouse. pts = ginput(2); selfo(1) = pts(1,1); % x position click ratón selfo(2) = pts(1,2); % y position click ratón

if ~diff(pts) self(3) = 0; % NO hay diferiencia de ángulo entre primer clicy segundo clic else % SI hay diferiencia de ángulo entre primer clic y segundo clic selfo(3) = atan2(pts(2,2)-pts(1,2),pts(2,1)-pts(1,1)); % Ángulo if selfo(3) < 0 % Corregir ángulo negativo (3º y 4º cuadrantes).

selfo(3) = 2*pi + selfo(3); end;end;

Anexo A. Programa

242

selfo(5) = get(shapecua,'value'); % Forma obstáculo, cuadrado =1, cicular = 0

elseif nargin > 2 propertyname = lower(propertyname);

if propertyname(1:2) == 'se' selfo = value; endend % end if nargin == 0

% -----------------------------------------------------------------------% wwx wwy wx wy R -> variables del fichero de Simulink% bounds -> fichero simulink% Función de saturación de las coordenadas del obstáculo xmax = [0+R 130-% R], ylim = [0+R 90-R].% -----------------------------------------------------------------------global wwx wwy R wx wy

temps = 1e-6;wwx = selfo(1);wwy = selfo(2);R = selfo(4);

% Control límites del terreno de juego del Obsta. "a"rk45('bounds',temps,[],[1e-7 temps temps 0 0 2]);

selfo(1) = wx;selfo(2) = wy;

% Evitar superposición obstáculo con otros elementos de la simulaciónerror = noencima(selfo,i,'o');

if error % Superposición, mensaje de error set(text2_error,'String',['Warning: Superposition Obstacle nº 'num2str(i) ' Coordinates x = ' num2str(selfo(1)) ' y = 'num2str(selfo(2)) ],...


% Función cálculo de coordenadas de dibujo del obstáculo[xdata ydata] = dibu_o(selfo(1), selfo(2), selfo(3), selfo(4), selfo(5));

% Guardar datos modificados del Obstáculo

% Set manejador del obstáculo, pantalla de ediciónset(OBSTACLES2(i),'XData',xdata,'YData',ydata);state = [i selfo];set(OBSTACLES2(i),'UserData',state);

% Numerar obstáculosset(text_num_sen(i),'position', [selfo(1)selfo(2)],'string',num2str(i,2),'visible','on');

% Set manejador del obstáculo, pantalla principal de simulaciónset(OBSTACLES(i),'XData',xdata,'YData',ydata);set(OBSTACLES(i),'UserData',state);

Anexo A. Programa

243

% Set en los buttons tipo edit en la pantalla de ediciónset(xob_coor,'string',num2str(selfo(1)));set(yob_coor,'string',num2str(selfo(2)));set(ang_obs_ro,'string',num2str(selfo(3)*180/pi));set(rad_obs_ro,'string',num2str(selfo(4)));

function [] = sobstak()

% -----------------------------------------------------------------------% SETOBSTÁCULOKEYBOARD()%% Función asociada al push button Update de datos del obstáculo.% Esta función se encarga de realizar el set de las coordenadas del Robot% a través de los valores% x, y, theta, radio y forma introducidos por teclado en la pantalla de% diseño de obstáculos.% selfo(1) = str2num(get(xob_coor,'string'... lectura coordenada x% del button tipo edit%% sobsta(i,'Self',selfo) --- Set vector de estado obstáculo "i"% -----------------------------------------------------------------------% SEE ALSO: obsta(), sobsta(), dobsta()


global OBSTACLES2 MAX_OBSTACLES MAX_SIZE_OBSTACLES obsNum obsSetktext2_error shapecua...

xob_coor yob_coor ang_obs_ro rad_obs_ro


% Índice del obstáculo.i = popup(OBSTACLES2,obsNum,1,text2_error,'Warning: Not existObstacles');

if i == 0 set(obsSetk,'value',0); return;end;

% Recuperar "UserData" state del obstáculostate = get(OBSTACLES2(i), 'UserData');i = state(1);% selfo = [coordenada"x" coordenada"y" theta radius shape]selfo = state(2:6);

% Comprobar datos introducidos por tecladoif (get(xob_coor,'string')=='' | get(yob_coor,'string')=='' | ... get(ang_obs_ro,'string')=='' | get(rad_obs_ro,'string')=='') % Mensaje de error, faltan datos. set(text2_error,'String','Warning: Date Input, is notcorrect','visible','on'); return;end

% Leer de los buttons tipo edits las coordenadas (transformar de string anúmero con la función de% aplicación str2num).

Anexo A. Programa

244

selfo(1) = str2num(get(xob_coor,'string')); % Leer coordenada x delbutton tipo editselfo(2) = str2num(get(yob_coor,'string')); % Leer coordenada y delbutton tipo edittheta = str2num(get(ang_obs_ro,'string')); % Leer coordenada ángulodel button tipo editselfo(3) = theta*pi/180; % De grados a radianes

% Comprobar ángulo de rotación introducido por tecladoif selfo(3) > 2*pi selfo(3) = selfo(3) - 2*pi;elseif theta < 0 selfo(3) = 2*pi + selfo(3);end;

selfo(4) = str2num(get(rad_obs_ro,'string')); % Radio del obstáculo

if selfo(4) > MAX_SIZE_OBSTACLES % No superar el máximo tamaño selfo(4) = MAX_SIZE_OBSTACLES;elseif selfo(4) < 2 selfo(4) = 2;end; % end if selfo(4) > MAX_SIZE_OBSTACLES

selfo(5) = get(shapecua,'value'); % Forma obstáculo, cuadrado = 1,cicular = 0

% Set de las nuevos datos en la estructura del obstáculo.sobsta(i,'Selfo',selfo);

function[] = chshape(op)

% -----------------------------------------------------------------------% CHSHAPE()%% Función asociada a los buttons tipo radio para la selección de la forma% del obstáculo.% Se realiza el set al button radio;% button tipo radio shapecir (circular) parámetro de entrada op = 0% button tipo radio shapecua (cuadrado) parámetro de entrada op = 1% -----------------------------------------------------------------------global shapecir shapecua

if op set(shapecua,'value',1);% Set value a 1 forma cuadrada set(shapecir,'value',0);% Set value a 0 forma circularelse set(shapecir,'value',1);% Set value a 1 forma circular set(shapecua,'value',0);% Set value a 0 forma cuadradaend;

Anexo A. Programa

245

function[] = chcolobs()

% -----------------------------------------------------------------------% CHANGECOLOROBSTACLES()%% Función asociada al popup de índices de los obstáculos en la pantalla% de diseño.% Esta función cambia el color del obstáculo "i" seleccionado, y además% renueva los parámetros en% los buttons tipo edit de la pantalla de diseño ("i" obstáculo% seleccionado en el popup).% set(xob_coor,... renovar buttons tipo edit%% El índice del obstáculo se lee del popup de la pantalla.% -----------------------------------------------------------------------% SEE ALSO: chcolsen(), chrobot(), chsize(), obsta(), sobsta(),sobstak()

global OBSTACLES2 MAX_OBSTACLES obsNum text2_error xob_Text xob_cooryob_coor ang_obs_ro...

rad_obs_ro shapecir shapecua


% Índice obstáculoi = popup(OBSTACLES2,obsNum,1,text2_error,'Warning: Not exist Obsta');

if i == 0 return;end

% Recuperar "UserData" state del obstáculostate = get(OBSTACLES2(i),'UserData');i = state(1);% selfo = [coordenada"x" coordenada"y" theta radius shape]selfo = state(2:6);

% Cambiar el color del obstáculo de índice "i"for j = 1:length(OBSTACLES2)

if OBSTACLES2(j) ~= 0 if i == j % Cambiar el color del obstáculo seleccionado en elpopup

set(OBSTACLES2(j),'FaceColor','m','EdgeColor','m');

elseset(OBSTACLES2(j),'FaceColor','w','EdgeColor','w');

end; % end if if i == menu(j) end; % end if OBSTACLES2(j) ~= 0end; % end for j=1:length(OBSTACLES2)

% Lectura-escritura de los parámetros del obstáculo "i" en los buttonstipo editset(xob_coor,'string',selfo(1)); % Button tipo edit coordenada xset(yob_coor,'string',selfo(2)); % Button tipo edit coordenada yset(ang_obs_ro,'string',num2str(selfo(3)*180/pi));set(rad_obs_ro,'string',selfo(4)); % Button tipo edit radio delobstáculo

Anexo A. Programa

246

% Set en los buttons tipo radio de elección de la forma del obstáculochshape(selfo(5));

clear state i selfo index

function [] = dobsta(i)

% -----------------------------------------------------------------------% DELETEOBSTÁCULO()% DELETEOBSTÁCULO(i)%% Función para borrar el obstáculo "i".% El índice "i" se lee del popup de la pantalla o bien se obtiene como% parámetro de entrada.% Los obstáculos se inicializan a cero, pero no se borran con delete,% debido a que es necesario% conservar el patch, sin perder la referencia de la Pantalla Principal% de Simulación del% obstáculo por si se volviera a crear uno nuevo.% -----------------------------------------------------------------------% SEE ALSO: dsensor(), dagent(), delpos(), obsta(), sobsta(), sobstak()

% Declaración de variables de la funciónglobal OBSTACLES OBSTACLES2 obsNum obsDel text2_error xob_cooryob_coor ang_obs_ro rad_obs_ro text_num_sen


if nargin == 0

% Índice del obstáculo i = popup(OBSTACLES2,obsNum,1,text2_error,'Warning: Not existObstacles');

if i == 0 set(obsDel,'value',0); return;end;end; % end if nargin == 0

% Recuperar "UserData" state del obstáculo. Borrar el patch del obsta%(inicializar a cero).if OBSTACLES2(i) ~= 0

% Recuperar "UserData" state del obstáculo state = get(OBSTACLES2(i), 'UserData'); i = state(1); selfo = state(2:6); selfo = [0 0 0 0 0]; % Inicializar vector de estado del obstáculo.

state = [0 selfo]; % Vector de datos del obstáculo set(OBSTACLES(i),'UserData',state); % UserData inicializado. set(OBSTACLES(i),'XData',0,'YData',0); % Inicializar patch, pero noborrarlo!!!

delete(OBSTACLES2(i)); % Borrar obstáculo pantalla de edición OBSTACLES2(i) = 0; % Borrar manejador del patch obstáculopantalla de edición

Anexo A. Programa

247

% Borrar numerar obstáculos set(text_num_sen(i),'position', [0 0],'string','','visible','off');

end; % end if OBSTACLES2(i) ~= 0

% Lectura-escritura de los parámetros del obstáculo "i" en los buttons% tipo editset(xob_coor,'string',''); % Button tipo edit coordenada xset(yob_coor,'string',''); % Button tipo edit coordenada yset(ang_obs_ro,'string','');set(rad_obs_ro,'string',''); % Button tipo edit radio del obstáculo

% Modificar el número de obstáculos en el vector OBSTACLES2while OBSTACLES2(length(OBSTACLES2)) == 0 OBSTACLES2 = OBSTACLES2(1:length(OBSTACLES2)-1);

if length(OBSTACLES2) == 0 break; end; % end if length(OBSTACLES2) == 0end % end while OBSTACLES2(length(OBSTACLES2)) == 0

% Función renovar índices de los obstáculos en el popup de la pantalla de% edición de obstáculospopup(OBSTACLES2,obsNum,0);

clear i s menu

A.9 Funciones Asociadas al Tipo de Movimiento en la Simulación

function [] = funforward()

% --------------------------------------------------------------------% FUNCIONFORWARD()%% Función asociada al button tipo radio del movimiento Forward.% En esta función, se activan-desactivan los diferentes buttons,% elementos, etc. necesarios para el funcionamiento en movimiento tipo% Forward.% --------------------------------------------------------------------% Declaración de variables de la funciónglobal WORLD_AXIS OBJECT_GRAS TRAILDIANA AGENTS text1_error graspingforward tracking other obSetk xobj_Text xobj_coor yobj_Text yobj_coor

axes(WORLD_AXIS);

set(text1_error,'visible','off');set(other,'value',0); % Radio de other a 0set(grasping,'value',0); % Radio de grasping a 0set(tracking,'value',0); % Radio de tracking a 0set(forward,'value',1); % Radio de forward a 1

set(obSetk,'Visible','off'); % Desactivar buttons no necesarios para% este movimiento

set(xobj_Text,'Visible','off');

set(xobj_coor,'Visible','off');

Anexo A. Programa

248

set(yobj_Text,'Visible','off');

set(yobj_coor,'Visible','off');

for i = 1:length(OBJECT_GRAS) % Borrar objetos a transportar (Grasping) if OBJECT_GRAS(i) ~= 0 % Existe objeto ? MAR_FINAL = ggras(i,'MAR_FINAL'); delete(MAR_FINAL); % Borrar triangulo de señalización destino delete(OBJECT_GRAS(i)); % Borrar objeto de Grasping OBJECT_GRAS(i) = 0; % Borrar manejador end % end if OBJECT_GRAS(i) ~= 0end % end for i = 1:length(OBJECT_GRAS)

if TRAILDIANA ~= 0 % Borrar recorrido de la diana (función Tracking) delete(TRAILDIANA); TRAILDIANA = [];end % end if exist(TRAILDIANA ~= 0)

clear global indice radio_circlear COOR_DIANA

function [] = fungras()

% -----------------------------------------------------------------------% FUNCIONGRASPING()%% Función asociada al button tipo radio del movimiento Grasping.% En esta función, se activan-desactivan los diferentes buttons,% elementos, etc. necesarios para% el funcionamiento en el movimiento tipo Grasping.% -----------------------------------------------------------------------

% Declaración de variables de la funciónglobal WORLD_AXIS OBJECT_GRAS AGENTS TRAILDIANA text1_error graspingforward tracking other obSetk...

xobj_Text xobj_coor yobj_Text yobj_coor

axes(WORLD_AXIS);

set(text1_error,'visible','off');set(forward,'value',0); % Radio de forward a 0set(tracking,'value',0); % Radio de tracking a 0set(other,'value',0); % Radio de other a 0set(grasping,'value',1); % Radio de grasping a 1

set(obSetk,'Visible','on','Callback','sgras;'); % Activar-desactivarbuttons no necesarios para este movimiento

delete(xobj_coor);clear global xobj_coor;global xobj_coor;

set(xobj_Text,'Visible','on','String','X Coordinate(<130cm):','Position',[.75 .65 .16 .03]);

xobj_coor = uicontrol('Visible','on','Style','Edit','Units','normalized','Position', [.91 .65 .05 .03], 'String', '', 'BackgroundColor', 'w', ...'HorizontalAlignment', 'Center', 'Callback','');

Anexo A. Programa

249

set(yobj_Text,'Visible','on','String','Y Coordinate(<90cm):','Position',[.75 .61 .16 .03]);

set(yobj_coor,'String','','Visible','on','Position',[.91 .61 .05 .03]);

for i=1:length(AGENTS) if AGENTS(i) ~= 0 & OBJECT_GRAS(i) == 0 % Existe Robot ? gras(i); % Crear objeto asociado al Robot (movimiento Grasping) end % end if AGENTS(i) ~= 0...end % end for i=1:length(AGENTS)

if TRAILDIANA ~= 0 % Borrar recorrido de la diana (función Tracking) delete(TRAILDIANA); TRAILDIANA = [];end % end if exist(TRAILDIANA ~= 0)clear global indice radio_circlear COOR_DIANA

function [] = funother()

% --------------------------------------------------------------% FUNCIONOTHER()%% Función asociada al button tipo radio del movimiento Other.% En esta función, se activan-desactivan los diferentes buttons,% elementos, etc. necesarios para% el funcionamiento en movimiento tipo Other.% --------------------------------------------------------------% Declaración de variables de la funciónglobal WORLD_AXIS OBJECT_GRAS AGENTS TRAILDIANA text1_error graspingforward tracking obSetk other...


axes(WORLD_AXIS);

set(text1_error,'visible','off');set(forward,'value',0); % Radio de forward a 0set(tracking,'value',0); % Radio de tracking a 0set(grasping,'value',0); % Radio de grasping a 0set(other,'value',1); % Radio de other a 1

set(obSetk,'visible','on','Callback','sother;'); % Cambio de función dellamada push-button "UpDate"


% Activar buttons necesarios para este movimiento

xobj_coor = uicontrol('Visible','on','Style','Edit','Units','normalized',...

'Position',[.91 .65 .05.03],'String','','BackgroundColor','w', ...

'HorizontalAlignment','Center','Callback','');

set(xobj_Text,'Visible','on','String','X Coordinate(Setpoint):','Position',[.75 .65 .16 .03]);

Anexo A. Programa

250

set(yobj_Text,'Visible','on','String','Y Coordinate(Setpoint):','Position',[.75 .61 .16 .03]);

set(yobj_coor,'Visible','on','String','','Position',[.91 .61 .05 .03]);

menu = find(AGENTS); % Índices no vacios en AGENTSif ~isempty(menu); % Comprobar vector no vacío set(yobj_coor,'String','','Visible','on'); set(xobj_coor,'String','','Visible','on');end;


if TRAILDIANA ~= 0 % Borrar recorrido de la diana (función Tracking) delete(TRAILDIANA); TRAILDIANA = [];end % end if exist(TRAILDIANA ~= 0)

% Vaciar el espacio de trabajo utilizado en esta funciónclear global indice radio_cir

function [] = funtracking()

% --------------------------------------------------------------% FUNCIONTRACKING()%% Función asociada al button tipo radio del movimiento Tracking.% En esta función, se activan-desactivan los diferentes buttons,% elementos, etc. necesarios para% el funcionamiento en movimiento tipo Tracking.% --------------------------------------------------------------% Declaración de variables de la funciónglobal WORLD_AXIS OBJECT_GRAS radio_cir text1_error grasping forwardtracking other obSetk indice...


axes(WORLD_AXIS);

set(text1_error,'visible','off');set(grasping,'value',0); % Radio de grasping a 0set(forward,'value',0); % Radio de forward a 0set(other,'value',0); % Radio de other a 0set(tracking,'value',1); % Radio de tracking a 1

set(obSetk,'Visible','off'); % Activar-desactivar buttons no necesariospara este movimiento


Anexo A. Programa

251

set(xobj_Text, 'Visible','on','String', 'Figure Type:', 'Position', [.75.65 .09 .03]);

xobj_coor =uicontrol('Visible','on','Style','popup','Units','normalized', ...

'Position',[.845 .65 .07 .04],'String','Circular|Arc|Spiral|Square', 'BackgroundColor', 'w', ...

'HorizontalAlignment', 'Center', 'Callback','');

set(yobj_Text,'Visible','on','String','Angle (Degrees):','Position',[.75.61 .12 .03]);

set(yobj_coor,'String','0','Visible','on','Position',[.87 .61 .03 .03]);


radio_cir = 35; % Inicializar factor de reducción para movimientoTrackingindice = 0; % Inicializar índice para el movimiento Tracking

clear COOR_DIANA

Anexo A. Programa

252

function [error] = gras(n,x,y,theta,radius)

% -----------------------------------------------------------------------% n = GRASPING;% n = GRASPING(x,y,theta)% n = GRASPING(x,y,theta,radius)%% Esta función crea un OBJETO (triangular), para el movimiento en% grasping (cada Robot tiene% asociado un objeto).%% Un Objeto es un tipo especial de "patch", donde se le define una% estructura de datos de tipo% "UserData" El patch no deja de ser un objeto-dibujo (con un espacio de% memoria asignado)% bidimensional, con una estructura muy manejable (manual de MATLAB% V4.0).% El patch gráfico, tiene un estado interno que permite moverlo de forma% dinámica por la% pantalla modificando sus coordenadas 'Xdata' 'Ydata'.% Estructura dada al patch (OBJECT_GRAS)% state(UserData of patch) = [i, selfo], donde% i -- indice del object% selfobj = [xobj yobj theta radius remocaldo] -- datos% propios del object%% El manejador del patch creado, se guarda en la posición "n" del vector% OBJECT_GRAS, para ser% recuperado en la ejecución del programa cuando se desee modificar algún% dato del objeto.%% La función inicializa, por defecto, la posición del objeto en las%coordenadas xinicial = 10*n,% yinicial = 75, en caso que no se reciban como parámetros de entrada.% -----------------------------------------------------------------------% SEE ALSO: ggras(), sgras()

% Declaración de variables de la funciónglobal OBJECT_GRAS MAX_AGENTS DEFAULT_OBSTACLE_WIDTH text1_errorcolor stop

% Inicializar vector de estado del objetoif nargin < 4 radius = DEFAULT_OBSTACLE_WIDTH/1.75;

if nargin < 3 x = 10*n; y = 15; theta = 0; end; % end if nargin < 3end; % end if nargin < 4

% selfobj = [coordenada"x" coordenada"y" theta radius remocaldo]selfobj = [x y theta radius 0];

% Evitar superposición objeto "grasping" con otros elementos de lasimulaciónerror = noencima(selfobj,n,'r');

Anexo A. Programa

253

if error % Superposición, mensaje de error stop = 1; set(text1_error,'String', ['Warning: Superposition Object "grasping"nº ' num2str(n) ' Coordinates x = ' num2str(selfobj(1)) ' y = 'num2str(selfobj(2)) ], 'visible', 'on'); % Mensaje de error return;end; % end if error

% Crear el patch que define el objeto (gráfico), UserData = state,% añadiendo el manejador de este objeto en el vector OBJECT_GRAS(n)

% Función coordenadas de dibujo del objeto triangular[xdata ydata] = dibu_o(selfobj(1),selfobj(2),selfobj(3),selfobj(4),2);

% Crear el patch principal del objetoa =patch('XData',xdata,'YData',ydata,'EraseMode','xor','FaceColor',color(n),...

'EdgeColor',color(n));

% Triangulo señalización de destino para el Robot una vez remolcado el% objeto[xdata ydata] = dibu_o((120 - n * 5),75,selfobj(3),selfobj(4),2);

% Crear el patch del marcador de destino del objetoMAR_FINAL =patch('visible','off','XData',xdata,'YData',ydata,'EraseMode','xor',...

'FaceColor','none','EdgeColor',color(n),'linewidth',1);

set(a,'UserData',[n selfobj MAR_FINAL]); % Guardar vector de datos delobjeto, "UserData" del patch

OBJECT_GRAS(n) = a; % Guardar manejador del patch

clear b a xdata ydata selfobj

function [] = sgras(i, propertyname,value,self)

% -----------------------------------------------------------------------% SGRAS(i, propertyname, value)%% Esta función se encarga de guardar el dato referido en el parámetro de% entrada como "propertyname"% en la estructura del Objeto (grasping) con el nuevo valor "value".% Si no existen parámetros de entrada, se realiza el set de las% coordenadas del Objeto con los% datos introducidos por teclado en los buttons tipo edit.% "i" índice del Robot Robot = índice del objeto.% "propertyname" referencia del dato tipo "UserData" para realizar el% set.% 'self'% "value" nuevo valor a guardar en la estructura del Objeto.% 'selfobj = [xobj yobj theta radius ¿remocaldo?] datos propios% del Objecto'% "self" vector de estado del Robot asociado al Objeto.% 'selfobj = [xobj yobj theta radius remocaldo] datos propios% del objeto grasping'% -----------------------------------------------------------------------% SEE ALSO: ggras, gras

Anexo A. Programa

254

global OBJECT_GRAS AGENTS text1_error stop dt agNum obSetk wwx wwy Rwx wy xobj_coor yobj_coor


if nargin == 0

% Índice del Objeto. i = popup(AGENTS,agNum,1,text1_error,'Warning: Not exist Object,create Robot');

if i == 0 return; end;

% Recuperar "UserData" state del object state = get(OBJECT_GRAS(i),'UserData'); i = state(1); % selfobj = [xobj yobj theta radius remocaldo] selfobj = state(2:6); MAR_FINAL = state(7);

if (get(xobj_coor,'string')=='' | get(yobj_coor,'string')=='') % Mensaje de error, faltan datos set(text1_error,'String','Warning: Date Input, is not correct','visible', 'on'); return; end; % end if (get(xobj_coor,'string')=='' |get(yobj_coor,'string')=='')

% Leer coordenadas introducidas por teclado selfobj(1) = str2num(get(xobj_coor,'string')); selfobj(2) = str2num(get(yobj_coor,'string')); selfobj(5) = 0; % Objeto NO remolcado set(MAR_FINAL,'visible','off');

% Evitar superposición objeto "grasping" con otros elementos de la % simulación error = noencima(selfobj,i,'r');

if error % Superposición, mensaje de error stop = 1; set(text1_error,'String',['Warning: Superposition Object "grasping"nº ' num2str(i) ' Coordinates x = ' num2str(selfobj(1)) ' y = 'num2str(selfobj(2)) ], 'visible','on'); % Mensaje de error return; end; % end if error

elseif nargin > 2 % Set datos, parámetros de entrada

state = get(OBJECT_GRAS(i),'UserData'); MAR_FINAL = state(7);


if propertyname(1:2) == 'se' % set vector de estado del objeto selfobj = value; end;end; % end if nargin == 0

Anexo A. Programa

255

% -----------------------------------------------------------------------% wwx wwy wx wy R -> variables del fichero de Simulink% bounds -> fichero simulink% Función de saturación de las coordenadas del objeto (grasping) xmax =% [0+R 130-R], ylim = [0+R 90-R].% -----------------------------------------------------------------------temps = 1e-5;wwx = selfobj(1);wwy = selfobj(2);R = selfobj(4);

if selfobj(5) == 0 R = selfobj(4); % Remolcado por Robot, radius Robot = radiusobjectelseif nargin == 4 R = self(4); % Remolcado por Robot, radius Robot = radiusobjectend;

% Control límites del terreno de juego del object "a"rk45('bounds',temps,[],[1e-5 temps temps 0 0 2]);

selfobj(1) = wx;selfobj(2) = wy;

% Coordenadas de dibujo del objeto triangular[xdata ydata] = dibu_o(selfobj(1),selfobj(2),selfobj(3),selfobj(4),2);

% Guardar datos modificados del Objecto.set(OBJECT_GRAS(i),'XData',xdata,'YData',ydata);

% Set datos "UserData" en la estructura del objetoset(OBJECT_GRAS(i),'UserData',[i selfobj MAR_FINAL]);

% Set coordenadas en los buttons tipo edit de la pantalla principal desimulaciónset(xobj_coor,'string',num2str(selfobj(1)));set(yobj_coor,'string',num2str(selfobj(2)));

% Vaciar el espacio de trabajo utilizado en esta funciónclear state xdata ydata menu i R selfobj self temps errorclear sgras

Anexo A. Programa

256

function [value] = ggras(i, propertyname)

% -----------------------------------------------------------------------% [value] = GETPOSITION(i,'PropertyName')%% Función para el "get" de los datos del objeto de grasping asociado a% cada Robot, desde cualquier% otra función.% Esta función realiza el get (coger) de las coordenadas-datos del% objeto, pasando como% parámetros de entrada el índice del objeto "i" (que coincide con el del% Robot) y el propertyname% del dato deseado.% "i" índice del Robot% "propertyname" referencia del dato deseado% self MAR_FINAL state% [value] = parámetro de salida.% -----------------------------------------------------------------------% SEE ALSO: getpos(), gagent(), gras()

% Declaración de variables de la funciónglobal OBJECT_GRAS

state = get(OBJECT_GRAS(i),'UserData'); % vector de estado del objetoselfobj = state(2:6); % selfobj = [xobj yobj theta radiusremocaldo]MAR_FINAL = state(7); % patch gráfico, marcador de destinodel objeto


% Comprobar y pasar el value según el propertyname de entrada

if propertyname(1:2) == 'se' value = selfobj;elseif propertyname(1:2) == 'ma' value = MAR_FINAL;elseif propertyname(1:2) == 'st' value = state;end

clear global state selfobj

Anexo A. Programa

257

A.10 Funciones Asociadas a la Ejecución de la Simulación

function [] = mainloop()

% ----------------------------------------------------------------------% MAINLOOP()%% Función asociada al push button Run.% Función para el RUN de la simulación, donde se realiza un bucle% iterativo hasta no pulsar STOP.% Esta función llama en forma de bucle a la función paso(), para dar la% sensación de simulación continuada.% ----------------------------------------------------------------------% SEE ALSO: paso()

global AGENTS text1_error stop veloSimula

menu = find(AGENTS);

if menu == [] % Si no hay Robots, escribir mensaje de aviso set(runButton, 'Value', 0); % Mensaje de error set(text1_error,'String','Warning: Not exist Robot, create Robot andthen Run','visible','on'); return;end

if veloSimula == 1 % Selección velocidad de dinamismo gráfico de lapantalla (valor de configuración) tiempo_refresco = 0.025; % Rapidelseif veloSimula == 2 tiempo_refresco = 0.1; % Mediumelseif veloSimula == 3 tiempo_refresco = 0.2; % Slowend;stop = 0;

while stop == 0 ta = clock; paso; % Función principal de simulación pause(1e-4); % Pausa durante 1e-5 segundos while ((clock - ta) <= tiempo_refresco) & (stop == 0) end;end; % end while stop == 0

% Vaciar el espacio de trabajo utilizado en esta funciónclear tiempo_refresco ta

Anexo A. Programa

258

function[] = paso()

% ---------------------------------------------------------------------% PASO()%% Función asociada al button Step. En esta función realiza la animación% de movimiento de los% Robots (movimiento secuencial de cada Robot).% Interactua con otras funciones, para el calculo de las consignas, la% simulación, sensorización y programación de la consigna de posición% (máquina de estados).% ---------------------------------------------------------------------global AGENTS MAX_SENSORS ESTADO_SENSORES dt loggButton stoplog_vlin text1_error forward dt_edit...

grasping tracking other minu minutos segundos segun contador

t0 = clock;

if length(AGENTS) ~= 0

b = size(log_vlin,2); % Tamaño datos para las gráficas b = b + 1; set(text1_error,'visible','off');

else % Mensaje de error set(text1_error,'String','Warning: Not exist Robot, create Robot andthen Run','visible','on'); return;end;

for a = 1:length(AGENTS)

if (AGENTS(a)~= 0 & stop==0)

% Datos de estado actual del Robot "a" % i = índice del robot % self = [xa ya theta radius velolineal veloangular pertur] % goal = [r psi libre?] coordenadas polares Robot-objetivo % confina = [control_name_file] % mast = [machine_states_file] % graf_sen = Line gráfica de los sensores % dades_sen = Matriz de datos de los sensores

% dades_sen(i,:) =[distan_detec, ang_detec, ang_rotate_sensor,position_dis, position_ang]

[state i self goal confina mast redon graf_sen dades_sen] =strucrob(a);

for index = 1:MAX_SENSORS % Inicializar estado de los sensores,antes de ser analizados

ESTADO_SENSORES(a,index) = 0; % Matriz de estado actual sensoresESTADO_VIEJO_SENSORES(index) = 0; % Vector de estado anterior

sensores

if dades_sen(index,1)~=0 % Si sensor detecta obstáculo (igual a rojo = [1 0 0]) if get(graf_sen(index),'color') == [1 0 0]

% Guardar estado "1", en vector de estado anterior ESTADO_VIEJO_SENSORES(index) = 1;

Anexo A. Programa

259

end; % Inicializar a verde sensor set(graf_sen(index),'color','g');

end;end;

% Calculo de la distancia y el ángulo entre el Robot y su diana. M = simgoal(self,a); % Guardar M = [r psi libre??] en Goal del Robot "a" [goal] = sagent(a,'Goal',M);

% Función de animación del movimiento del Robot en la pantalla de% simulación (simulación)

[self] = dynamics(a,self,goal,confina,dt);

for j = 1:length(AGENTS)

if ((AGENTS(j) ~= 0) & (j~=a))

self2 = gagent(j,'Self');

% Función de choque entre Robots self = xocrobot(self,j,self2);

% Comprobar detección sensores Robot "a" respecto los otros % Robots ESTADO_SENSORES(a,:) =

sensor(self,graf_sen,dades_sen,self2(1),self2(2),self2(4),self2(3),ESTADO_SENSORES(a,:),1);

end; % end if ((AGENTS(j) ~= 0) & (j~=a)) end; % end for j = 1:length(AGENTS)

for j = 1:length(OBSTACLES)

% Recuperar "UserData" state del obstáculostate = get(OBSTACLES(j), 'UserData');i = state(1);% selfobs = [x y theta radius shape] --- vector datos del Obstáculoselfobs = state(2:6);

if (i ~= 0)

% Función de choque entre Robot-Obstáculo self = xocobsta(self,selfobs);

% Comprobar detección sensores Robot "a" respecto los obstáculos ESTADO_SENSORES(a,:) =

sensor(self,graf_sen,dades_sen,selfobs(1),selfobs(2),selfobs(4),selfobs(3),ESTADO_SENSORES(a,:),selfobs(5));

end; % end if (i ~= 0) end; % end for j = 1:length(OBSTACLES)

if get(grasping,'value') % Movimiento tipo Grasping

[selfobj self] = object(a,self);

% selfobj = [xobj yobj theta radius remocaldo] datos propios del% Objectosgras(a,'Self',selfobj);

Anexo A. Programa

260

% Dibujar Robot con las coordenadas finales

[self] = sagent(a,'Self',self,selfobj);

% Asignar consigna posición movimiento grasping[self selfdia] = assig_po(a,self,selfobj);

elseif get(tracking,'value') % Movimiento tipo Tracking% Dibujar Robot con las coordenadas finales[self] = sagent(a,'Self',self);% Asignar consigna posición movimiento tracking[self selfdia] = assigpo2(a,self);

elseif get(forward,'value') % Movimiento tipo Forward% Dibujar Robot con las coordenadas finales[self] = sagent(a,'Self',self);

% Asignar consigna posición movimiento forward[self selfdia] = assigpo3(a,self);

elseif get(other,'value') % Movimiento tipo Other% Dibujar Robot con las coordenadas finales[self] = sagent(a,'Self',self);

% Asignar consigna posición movimiento other[self selfdia] = assigpo4(a,self);

end; % end if get(grasping,'value')

% selfdia = [x y libre] vector datos de la diana [selfdia] =feval(get(mast,'UserData'),self,selfdia,ESTADO_SENSORES(a,:),ESTADO_VIEJO_SENSORES,a);

% Set de las coordenadas de la diana (consigna de posición del% Robot "a")

setpos(a, selfdia(1),selfdia(2),selfdia(3));

% Guardar datos para realizar la gráficas si LoggButton == 1 if get(loggButton,'Value') % Solo SI Logger activado

global x_ag y_ag logv_m_d logi_m_d logv_m_i logi_m_i logp_m_dlogp_m_i log_vang

% Guardar resultados de la simulación x_ag(a,b) = self(1); y_ag(a,b) = self(2); log_vlin(a,b) = self(5); log_vang(a,b) = self(6); logv_m_d(a,b) = v_m_d; logv_m_i(a,b) = v_m_i; logi_m_d(a,b) = i_m_d; logi_m_i(a,b) = i_m_i; logp_m_d(a,b) = p_m_d; logp_m_i(a,b) = p_m_i;

end; % end if get(loggButton,'Value') end; % end if (AGENTS(a)~= 0 & stop==0)end; % end for a = 1:length(AGENTS)

Anexo A. Programa

261

% Control del Total grain time% Tiempo de muestreo mínimo para la monotorización% "etime" devuelve el tiempo en segundos entre clock y t0tirepan = etime(clock,t0) + segun; % Tiempo refresco pantalla entre stepy step

if tirepan >= 60 % Añadir un minuto tirepan = 0; minu = minu + 1; set(minutos,'string',[num2str(minu) ' min']); % set minutosend;

set(segundos,'string',[num2str(tirepan,4) ' s']); % set segundos

segun = tirepan;

% Recordar que dt = Grain Time simulaciónvector1 = [];vector1 = get(dt_edit,'string'); % Valor actual de Grain Time Total

j = 1;vector2 = [];while vector1(j) ~=' '; vector2(j) = vector1(j); j = j + 1;end;

dt_total = str2num(vector2) + dt;

set(dt_edit,'string',[num2str(dt_total,6) ' s']);% set Total Grain Timepantalla

clear j u a self self2 dt_total t0 tirepan b selfdia selfobs vector2vector1 confina mast

function [M] = simgoal(self,i)

% -----------------------------------------------------------------------% [M] = SIMGOAL(self,i)%% Función para el calculo de la distancia-ángulo entre el Robot y la% diana asociada a esta Robot.% i = índice del Robot.% self = vector de estado del Robot (coordenadas)% self = [xa ya theta Ra velolineal veloangular pertur]% Parámetro de salida --- M = vector de salida vector de estado% M = [distancia ángulo libre??];% -----------------------------------------------------------------------

xydia = getpos(i,'Location'); % xydia() = [xdia ydia libre] Coordenadasde la diana (setpoint)

% -----------------------------------------------------------------------%En estas operaciones se realiza el calculo de la distancia entre dos% puntos, así como el ángulo existente (datos en forma polar).% -----------------------------------------------------------------------relxy = [xydia(1) xydia(2)] - [self(1) self(2)];

Anexo A. Programa

262

r = sqrt(sum(relxy.^2)); % Distancia entre puntos (entre Robot y sudiana)

psi = 0;

psi = atan2((xydia(2)-self(2)),(xydia(1)-self(1)));

psi = psi - self(3);

if psi < -pi psi= 2*pi+psi;end;

% M = [distancia_Robot-diana ángulo_Robot-diana libre]M = [r psi xydia(3)];

clear r x s xydia Rot dpsi psi relxy rotxy

function[self] = dynamics(a,self,goal,confina,dt)

% -----------------------------------------------------------------------% DYNAMICS(a,self,goal,confina)%% La función dynamics, realiza la simulación dinámica para el control de% velocidad y de posición del Robot. Con todos los datos de estado del% Robot, y a partir del fichero Simulink de control, se simula el modelo% del Robot (rk45...), obteniendo los resultados de velocidad, intensidad% y par de los motores.% Con los datos obtenidos en la simulación, se calculan las coordenadas% de posición del Robot en la función [...] = calxyth(...).% El parámetro de salida [self] corresponde al vector de estado del Robot% con las nuevas coordenadas de posición, velocidad lineal y velocidad% angular.% -----------------------------------------------------------------------% SEE ALSO: paso(), defaultc(simulink), motors(simulink)

global KPV KPA KIV x0 y0 d_rel a_rel pertur R uxjv uyjv wwx wwyv_lin v_ang v_m_d v_m_i i_m_d...

i_m_i p_m_d p_m_i veloc par_max e_m_d e_m_i uthetad uthetaipar_car_gras

KPA = 1;KPV = 1;KPI = 1;

% "dt" Grain Time simulación fichero Simulink

% Recordar que uxjv uxjv guardan las anteriores coordenadas del Robot

% self = [xa ya theta radius velolineal veloangular pertur]% "xa" valor de coordenada x del Robot% "ya" valor de coordenada y del Robot% "theta" valor de ángulo de giro del Robot, orientación% "radius" radio del Robot, separación entre ruedas% "velolineal" Velocidad lineal Robot parámetro de salida% "veloangular" Velocidad angular Robot parámetro de salida% "pertur" par de perturbación de los motores del Robot (Par deperturbación)

Anexo A. Programa

263

x0 = self(1); y0 = self(2); theta = self(3);R = self(4); pertur =self(7);

% Posición diana (objetivo) respecto al Robot "a", goal% Recuperar coordenadas polares entre Robot y dianarr = goal(1); % Distancia diana-Robotbb = goal(2); % Angulo diana-Robot

% get(confina,'Xdata') --- Tipo de controlador:0(Motors Simulink),1(AllSimulink)if get(confina,'Xdata') == 0 % Realizar simplemente la Simulación deMotors con Simulink

dx = (uxjv(a) - x0); % Cálculo de la consigna por código, nofichero Simulink dy = (uyjv(a) - y0); veloc = sqrt(dx*dx+dy*dy)/dt; gir = (bb * 180/pi * KPA) / dt; consigna_v = (rr * KIV) / dt;

v_1 = (KPV * (consigna_v-veloc) + gir); v_2 = (KPV * (consigna_v-veloc) - gir);

% Simulación de los motores con Simulink temps = dt; wwx = v_1; wwy = v_2;

% confina (Userdata) = [control_file_name] rk45(get(confina,'UserData'),temps,[],[1e-3 temps/2e3 temps/50 0 02]);

else % Realizar todo el Control con Simulink

% Datos para la llamada del fichero de control de Simulink (fichero =confina) temps = dt; % Distancia real diana-Robot d_rel = rr; % Ángulo en radianes diana-Robot a_rel = bb; % confina (Userdata) = [control_file_name] rk45(get(confina,'UserData'),temps,[],[1e-4 temps/2e3 temps/50 0 0 2]);

end; % end if tipus

self(5) = v_lin; % "v" Velocidad lineal Robotself(6) = v_ang; % "omega" Velocidad angular Robotself(7) = 0; % Reinicializar perturbación del Robot después del

% análisis% "pertur" par de perturbación de los motores del Robot a 0

uxjv(a) = self(1); % Guardar coordenada "x" anterioruyjv(a) = self(2); % Guardar coordenada "y" anterior

% Función de cálculo de "xa","ya","theta" del Robot "a"% (dinámica del Robot)[self] = calxyth(self,a,dt);clear ang goal vis confina dx dy temps

Anexo A. Programa

264

function[self] = calxyth(self,a,dt);

% -----------------------------------------------------------------------% CALCULARXYTH(self,a,dt)%% Función de cálculo de las coordenadas para la dinámica del Robot.% La velocidad lineal y angular se obtienen del resultado de la% Simulación del fichero Simulink.% espacio = velocidad * tiempo.%% a = índice del Robot% Cálculo coordenada x del Robot% x = x0 + v_lineal * cos(theta0) * dt% Cálculo coordenada y del Robot% y = y0 + v_lineal * sin(theta0) * dt% Ángulo de giro (orientación) = Velo_angular * t (t = diferencial de% tiempo dt)% self = [xa ya theta radius velolineal veloangular pertur]% "xa" valor de coordenada x del Robot% "ya" valor de coordenada y del Robot% "theta0" valor de ángulo de giro del Robot% "radius" Radio del Robot, (2 * radius) separación entre% ruedas% "velolineal" Velocidad lineal Robot parámetro de salida% "veloangular" Velocidad angular Robot parámetro de salida% "pertur" par de perturbación de los motores del Robot (Par de% perturbación)% -----------------------------------------------------------------------% SEE ALSO: dynamics(), paso()

global e_m_d e_m_i uthetad uthetai

% "dt" Grain Time simulación fichero Simulink

% uthetad(a) = e_m_d; % Espacio anterior recorrido por el motor derecho% uthetai(a) = e_m_i; % Espacio anterior recorrido por el motor

% izquierdo% self = [xa ya theta radius velolineal veloangular pertur]% "xa" valor de coordenada x del Robot% "ya" valor de coordenada y del Robot% "theta0" valor de ángulo de giro del Robot% "radius" Radio del Robot, (2 * radius) separación entre ruedas% "velolineal" Velocidad lineal Robot parámetro de salida% "veloangular" Velocidad angular Robot parámetro de salida% "pertur" par de perturbación de los motores del Robot (Par de% perturbación)

% Factor de reducción de la velocidad lineal, debido al choque-roce% contra los límites (paredes)% Función de Factor de reducción = factor_xoc[factor_xoc,self] = parets(self);

% Reducción de la Velocidad lineal debido al choque con los límites de la% simulaciónself(5) = self(5) * factor_xoc;

% Espacio recorrido = velo_lineal * t (t = Grain Time "dt")% espacio_lineal = (e_m_d + e_m_i)/2;% espacio_angular = (e_m_d - e_m_i)/(2*self(4));% espacio_lineal = (abs(e_m_d-uthetad(a))+abs(e_m_i-uthetai(a)))/2;

Anexo A. Programa

265

% espacio_angular = (abs(e_m_d-uthetad(a)) - abs(e_m_i-uthetai(a)))/(2*self(4))

espacio_lineal = self(5) * dt;espacio_angular = self(6) * dt;

% Ángulo de giro (orientación) = Velo_angular * t (t = diferen. tiempodt)self(3) = self(3) + espacio_angular; % theta = theta0 + velo_angular *dt

% x = x0 + v_lineal * cos(theta0) * dtself(1) = self(1) + espacio_lineal * cos(self(3));

% y = y0 + v_lineal * cos(theta0) * dtself(2) = self(2) + espacio_lineal * sin(self(3));

clear espacio_lineal espacio_angular

function [ESTADO_SENSORES] =sensor(self,graf_sen,dades_sen,x_obs,y_obs,ra_obs,theta_obs,ESTADO_SENSORES,tipo);

% -----------------------------------------------------------------------% SENSOR(a,self,graf_sen,dades_sen,x_obs,y_obs,ra_obs)%% Función de detección de obstáculos con los sensores del Robot.% self = vector de estado del Robot% self = [xa ya theta radius velolineal veloangular pertur]% graf_sen = representación gráfica de los sensores (objeto tipo line)% dades_sen = matriz de datos de los sensores% dades_sen(i,:) = [distancia_detección, ángulo_detección,% ang_rotación_sensor, position_dis, position_ang]% x_obs-y_obs = coordenadas del obstáculo (otro Robot u obstáculo)% ra_obs = radio del obstáculo% theta_obs = ángulo de rotación de obstáculo% ESTADO_SENSORES vector de estado de los sensores% ESTADO_SENSORES(índice del sensor) = estado del sensor% estado del sensor = 0 NO detecta obstáculo% estado del sensor = 1 SI detecta obstáculo% tipo = 1 obstáculo cuadrado% tipo = 0 obstáculo circular%% La función cambia el color del sensor a rojo, en caso de que se detecte% el obstáculo. Si no se detecta% ningún obstáculo, el sensor no varia su estado.%% Como obstáculo se entiende cualquier elemento de la simulación, un% verdadero obstáculo u otro Robot, ya que% con los parámetros de entrada no se distinguen tipos de elementos de la% simulación. Ahora bien, en caso de% ser cuadrado, hay que tener en cuenta que es un caso especial, ya que% según desde el ángulo que se mire el% obstáculo, ofrece diferentes distancias desde el contorno de éste al% origen del sensor. En cambio, un obstáculo circular siempre tiene la% misma distancia.% -----------------------------------------------------------------------

Anexo A. Programa

266

global MAX_SENSORS

theta = self(3); % Ángulo de rotación del Robot

% Datos de los sensores del Robot en gráfica = graf_sen --- datos =dades_senfor index= 1:MAX_SENSORS % Bucle de comprobación de todos lossensores (index = índice del sensor)

if dades_sen(index,1)~=0 % existe sensor ? ? ?

dis_obs = ra_obs; % Distancia igual al radio del obstáculo (siobstáculo circular) ra_obs1 = ra_obs;

% Obtener las coordenadas de origen del sensor x_sen = self(1) + dades_sen(index,4) *cos(dades_sen(index,5)+theta); y_sen = self(2) + dades_sen(index,4) *sin(dades_sen(index,5)+theta);

% Calculo del ángulo coordenada origen del sensor-coordenadacentral del obstáculo angle = atan2((y_obs-y_sen),(x_obs-x_sen));

% Calculo de la distancia coordenada origen del sensor-coordenadacentral del obstáculo distance2 = sqrt((x_sen - x_obs)^2 + (y_sen - y_obs)^2);

if tipo == 1 % Si el obstáculo es cuadrado% Corregir la distancia para adaptarla a la forma

cuadrada if angle < 0

angle2 = angle + pi; else angle2 = pi - angle;

end;

% Corregir ancho del obstáculo según el ángulo existente entreorigen del sensor-coordenada

% central del obstáculo

while angle2 < -pi angle2 = angle2 + pi; end;

dis_obs = discua(angle2,ra_obs,theta_obs);

% Calcular "extremos" del sensor x1 = x_sen + cos((-dades_sen(index,2)/2)+theta+dades_sen(index,3)) * dades_sen(index,1); y1 = y_sen + sin((-dades_sen(index,2)/2)+theta+dades_sen(index,3)) * dades_sen(index,1);

x2 = x_sen +cos((dades_sen(index,2)/2)+theta+dades_sen(index,3)) *dades_sen(index,1); y2 = y_sen +sin((dades_sen(index,2)/2)+theta+dades_sen(index,3)) *dades_sen(index,1);

Anexo A. Programa

267

% Calculo de la distancia extremo1 sensor-centro del obstáculo dis1 = sqrt((x1 - x_obs)^2 + (y1 - y_obs)^2);

% Calculo de la distancia extremo1 centro del obstáculo dis2 = sqrt((x2 - x_obs)^2 + (y2 - y_obs)^2);

if dis1 <= dis2 % Calculo del ángulo extremo1 sensor-centro del obstáculo angle2 = atan2((y_obs-y1),(x_obs-x1)); else % Calculo del ángulo extremo1 sensor-centro del obstáculo angle2 = atan2((y_obs-y2),(x_obs-x2)); end;

distan = discua(angle2,ra_obs,theta_obs);

if distan < dis1 | distan < dis2 ra_obs1 = distan; end;

clear angle2 dis1 dis2 end; % end if tipo==1

% Distancia entre origen del sensor y el contorno del obstáculo distance = distance2 - dis_obs;

if (distance < dades_sen(index,1)) % Distancia existente, menor quela distancia de detección del sensor

if angle < 0 angle = (2*pi) + angle;end;

angle = abs(angle-theta-dades_sen(index,3)); % Margen den ángulo±90º

if angle > 2*pi % Margen a -90º angle = angle - 2*pi;end;

% Aplicación del teorema del coseno, para calcular el ángulorelativo al tamaño del obstáculo

a = (((ra_obs1^2)-(distance2^2)-(dades_sen(index,1)^2))/(-2*distance2*dades_sen(index,1)));

phi = acos(a); % Ángulo de corrección según tamaño del obstáculo

if distance2 <= dades_sen(index,1) % Si el sensor rebasatotalmente el centro obstáculo, se asigna

phi = asin(ra_obs1/distance2); % el máximo ángulo decorrección

end;

% ángulo total de detección del sensor = (ángulo_detección +ángulo_relativo_al_tamaño_del_obstáculo)

angle_detec = ((dades_sen(index,2))/2) + phi;

if (angle <= angle_detec) | (angle >= ((2*pi) - angle_detec))

% Detección del obstáculo. Color del sensor rojo y su estado = 1

Anexo A. Programa

268

set(graf_sen(index),'color','r'); ESTADO_SENSORES(index) = 1;

end; % end if (angle <= angle_detec) ... end; % end if (distance<dades_sen(index,1)) end; % end if dades_sen(index,1)~= 0end; % end de for index = 1:MAX_SENSORS

clear theta angle_detec x_sen y_sen index distance angle distance2 phix_obs y_obs ra_obs tipo ra_obs1

function [selfa,selfdia] = assig_po(a,selfa,selfobj)

% -----------------------------------------------------------------------% ASSIGNAR_POSITION(a,selfobj,selfa) Grasping%% Asignación de las coordenadas al setpoint (diana), en el movimiento% tipo grasping.% Esta función se encarga de realizar el set de las coordenadas de la% diana, devolviendo en el parámetro de salida "selfdia" la consigna de% posición, controlando así la trayectoria del Robot.% a = índice del Robot% selfobj = vector de estado del objeto a grasping% --- selfobj = [xobj yobj theta radius remocaldo] ---% selfdia = vector de estado de la diana (setpoint)% --- seldia = [xdia ydia libre] ---% selfa = vector de estado del Robot% --- selfa = [xa ya theta Ra velolineal veloangular pertur] --%%% En el movimiento Grasping, siempre se persigue el obtener el objeto% triangular, y una vez% conseguido (transportado) se lleva a las coordenadas de destino.% -----------------------------------------------------------------------% SEE ALSO: assigpo2(), assigpo3(), assigpo4()

if selfobj(5) == 0 % NO transporte

% Coordenadas de consigna iguales a las del objeto xdia = selfobj(1);

% El destino del Robot, son las coordenadas del objeto ydia = selfobj(2);

elseif selfobj(5) == 1 % SI transporte xdia = 120 - a * 5; % Coordenadas de consigna del Robot, iguales a

% las coordenadas de destino para dejar el objeto ydia = 75;end; % end if selfobj(5) == 0

selfdia = [xdia ydia 0]; % Asignación vector de datos de la diana% (setpoint) parámetro de salida

clear xdia ydia

Anexo A. Programa

269

function [selfobj,selfa] = object(a,selfa)

% -----------------------------------------------------------------------% [selfobj,selfa] = OBJECT(a,selfa)%% En esta función se controla la distancia entre el Robot y el Objeto% "grasping" asociado a este% Robot. Cuando el objeto es tocado por el Robot, éste pasa a ser% transportado.% Una vez el objeto es transportado, en esta misma función se controla la% distancia entre el Robot% y las coordenadas de destino del objeto (xdes = 120 - a * 5, ydes =% 75), para que el objeto deje% de ser trasnportado.% selfobj(5) = 1 Objeto remolcado por el Robot% selfobj(5) = 0 Objeto NO remolcado por el Robot%% Los parámetros de salida, corresponden a los vectores de estado del% Robot y del objeto.% selfa = [xa ya theta Ra velolineal veloangular pertur] datos% propios del Robot% selfobj = [xobj yobj theta radius ¿remocaldo?] datos propios del% Objecto% -----------------------------------------------------------------------global OBJECT_GRAS par_max par_car_gras

% selfa = [xa ya theta Ra velolineal veloangular pertur] datos propiosdel Robotxa = selfa(1); % "xa" valor de coordenada x del Robotya = selfa(2); % "ya" valor de coordenada x del Robottheta = selfa(3); % "theta" ángulo de giro del RobotRa = selfa(4); % "Ra" radio del Robot

%per_carga = par_max/20; % Aumento perturbación debido a la cargadel objetoper_carga = par_car_gras; % Par de carga para los motores del objetograsping

% Comprobar si el Robot llega al Objeto

if (OBJECT_GRAS(a) ~= 0) % Existe objeto ?

% selfobj = [xobj yobj theta radius ¿remocaldo?] datos propios del % Objecto selfobj = ggras(a,'self'); MAR_FINAL = ggras(a,'MAR_FINAL');

if selfobj(5) == 0 % Objeto NO transportado

% Coordenadas del Objeto asociado al Robot "a" (grasping) xobj = selfobj(1); yobj = selfobj(2); Robj = selfobj(4)/3; % Distancia entre Robot y objeto distance = sqrt((xa - xobj)^2+(ya - yobj)^2); % Angulo entre Robot y objeto angle = atan2((ya - yobj),(xa - xobj));

if distance <= (Ra + Robj) % Condición de choque por distancia% con el objeto

Anexo A. Programa

270

selfobj(5) = 1; % Objeto pasa ha ser remolcado por el Robotset(MAR_FINAL,'visible','on'); % visualizar destinoselfobj(1) = xa; % Coordenadas objeto iguales a las coordenadas

% del Robotselfobj(2) = ya; % Remolque del objetoselfa(7) = selfa(7) + per_carga; % Aumento de la perturbación en

% el Robot debido a la carga del objeto end; % end if distance <= (Ra + Robj)

elseif selfobj(5) == 1 % Objeto SI transportado

% Coordenadas objeto iguales a las coordenadas del Robot selfobj(1) = xa;

selfobj(2) = ya; % Remolque del objeto% Aumento de la perturbación en el Robot debido a la carga del% objeto

selfa(7) = selfa(7) + per_carga;

% Coordendas de destino del Robot para dejar el objeto xdes = 120 - a * 5; ydes = 75; Rdes = selfobj(4)/3;

% Distancia entre Robot-destino del objeto distance = sqrt((xa - xdes)^2 + (ya - ydes)^2);

% El objeto deja de ser transportado. Se deja en las coordenadas de% destino

if distance <= (Ra + Rdes)set(MAR_FINAL,'visible','off');% Volver a buscar el Objeto a las coordenadas iniciales para% empezar de nuevoselfobj(1) = 10*a;selfobj(2) = 15;selfobj(5) = 0; % Objeto NO remolcado por el Robot% Disminución de la perturbación en el Robot debido a la descarga% del objetoselfa(7) = selfa(7) - per_carga;

end; % if distance <= (Ra + Rdes) end; % end if selfobj(5) == 0end; % end if if (OBJECT_GRAS(a) ~= 0)

% Vaciar el espacio de trabajo utilizado en esta funciónclear angle distance xdes ydes Rdes xdia ydia Rdia xa ya Ra per_cargaclear object

Anexo A. Programa

271

function [selfa,selfdia] = assig_po2(a,selfa)

% -----------------------------------------------------------------------% ASSIGNAR_POSITION(a,selfa) Tracking%% Asignación de las coordenadas al setpoint (diana), en el movimiento% tipo tracking.% Esta función se encarga de realizar el set de las coordenadas de la% diana, devolviendo en el parámetro de salida "selfdia" la consigna de% posición, controlando así la trayectoria del Robot.% a = índice del Robot% selfdia = vector de estado de la diana (setpoint)% --- seldia = [coordenada"x" coordenada"y" libre] ---% selfa = vector de estado del Robot% --- selfa = [xa ya theta Ra velolineal veloangular pertur] --%% El Robot persigue a la diana (setpoint), que describe una determina% figura en su movimiento.% Opción = 1 Circular con desfase% Opción = 2 Elipse con desfase% Opción = 3 Circular con desfase, pero con un radio decreciente a% cada vuelta% Opción = 4 Cuadrado% -----------------------------------------------------------------------% SEE ALSO: assig_po(), assigpo(), assigpo3(), assigpo4()

global xobj_coor yobj_coor indice radio_cir TRAILDIANA color

ELEMENTOS = 1200;

if indice == ELEMENTOS indice = 0; if radio_cir < 5 % Control de radio en el movimiento tipo 3 radio_cir = 37.5; end % end radio_cir < 5 radio_cir = radio_cir - 2.5;end % end if indice == ELEMENTOS

indice = indice + 1;

% Movimiento de la diana en forma de figura: circulo, elipse, etc ...angdia = linspace(0,2*pi,ELEMENTOS);

figura = get(xobj_coor,'Value'); % Figura seleccionada, número?

% Movimiento por defecto de la diana TRACKING. Circulo con ángulo de% desfase (opción 1)xdia = 65 + 30 * cos(angdia(indice));ydia = 45 + 30 * sin(angdia(indice) +str2num(get(yobj_coor,'string'))*pi/180);

if figura == 2 % Elipse, con ángulo de desfase (opción 2)

xdia = 65 + 20 * sin(angdia(indice)) * sin(angdia(indice)); ydia = 45 - 40 * sin(angdia(indice)) *cos(str2num(get(yobj_coor,'string'))*pi/180);

elseif figura == 3 % Circulos cada vez más pequeños, espiral (opción 3) xdia = 65 + radio_cir * cos(angdia(indice));

Anexo A. Programa

272

ydia = 45 + radio_cir * sin(angdia(indice)) *cos(str2num(get(yobj_coor,'string'))*pi/180);

elseif figura == 4 % Movimiento cuadrado (opción 4) radio = 50; if angdia(indice) <= (pi/4) | angdia(indice) >= (7*pi/4) xdia = 65 + radio * cos(pi/4); ydia = 45 + radio * sin(angdia(indice)); elseif angdia(indice) > (pi/4) & angdia(indice) <= (3*pi/4) xdia = 65 + radio * cos(angdia(indice)); ydia = 45 + radio * sin(pi/4); elseif angdia(indice) > (3*pi/4) & angdia(indice) <= (5*pi/4) xdia = 65 - radio * cos(pi/4); ydia = 45 + radio * sin(angdia(indice)); elseif angdia(indice) > (5*pi/4) & angdia(indice) < (7*pi/4) xdia = 65 + radio * cos(angdia(indice)); ydia = 45 - radio * sin(pi/4); end;end; % end if get(yobj_coor,'string') == 1

% Patch gráfico que define la forma de la figura que describe la dianap = patch('XData', [xdia+.1 xdia+.1 xdia-.1 xdia-.1],'EraseMode', 'xor','YData', [ydia-.1 ydia+.1 ydia+.1 ydia-.1 ],'FaceColor',color(a),'EdgeColor', 'none');

TRAILDIANA = [TRAILDIANA p]; % Rastro de la diana

selfdia = [xdia ydia 0]; % Asignación vector de datos de la diana(parámetro de salida)

clear ELEMENTOS libre xdia ydia angdia p figura radio

Anexo A. Programa

273


% -----------------------------------------------------------------------% ASSIG_POSITION3(a,selfa)%% Asignación de las coordenadas al setpoint (diana), en el movimiento% tipo forward.

% Esta función se encarga de realizar el set de las coordenadas de la% diana, devolviendo en el parámetro de salida "selfdia" la consigna de% posición, controlando así la trayectoria del Robot.% a = índice del Robot% selfdia = vector de estado de la diana (setpoint)% --- seldia = [coordenada"x" coordenada"y" libre] ---% selfa = vector de estado del Robot% --- selfa = [xa ya theta Ra velolineal veloangular pertur] --% libre = variable para evitar obstáculo%% El movimiento Forward permite al Robot el tomar una trayectoria de% siempre hacia adelante.% -----------------------------------------------------------------------% SEE ALSO: assig_po(), assigpo2(), assigpo4()

% Datos correspondientes al Robot "a" self = [xa ya theta Ra velolinealveloangular pertur]

xa = selfa(1); % Coordenada x del Robotya = selfa(2); % Coordenada y del Robottheta = selfa(3); % Ángulo del RobotRa = selfa(4); % Radio del Robot

% Movimiento del setpoint (diana), sigue la dirección del Robot, siemprehacia adelantexdia = xa + 8 * Ra * cos(theta);ydia = ya + 8 * Ra * sin(theta);

selfdia = [xdia ydia 0]; % Asignación vector de datos de la dianaparámetro de salida

clear xa ya theta Ra libre xdia ydia

Anexo A. Programa

274


% -----------------------------------------------------------------------% ASSIG_POSITION4(a,selfa)%% Asignación de las coordenadas al setpoint (diana), en el movimiento% tipo other.% Esta función se encarga de realizar el set de las coordenadas de la% diana, devolviendo en% el parámetro de salida "selfdia" la consigna de posición, controlando% así la trayectoria del Robot.% a = índice del Robot% selfdia = vector de estado de la diana (setpoint)% --- seldia = [coordenada"x" coordenada"y" libre] ---% selfa = vector de estado del Robot% --- selfa = [xa ya theta Ra velolineal veloangular pertur] --%% El valor de las coordenadas se recupera de la función sother(), donde a% partir de la matriz% COOR_DIANA(a,1) = x, COOR_DIANA(a,2) = y donde se han guardado.% -----------------------------------------------------------------------% SEE ALSO: assig_po(), assigpo2(), assigpo3()

[xdia ydia] = sother(a); % Obtener coordenadas de destino del Robotasignadas con el push-button Update

selfdia = [xdia ydia 0]; % Asignación vector de datos de la dianaparámetro de salida

clear xdia ydia a

A.11 Funciones de Control de Choques

function [self] = xoc_obstacles(self,selfobs);

% -----------------------------------------------------------------------% [self] = XOC_OBSTACLES(self,selfobs);%% Función de simulación del choque entre Robot y Obstáculo.% Como parámetro de entrada, son necesarios los vectores de datos delRobot y Obstáculo.% La salida de la función consiste en desviar la trayectoria del Robot en% caso que se produzca un choque (efecto de choque con el obstáculo).% self = [xa ya theta radius velolineal veloangular pertur] -- vector% datos del Robot% selfobs = [x y theta radius shape] -- vector datos del Obstáculo% shape = 0 circular% shape = 1 cuadrado% shape = 2 triangular% Además de no permitir la superposición entre el Robot y el obstáculo.%% Cuando la distancia entre ambos elementos es inferior a la suma de sus%radios, se produce el% choque.% -----------------------------------------------------------------------% SEE ALSO: xocparet, xocrobot

Anexo A. Programa

275

global par_max

Robs = selfobs(4); % Radio del obstáculo por defectoRrobot = self(4); % Radio del robot por defecto

% Calculo ángulo Robot-Obstáculoangle = atan2((self(2)-selfobs(2)),(self(1)-selfobs(1)));

if selfobs(5) == 1 % Si el obstáculo es cuadrado % Corregir ancho del obstáculo según el ángulo de choque Robs = discua(angle,selfobs(4),selfobs(3));end;

% Calculo distancia Robot-Obstáculodistance = sqrt((self(2)-selfobs(2))^2+(self(1)-selfobs(1))^2);

if distance <= (Rrobot + Robs) % Se produce choque obstáculo-Robot?

per_choque = par_max/4; % Factor de aumento perturbación debido alchoque entre obstáculo-Robot distan_xoc = 0.15; % Factor de rebote del Robot debido al choqueentre obstáculo-Robot ang_xoc = 0*pi/180; % Factor de aumento ángulo Robot debido al choqueentre obstáculo-Robot

% Ángulo del Robot al Obstáculo % angle = atan2((selfobs(2)-self(2)),(selfobs(1)-self(1)))

% Perturbación par motores Robot al producirse choque self(7) = self(7) + per_choque;

% Corregir coordenadas del Robot, para que no se superponga al Obstáculo% self(1) = selfobs(1) + cos(angle) * (Rrobot + selfobs(4) + distan_xoc);% self(2) = selfobs(2) + sin(angle) * (Rrobot + selfobs(4) + distan_xoc);

self(1) = selfobs(1) + cos(angle) * (Rrobot + Robs + distan_xoc); self(2) = selfobs(2) + sin(angle) * (Rrobot + Robs + distan_xoc);

if (cos(angle) < 0 & cos(self(3)) > 0) | (cos(angle) > 0 &cos(self(3)) < 0)

% Corregir ángulo Robot debido al choque self(3) = self(3) + ang_xoc;end; % si el choque es frontal

end; % end if distance <= (self(4) + selfobs(4))

clear distance angle selfobs per_choque ang_xoc Robs Rrobot angle2

Anexo A. Programa

276

function [factor_xoc,self] = parets(self)

% -----------------------------------------------------------------------% [factor_xoc] = PARETS(self)%% Esta función, se encarga del factor de reducción de la velocidad del% Robot cuando se produce% el choque del Robot con las paredes (límites) del terreno de% simulación.%% El parámetro de entrada en la función, corresponde al vector de estado;% posición, ángulo y velocidad del Robot.% self = [x y theta Ra v omega pertur] internal state of Robot "a"%% El parámetro de salida, "factor_xoc", es el factor de reducción de% velocidad debido al roce con los límites por parte del Robot.% factor_xoc ~ 0 roce elevado.% factor_xoc ~ 1 roce mínimo.%% Para conocer si realmente el Robot choca con los límites, se hace% servir la función xocparet.% -----------------------------------------------------------------------% SEE ALSO: xoc_paret()

global perturba

perturba = 0; % Inicializar posible perturbación por choque paredes a 0.factor_xoc = 0; % Inicialzar factor de roce-choque a 0.

% self = [x y theta Ra v omega pertur] internal state of Robot "a"x = self(1); % Coordenada "x" del Robot-jugador "a"y = self(2); % Coordenada "y" del Robot-jugador "a"theta = self(3); % Angulo del Robot-jugador "a"Ra = self(4); % Radio-anchura del Robot-jugador "a"

% Existe roce con límites este y oesteif xocparet('nord',self) & xocparet('oest',self) %'Nord - Oest' factor_xoc = cos(theta-pi/4);% Si hay roce, entonces factor_xoc < 1

% Existe roce con límites sur y oesteelseif xocparet('sud',self) & xocparet('oest',self) %'Sud - Oest' factor_xoc = cos(theta-3*pi/4);% Si hay roce, entonces factor_xoc < 1

% Existe roce con límites sur y esteelseif xocparet('sud',self) & xocparet('est',self) %'Sud - Est' factor_xoc = cos(theta+3*pi/4);% Si hay roce, entonces factor_xoc < 1

% Existe roce con límites norte y esteelseif xocparet('nord',self) & xocparet('est',self) %'Nord - Est' factor_xoc = cos(theta+pi/4);% Si hay roce, entonces factor_xoc < 1

% Existe roce con límite del oesteelseif xocparet('oest',self) %'Oest' factor_xoc = cos(theta-pi/2);% Si hay roce, entonces factor_xoc < 1

Anexo A. Programa

277

% Existe roce con límite del esteelseif xocparet('est',self) %'Est' factor_xoc = cos(theta+pi/2);% Si hay roce, entonces factor_xoc < 1

% Existe roce con límite del surelseif xocparet('sud',self) %'Sud' factor_xoc = cos(theta-pi);

% Existe roce con límite del esteelseif xocparet('nord',self) %'Nord' factor_xoc = cos(theta);% Si hay roce, entonces factor_xoc < 1

end % end if xocparet...

if factor_xoc<0 % 0 <= Factor de choque siempre <= 1 factor_xoc = 0;end % end if factor_xoc<0self(7) = perturba;

factor_xoc = (1 - factor_xoc);

clear global perturba

function [ret,self] = xocparet(car,self)

% -----------------------------------------------------------------------% [ret] = XOCPARET(car,self)%% En esta función se comprueba la situación del Robot respecto a los% límites de la simulación.% Cuando el Robot se encuentra muy cercano a cualquiera de los límites,%ya sea Norte, Sur, Este% u Oeste, coincidiendo su posición con el parámetro de entrada "car", la% función retorna como parámetro de salida ret = 1. En caso que el Robot% este alejado de los límites, ret = 0.% car = Norte Sur Este Oeste% self = [x y theta Ra v omega pertur] --- Vector de estado del% Robot% [mxc, mxf] = [130,90] --- Máxinma coordenada x-y de% la simulación.% [0,0] --- Mínima coordenada x-y de la% simulación.% -----------------------------------------------------------------------% SEE ALSO: xocobsta, xocrobot

global par_max perturba

mxc = 130; % Máxinma coordenada x del terreno de juegomxf = 90; % Máxinma coordenada y del terreno de juegoRAO = 0.98; % Factor de reducción

x = self(1); % Coordenada "x" del Robot-jugador "a"y = self(2); % Coordenada "y" del Robot-jugador "a"theta = self(3); % Angulo del Robot-jugador "a"Ra = self(4); % Radio-anchura del Robot-jugador "a"

Anexo A. Programa

278

proximitat = Ra/RAO;

if strcmp(car,'est') % Comprobar punto cardinal del Robot if y <= proximitat % Se supera el límite ??? %'Est'; ret=1; % Si !!!, se supera el límite reduc_par_car = abs(sin(theta)); % Reducción perturbación choquesegún ángulo del Robot else ret=0; % No !!!, se supera el límite end % end if y < proximitat

elseif strcmp(car,'oest')% Comprobar punto cardinal del Robot if y >= mxf-proximitat% Se supera el límite ??? %'Oest'; ret=1; % Si !!!, se supera el límite reduc_par_car = abs(sin(theta)); % Reducción perturbación choquesegún ángulo del Robot else ret=0; % No !!!, se supera el límite end % end if y > mxf-proximitat

elseif strcmp(car,'sud')% Comprobar punto cardinal del Robot if x <= 0+proximitat % Se supera el límite ??? %'Sud' ret=1; % Si !!!, se supera el límite reduc_par_car = abs(cos(theta)); % Reducción perturbación choquesegún ángulo del Robot else ret=0; % No !!!, se supera el límite end % end if x <= 0+proximitat

elseif strcmp(car,'nord')% Comprobar punto cardinal del Robot if x >= mxc-proximitat% Se supera el límite ??? %'Nord' ret=1; % Si !!!, se supera el límite reduc_par_car = abs(cos(theta)); % Reducción perturbación choquesegún ángulo del Robot else ret=0; % No !!!, se supera el límite end % end if x >= mxc-proximitat

end % end if strcmp(car,'est')

per_choque = (par_max/10) * reduc_par_car; % Factor de perturbacióndebido al choque entre pared-Robot

% Perturbación del par de los motores debido al choque con los límitesif ret == 1 perturba = self(7) + per_choque; % Perturbación par motores Robot alproducirse choque con la paretend % end if ret == 1

clear per_choque RAO proximitat Ra x y theta mxf mxc reduc_par_car

return;

Anexo A. Programa

279

function [self1] = xoc_robot(self1,j,self2);

% -----------------------------------------------------------------------% [self1] = XOC_ROBOTS(self1,j,self2);%% Función de simulación del choque entre Robots.% Como parámetros de entrada, son necesarios los vectores de datos de los% Robots, así como el% índice del Robot del que no se retorna su vector de estado.% La salida de la función consiste en desviar la trayectoria de los% Robots en caso que se% produzca el choque entre ambos.%% self = [xa ya theta radius velolineal veloangular pertur] -- vector% datos de los Robots% "self1" vector de estado Robot1% "j" índice del Robot2% "self2" vector de estado Robot2%% El parámetro de salida, self1 (vector de estado del Robot1) corrigue la% posición del Robot% debido al choque evitando la superposición entre Robots.% -----------------------------------------------------------------------% SEE ALSO: xocobsta(), xocparet()

global par_max

angle_xoc = (20*pi/180);% Ángulo de corrección debido al choque entreRobotsfactor_xoc = 1; % Factor de corrección de la posición debido alchoque entre Robotsper_choque = par_max/7; % Aumento perturbación debido al choque entreRobots

% Ángulo del Robot1 al Robot2angle = atan2((self2(2) - self1(2)),(self2(1) - self1(1)));

Rrobot1 = self1(4); % Radio del Robot 1Rrobot2 = self2(4); % Radio del Robot 2

% Calculo ángulo Robot-Obstáculoangle = atan2((self2(2)-self1(2)),(self2(1)-self1(1)));

% Corregir ancho del Robot 1 según el ángulo de choqueRrobot1 = discua(angle,self1(4),self1(3));

if angle < 0 angle2 = angle + pi;else angle2 = pi - angle;end;

% Corregir ancho del Robot 1 según el ángulo de choqueRrobot2 = discua(angle2,self2(4),self2(3));

% Calculo distancia Robot1-Robot2distance = sqrt((self1(2) - self2(2))^2 + (self1(1) - self2(1))^2);

if distance <= (Rrobot1 + Rrobot2) % Existe choque ?

Anexo A. Programa

280

if angle < 0 angle = 2*pi + angle; end;

if (self1(4) >= self2(4)) % Si el tamaño Robot "a" no es mayor, sihay choque pero

% no hay efectos del choque

% Si existe la misma dirección de movimiento, no hay choque if (sin(self1(3))<0 & sin(self2(3))<0) | (sin(self1(3))>0 &sin(self2(3))>0)

if (cos(self1(3))<0 & cos(self2(3))<0) | (cos(self1(3))>0 &cos(self2(3))>0)

self2(1) = self1(1) + cos(angle)*(Rrobot2 + Rrobot1); self2(2) = self1(2) + sin(angle)*(Rrobot2 + Rrobot1);

% Dibujar Robot "j" con las coordenadas corregidas debido alchoque self2(7) = self2(7) + per_choque; % Perturbación Robot "j"al producirse un choque

sagent(j,'Self',self2); return;end;

end; % end if (sin(self1(3)) < 0 & ...

self2(7) = self2(7) + per_choque; % Perturbación Robot"j" alproducirse un choque self1(7) = self1(7) + per_choque; % Perturbación Robot"a" alproducirse un choque

% Corregir las coordenades Robot "j" por el choque self2(1) = self1(1) + cos(angle) * (Rrobot2 + Rrobot1 +factor_xoc); self2(2) = self1(2) + sin(angle) * (Rrobot2 + Rrobot1 +factor_xoc);

angle = angle - self1(3);

if angle<0 angle = 2*pi + angle;

end;

% Correción del ángulo Robot "a" debido al choque, según en queángulo se produce el choque % entre Robots if ((angle>(pi/8)) & (angle<(pi/2))) | ((angle>(5*pi/4)) &(angle<(3*pi/2)))

self1(3) = self1(3) - angle_xoc; end;

if ((angle<(3*pi/4)) & (angle>(pi/2))) | ((angle>((2*pi)-(pi/8))) &(angle>(3*pi/2)))

self1(3) = self1(3) + angle_xoc; end;

% Set Robot "j" con las coordenadas corregidas debido al choque sagent(j,'Self',self2);

else % else if self1(4)>=self2(4)

Anexo A. Programa

281

% Corregir coordenadas Robot "a" por el choque (no superposición) self1(1) = self2(1) - cos(angle)*(Rrobot2 + Rrobot1); self1(2) = self2(2) - sin(angle)*(Rrobot2 + Rrobot1); self1(7) = self1(7) + per_choque; % Perturbación Robot"a" alproducirse un choque

end; % end if self1(4) >= self2(4)end; % end if distance < (Rrobot1 + Rrobot2)

clear distance angle factor_xoc per_choque j self2 angle_xoc angle2

A.12 Función de Tratamiento Gráfico de los Resultados

function [] = plotdata()

% -----------------------------------------------------------------------% PLOTDATA()%% Esta función realiza la representación gráfica de los resultados;%coordenadas del Robot,% intensidad de motores, par motores, velocidad lineal y velocidad% angular. Se representan los datos del Robot seleccionado en el popup de% la pantalla de simulación. La principal orden utilizada en esta función% es plot() (representación bidimensional).% -----------------------------------------------------------------------global AGENTS plotButton agNum dt text1_error x_ag y_ag log_vlinlog_vang logv_m_d logi_m_d...

logv_m_i logi_m_i logp_m_d logp_m_i color

tama_letra = 18;tama_letra2 = 14;


% Índice del Robot (leer de la pantalla).i = popup(AGENTS,agNum,1,text1_error,'Warning: Not exist Robot');

if i == 0 set(plotButton,'Value',0); return;end

% Comprobar si hay datos para realizar gráficaif length(log_vlin(i,:)) == 1 % Mensaje de error. En este caso no haydatos set(text1_error,'String','Warning: Not exist data','visible','on'); return;end;

% -----------------------------------------------% Gráfica de Velocidad Lineal y angular del Robot% -----------------------------------------------

figure('Name',['Graphic of the Dates of Velocities Robot 'num2str(i)],'interruptible','yes',...


[b] = length(log_vlin(i,:));

Anexo A. Programa

282

t = [0:dt:(b-1)*dt]; % Vector de tiempos. División temporal cada graintime segundos.subplot(2,1,1);plot(t,log_vlin(i,:),color(i));axis([0 max(t) min(log_vlin(i,:)) max(log_vlin(i,:))+0.5]);title(['Lineal Velocity Robot Number: 'num2str(i)],'fontsize',tama_letra);ylabel('Velo. (cm/s)','fontsize',tama_letra);grid on;

subplot(2,1,2);plot(t,log_vang(i,:),color(i));axis([0 max(t) min(log_vang(i,:)) max(log_vang(i,:))+0.1]);title(['Angular Velocity Robot Number: 'num2str(i)],'fontsize',tama_letra);xlabel('Time s.','fontsize',20);ylabel('Velo. angu. (rad/s)','fontsize',tama_letra);grid on;zoom;

% ----------------------------% Gráfica de Coordenadas Robot% ----------------------------

figure('Name', ['Graphic of the Coordenates Robot 'num2str(i)],'interruptible','yes',...


plot(x_ag(i,:),y_ag(i,:),color(i));axis([0 130 0 90]);grid on;title(['Evolution coordinates Robot Number: 'num2str(i)],'fontsize',tama_letra);xlabel('Coordinate X (cm)','fontsize',tama_letra);ylabel('Coordinate Y (cm)','fontsize',tama_letra);legend('Robot');

[b] = length(logv_m_i(i,:));t = [0:dt:(b-1)*dt]; % Vector de tiempos. División temporal cada graintime segundos.

c(1) = max(logv_m_d(i,:)); % Valor máximo del vector de velocidad del% motor derecho

c(2) = max(logv_m_i(i,:)); % Valor máximo del vector de velocidad del% motor izquierdo

a = max(c); % Valor maximo entre velocidades motor derecho-% izquierdo.

c(1) = min(logv_m_d(i,:)); % Valor mínimo del vector de velocidad del% motor derecho

c(2) = min(logv_m_i(i,:)); % Valor mínimo del vector de velocidad del% motor izquierdo

d = min(c); % Valor mínimo entre velocidades motor derecho-% izquierdo.

% ---------------------------------------% Gráfica referente a los datos del motor% ---------------------------------------figure('Name',['Graphic of the Dates of the Motors Robot 'num2str(i)],'interruptible','yes','Units', 'normalized', 'Position', [0 01 1]);

Anexo A. Programa

283

subplot(3,1,1);plot(t,logv_m_d(i,:),'b:'); % Representación de la velocidad motorderechohold on;plot(t,logv_m_i(i,:),'m--'); % Representación de la velocidad motorizquierdoaxis([0 max(t) d (a+2)]);title(['Velocity of the Motors Robot Number: 'num2str(i)],'fontsize',tama_letra);

legend('Motor Right','Motor Left');

ylabel('Velo. Motors (rad/s)','fontsize',tama_letra);grid on;

c(1) = max(logi_m_d(i,:)); % Valor máximo del vector de intensidad delmotor derechoc(2) = max(logi_m_i(i,:)); % Valor máximo del vector de intensidad delmotor izquierdoa = max(c); % Valor maximo entre intensidad motor derecho-izquierdo.c(1) = min(logi_m_d(i,:)); % Valor mínimo del vector de intensidad delmotor derechoc(2) = min(logi_m_i(i,:)); % Valor mínimo del vector de intensidad delmotor izquierdod = min(c); % Valor mínimo entre intensidad motor derecho-izquierdo.

subplot(3,1,2);plot(t,logi_m_d(i,:),'b:'); % Representación de la intensidad motorderechohold on;plot(t,logi_m_i(i,:),'m--'); % Representación de la intensidad motorizquierdoaxis([0 max(t) d a]);title(['Intensity of the Motors Robot Number: 'num2str(i)],'fontsize',20);grid on;ylabel('Intesity (A)','fontsize',tama_letra);

c(1) = max(logp_m_d(i,:)); % Valor máximo del vector de par del motor% derecho

c(2) = max(logp_m_i(i,:)); % Valor máximo del vector de par del motor% izquierdo

a = max(c); % Valor maximo entre intensidad motor derecho-izquierdo.c(1) = min(logp_m_d(i,:)); % Valor mínimo del vector de par del motor

% derechoc(2) = min(logp_m_i(i,:)); % Valor mínimo del vector de par del motor

% izquierdod = min(c); % Valor mínimo entre intensidad motor derecho-izquierdo.

subplot(3,1,3);plot(t,logp_m_d(i,:),'b:'); % Representación del par motor derechohold on;plot(t,logp_m_i(i,:),'m--'); % Representación del par motor izquierdoaxis([0 max(t) d a]);title(['Torque of the Motors Robot Number: ' num2str(i)],'fontsize',20);

xlabel('Time s.','fontsize',tama_letra);ylabel('Torque (N·m)','fontsize',tama_letra);

Anexo A. Programa

284

grid on;hold off;

clear a b c t tama_letraclear plotdatareturn;

A.13 Funciones Auxiliares

Función bounds

wy

wy

wwy

wwy

wwx

wwx

wx

wx

Función para la saturación de las coordenadas del Robot u otros elementos

Las variables wwx, wwy, wx, wy y R son variables tipo GLOBALS

Parámetros saturación : R = Radius Object, Xmax = 130, Ymax = 90

function [distan] = discua(angulo,radio_obs,angu_obs);

% -----------------------------------------------------------------------% [distan] = DISCUA(angulo,radio_obs,theta_obs);%% Esta función devuelve la distancia existente desde el centro del% obstáculo-Robot hasta su contorno, según el ángulo existente entre este% obstáculo-Robot y otro punto de referencia.%% angulo = ángulo entre el obstáculo-Robot y otro punto de referencia% radio_obs = radio del obstáculo-Robot% theta_obs = ángulo de rotación del obstáculo-Robot%% distan = Distancia desde el centro del obstáculo-Robot hasta su% contorno según el ángulo "angulo"% -----------------------------------------------------------------------angulo = angulo - angu_obs; % Corregir ángulo existente entre elobstáculo-Robot y otro punto

% de referencia según el ángulo de rotación delobstáculo-Robotwhile angulo < -pi angulo = angulo + pi;end;

% Distancia desde el centro del obstáculo-Robot hasta su contorno segúnel ángulo "angulo"if abs(angulo) <= (pi/4) | abs(angulo) >= (3*pi/4) % Distancia deseparación adaptada a la forma cuadrada distan = abs((radio_obs*cos((1/4)*pi)) / cos(angulo));else distan = abs((radio_obs*sin((1/4)*pi)) / sin(angulo));

Anexo A. Programa

285

end;end;

clear angulo radio_obs angu_obs

function[error,indice_sal] = noencima(selfobj,i,tipo)

% --------------------------------------------------------------------% [error] = NOENCIMA(selfobj,i,tipo)% selfobj = [xobj yobj theta radius...] vector de estado objeto a% comprobar que no hay superposición% "i" índice del elemento, ya sea Robot, obstáculo u objeto% grasping% "tipo" tipo de elemento referenciado --- Robot 'r' 'o'% Obstáculo ---%% Esta función responde con el parámetro de salida "error" igual a "1"% cuando el objeto referenciado con el vector de estado "selfobj" e% índice "i" se superpone en la pantalla de simulación a otro elemento% existente% en la simulación, ya sean Robots, objetos grasping u obstáculos.% --------------------------------------------------------------------global AGENTS OBSTACLES OBJECT_GRAS grasping

error = 0;MIN_DIS_OBSTA = 2; % Mínima distancia permitida entre obstáculos

% Control de choque entre los Robots, no superposiciónfor j = 1:length(AGENTS) % existe Robot

% Recuperar "UserData" state de los Robots if ((AGENTS(j) ~= 0) & (~error) & ((j~=i) | tipo == 'o')) % ExisteRobot?

% Recuperar vector datos Robot 'j' self = [x y theta radiusvelolineal veloangular pertur] selfa = gagent(j,'Self'); % Vector de estado del segundo Robot

% Calculo distancia Robot 'j'-Objeto entrada distance = sqrt((selfobj(2)-selfa(2))^2+(selfobj(1)-selfa(1))^2);

if distance <= (selfobj(4) + selfa(4)) % Superposición ? error = 1; % SI ! superposición con Robot 'j' end; % end if distance <= (selfobj(4) + selfa(4))

% Existe objeto "grasping"if (get(grasping,'value') & OBJECT_GRAS(j) ~= 0 & (~error))% Recuperar vector datos objeto grasping 'j' selfgras = [xgras% ygras theta radius remocaldo]selfgr = ggras(j,'Self'); % Vector de estado del objeto

% grasping 'j'

% Calculo distancia objeto entrada-objeto grasping 'j'distance = sqrt((selfobj(2)-selfgr(2))^2+(selfobj(1)-selfgr(1))^2);

if distance <= (selfgr(4) + selfobj(4)) % Superposición ? error = 1; % SI ! superposición con objeto "grasping" "j"end; % end if distance <= (selfgr(4) + selfobj(4))

end; % end if get(grasping,'value')

Anexo A. Programa

286

end; % end if (i ~= 0)end; % end for j = 1:length(AGENTS)

if ~error %& tipo ~= 'o' % Superposición obstáculos-Robots ? % Control superposición con los Obstáculos de la simulación for j = 1:length(OBSTACLES)

% Recuperar "UserData" state del obstáculo state = get(OBSTACLES(j), 'UserData'); u = state(1);

if (u ~= 0) & (~error) % Existe obstáculo?

% selfobs = [x y theta radius shape] --- vector datos del Obstáculoselfobs = state(2:6);

% Calculo distancia objeto entrada-Obstáculodistance = sqrt((selfobj(2)-selfobs(2))^2+(selfobj(1)-

selfobs(1))^2);

Robs1 = selfobj(4); Robs2 = selfobs(4);

if selfobs(5) == 1 % Si el obstáculo es cuadrado % Corregir la distancia de separación para adaptarla a la forma % cuadrada

angle = atan2((selfobj(2)-selfobs(2)),(selfobj(1)-selfobs(1)));

% Corregir ancho del obstáculo según el ángulo Robs2 = discua(angle,selfobs(4),selfobs(3));

end;

% Si el obstáculo del parámetro de entrada es cuadrado if tipo == 'o' & selfobj(5) == 1

% Corregir la distancia de separación para adaptarla a la forma % cuadrada

angle2 = atan2((selfobs(2)-selfobj(2)),(selfobs(1)-selfobj(1)));

% Corregir ancho del obstáculo según el ángulo Robs1 = discua(angle2,selfobj(4),selfobj(3));

end;

% Superposición obstáculo - elemento de la simulaciónif distance < (Robs1 + Robs2) & tipo ~= 'o' error = 1; % SI ! superposición con obstáculo "j" indice_sal = u;end; % end if distance <= (selfobj(4) + selfobs(4))

% Superposición entre obstáculos ?%if distance < (Robs1 + Robs2) & tipo == 'o' & u ~= i% Superposición entre obstáculos ? % cumplir una mínima distanciaif distance <= (MIN_DIS_OBSTA) & tipo == 'o' & u ~= i error = 1; % SI ! superposición con obstáculo "j" indice_sal = u;end; % end if distance <= (selfobj(4) + selfobs(4))

end; % end if (i ~= 0)

Anexo A. Programa

287

end; % end for u = 1:length(OBSTACLES)end; % end if ~error

clear selfa selfobs selfobj selfgr u j state angle distance tipo angleclear noencima

function [indice] = popup(vector,pop,opcion,txt,mensa)

% ----------------------------------------------------------------------% [indice] = POPUP(vector,pop,opcion,txt,mensa)%% La función popup sirve para trabajar con los diferentes "uicontrol% popup" de las pantallas, donde se seleccionan-guardan los índices de% los elementos; Robots, obstáculos y sensores. Esta función devuelve en% caso de elegir la opción '1', el índice seleccionado con el "uicontrol% popup" referenciado en el parámetro de entrada 'pop'.% --- 'vector' vector de manejadores de los elementos (Robots,% obstáculos, etc.)% --- 'pop' manejador del "uicontrol popup"% --- 'opción' operación a realizar ('1' retornar índice '0' renovar% popup)% --- 'txt' manejador del mensaje de error de las diferentes% pantallas,% --- 'mensa' mensaje de error a presentar.%% Con la opción '0', simplemente se renueva el popup 'pop', con los% elementos del vector 'vector'.% ----------------------------------------------------------------------if opcion % Índice seleccionado en el popup ?

menu = find(vector); % Índices ~= 0 en el popup if ~isempty(menu); % Comprobar vector no vacío % get(pop,'Value') contiene la posición selecciona en el vectorpopup indice = menu(get(pop,'Value')); % Obtener índice, value =posición vector popup else indice = 0; % NO hay índice, presentar mensaje set(txt,'string',mensa,'visible','on'); return; end;

else % Renovar "popup" con los índices del vector 'vector' % Update Menu. Renovar índices del popup "pop" menu = find(vector); % Índices ~= 0 en el popup

if length(menu) == 0 % Comprobar que existen elementos indice = 0; set(pop,'String','','Value',0);

else % else if length(vector) == 0

indice = menu(1); s = sprintf('%2d',menu(1));

for j=2:length(menu) % Ordenar índicess = [s sprintf('|%2d',menu(j))];% Almacenar índices no vacios

end; % end for j=2:length(menu)

Anexo A. Programa

288

set(pop,'String',s); % Set en el popup referenciado

end; % end if length(vector) == 0end; % end if op...

A.14 Funciones de Fin de la Simulación

function [] = limpiar(i)

% -----------------------------------------------------------------------% LIMPIAR(i), clear all pantalla de simulación y datos gráficos.%% Función de clear del TRAIL dejado por el Robot (en el caso de que haya% sido activado), además,% de inicializar y borrar los datos gráficos (Logger).% En ambos casos es necesario que los respectivos buttons tipo radio% permanezcan activados, si se% desea realizar la operación de clear.%% En caso de existir parámetro de entrada (índice del Robot), se realiza% el clear de las variables% gráficas (opción de LOAD data).% -----------------------------------------------------------------------% Declaración de variables de la función

global AGENTS TRAIL TRAILDIANA text1_error x_ag y_ag log_vlinlog_vang logv_m_d logi_m_d...

logv_m_i logi_m_i logp_m_d logp_m_i minutos segundos dt_edit segunminu trailButton ...

loggButton


if nargin == 1 % Inicializar variables antes del LOAD Data

for j = 1:length(x_ag) % Actualizar las coordenadas actuales del Robot x_ag(i,j) = 0;y_ag(i,j) = 0; logv_m_d(i,j) = 0;logv_m_i(i,j) = 0; logi_m_d(i,j) = 0;logi_m_i(i,j) = 0; logp_m_d(i,j) = 0;logp_m_i(i,j) = 0; log_vlin(i,j) = 0;log_vang(i,j) = 0; end % end for j = 1:length(AGENTS)

return;end; % end if nargin == 1

% Limpiar los pequeños patch's gráficos de TRAILif get(trailButton,'value') & TRAIL ~= [] % Borrar si el Trail estáactivado delete(TRAIL); TRAIL = []; % Inicializar vector manejadores rastro de los Robotsend % end if get(trailButton,'value')

if TRAILDIANA ~= [] % Borrar recorrido de la diana (Tracking) delete(TRAILDIANA); TRAILDIANA = [];end % end if exist(TRAILDIANA ~= 0)

Anexo A. Programa

289

% Inicializar vectores del Logger de velocidades, y los Loggers de losdatos de los motores.if get(loggButton,'value') % Borrar si el Logger está activado

x_ag = 0;y_ag = 0; logv_m_d = 0;logv_m_i = 0; logi_m_d = 0;logi_m_i = 0; logp_m_d = 0;logp_m_i = 0; log_vlin = 0;log_vang = 0;

for j = 1:length(AGENTS) if AGENTS(j) ~= 0

% Inicializar las coordenadas para las variables gráficas% self = [coor."x" coor."y" theta radius velolineal veloangular

pertur]selfa = gagent(j,'Self');x_ag(j,1) = selfa(1);y_ag(j,1) = selfa(2);logv_m_d(j,1) = 0;logv_m_i(j,1) = 0;logi_m_d(j,1) = 0;logi_m_i(j,1) = 0;logp_m_d(j,1) = 0;logp_m_i(j,1) = 0;log_vlin(j,1) = 0;log_vang(j,1) = 0;selfa(5) = 0;selfa(6) = 0;selfa(7) = 0;sagent(j,'Self',selfa); % Inicializar vector de estado Robot "j"

end; % end if AGENTS(j)~= 0 end; % end for j = 1:length(AGENTS)

% Inicilaizar tiempos de simulación segun = 0;minu = 0; set(minutos,'string','0 min'); % Set buttons text inicializados set(segundos,'string','0 s'); set(dt_edit,'string','0 s');

end; % end if get(loggButton,'value')

clear limpiarclear j i

Anexo A. Programa

290

function [] = reset()% ----------------------------------------------------------------------% RESET()%% Función asociada al push button de ReSet en el programa de simulación.% Cierra todas las pantallas y vacia la memoria de variables.% Vuelve a cargar el programa de simulación (función comensa).% ----------------------------------------------------------------------quit1(0); % Salir del programa de simulacióncomensa; % Función de inicio del programa

A.15 Función de Cierre y Salida de las Diferentes Pantallas

function[error] = quit1(op)

% ----------------------------------------------------------------------% QUIT(op)%% Esta función se utiliza para cerrar las diferentes pantallas de diseño% cuando se sale de ellas por el label de la barra de herramientas% "Quit". Cierra la pantalla y limpia la memoria de variables no% necesarias. Cada opción de entrada esta asociada con una pantalla:% op = 0 Salir de la simulación, cerrar todas la pantallas y limpiar% toda la memoria.% op = 1 Salir de la pantalla de diseño del Robot.% op = 2 Salir de la pantalla de diseño de obstáculos.% op = 3 Salir de la pantalla de configuración.% ----------------------------------------------------------------------global AGENTS WORLD_AXIS stop

error = 0;

if op == 0 if stop == 1 % Antes de salir se ha de comprobar si la simulaciónesta en STOP close all; % Cerrar todas las pantallas-gráficas actuales. clear all; % Vacía el espacio de trabajo MATLAB V4.0. clear functions; clc; % Mensaje de despedida fprintf(1,'\n\n This program was developed by Albert Oller andSantos Barquero.\n\n'); end;

elseif op==1

% Salir de la pantalla de edición del Robot. Cierra esta pantalla y % guarda el nombre del fichero de control. En el caso, de que este % fichero no exista, se escribe el mensaje de aviso.

global DISPLAY2 nafi_con num_ro name_file text1_error text2_errorra_fi_con1 maes_fi

% num_ro = índice del Robot que se esta editando a = num_ro;

% Función recuperar (get) la estructura de datos del Robot [confina] = gagent(a,'Confina'); % fichero simulink de control

Anexo A. Programa

291

% Función recuperar (get) la estructura de datos del Robot [mast] = gagent(a,'mast'); % fichero control máquina de estados

% Selección tipo de fichero de control Simulink if get(ra_fi_con1,'value') set(confina,'Xdata',1); % Todo el sistema por fichero Simulink else set(confina,'Xdata',0); % Solo los motores por fichero Simulink end;

% Añadir la extensión al nombre del fichero simulink de control, para % comprobar su existencia name_file = [get(nafi_con,'string') '.' 'm'];

if exist(name_file) % Comprobar la existencia del fichero set(confina,'UserData',get(nafi_con,'string'));

% Función guardar (set) en la estructura de datos del Robot el% fichero de control

sagent(a,'confina',confina);

else % Si no existe el fichero, mensaje de aviso set(text2_error,'String','Warning: Simulink Control File notexist','visible', 'on'); % Mensaje de error, no existe fichero de

% Simulink error = 1; return; % No cerrar pantalla de edición-diseño del Robot del Robot end % end if exist(name_file)

% Añadir la extensión al nombre del fichero máquina de estados, para % comprobar su existencia name_file = [get(maes_fi,'string') '.' 'm'];

if exist(name_file) % Comprobar la existencia del fichero set(mast,'UserData',get(maes_fi,'string'));

% Cerrar pantalla de edición-diseño del Robot del Robot close(DISPLAY2);

% Función guardar (set) en la estructura de datos del Robot el% fichero de control

sagent(a,'mast',mast);

else % Si no existe el fichero, mensaje de aviso set(text2_error,'String','Warning: Programation File sensors notexist', 'visible','on'); % Mensaje de error, no existe fichero de

% Simulink error = 1; return; % No cerrar pantalla de edición-diseño del Robot del Robot end % end if exist(name_file)

% Liberar memoria, clear variables y funciones de la pantalla de % edición-diseño del Robot clear edirobot asensor chcolsen chsize dsensor editsen file quit1

clear DISPLAY2 WORLD_AXIS2 AGENT_AMPLI nfc sen_Text num_sen add_sendel_sen edit_sen...

dis_Text dis_sen ang_Text roText ch_si_ro size_ro size_Textcon_Text edit_mot quit nafi_text1 ra_fi_con1 save load x0_ampli y0_amplinum_ro text2_error maes_fi text_num_sen

Anexo A. Programa

292

elseif op==2

% Salir de la pantalla de edición de Obstáculos.

global WORLD_AXIS DISPLAY3 text2_error

close(DISPLAY3); % Cerrar pantalla de edición de obstáculos

% Liberar memoria, clear variables de la pantalla de edición-diseño de % obstáculos clear editobst obsta sobsta sobstak dobsta quit1

clear global DISPLAY3 WORLD_AXIS2 OBSTACLES2 save2 load2 obsNum obsDelobsSet obsAdd obsDel xob_coor yob_coor ang_obs_ro rad_obs_ro shapesha_obs_Text options2 save2 load2 quit2 shapecua shapecir avisotext_num_sen

% Valores de referencia ejes de la pantalla de simulación axes(WORLD_AXIS);

elseif op >= 3

% Salir de la pantalla de configuración de las variables de lasimulación

global DISPLAY4 MAX_AGENTS MAX_A MAX_SIZE MIN_SIZE MAX_SI MIN_SIMAX_SENSORS MAX_SE dt_text MAX_ANG_SEN MIN_ANG_SEN MAX_DIS_SENMIN_DIS_SEN MAX_MIN_ROT MA_DS MI_DS MA_ANG diana MI_ANG MA_MI_ROTMAX_OBSTACLES MAX_SIZE_OBSTACLES MAX_O MAX_SI_O dt D_T DIN VIT vitalveloRun

% Salvar datos de configuración en el fichero de "varcfg.m" (variables % de configuración).

fid = fopen('varcfg.m','w'); % Abrir fichero para escribir

% ------------------------------------ % Inicio salvar datos de configuración % ------------------------------------

fprintf(fid,'diana\n'); % Save dato diana

if get(DIN,'value') fprintf(fid,'on\n','on'); diana = 'on'; else fprintf(fid,'off\n','off'); diana = 'off'; end % end if get(DIN,'value')

fprintf(fid,'vital\n'); % Save dato vital

if get(VIT,'value') fprintf(fid,'on\n','on'); vital = 'on'; else fprintf(fid,'off\n'); vital = 'off'; end % end if get(DIN,'value')

Anexo A. Programa

293

fprintf(fid,'MAX_AGENTS\n'); % Save dato MAX_AGENTS if abs(str2num(get(MAX_A,'string'))) > 7 fprintf(fid,'%g\n',7); else fprintf(fid,'%g\n',abs(str2num(get(MAX_A,'string')))); end;

fprintf(fid,'MAX_SIZE\n'); % Save dato MAX_SIZE fprintf(fid,'%g\n',abs(str2num(get(MAX_SI,'string'))));

fprintf(fid,'MIN_SIZE\n'); % Save dato MIN_SIZE fprintf(fid,'%g\n',abs(str2num(get(MIN_SI,'string'))));

fprintf(fid,'dt\n'); % Save dato dt

dt = abs(str2num(get(D_T,'string'))/1000);

if dt < 1e-9 % Si "dt" próximo a 0 dt = 1e-9; % Valor mínimo será de 1e-6 s end;

fprintf(fid,'%g\n',dt);

MAX_SEN1 = abs(str2num(get(MAX_SE,'string')));

if MAX_SEN1 == 0 MAX_SEN1 = 1; end;

fprintf(fid,'MAX_SENSORS\n'); % Save dato MAX_SENSORS fprintf(fid,'%g\n',MAX_SEN1);

fprintf(fid,'MAX_DIS_SEN\n'); % Save dato MAX_DIS_SEN fprintf(fid,'%g\n',abs(str2num(get(MA_DS,'string'))));

fprintf(fid,'MIN_DIS_SEN\n'); % Save dato MIN_DIS_SEN fprintf(fid,'%g\n',abs(str2num(get(MI_DS,'string'))));

fprintf(fid,'MAX_ANG_SEN\n'); % Save dato MAX_ANG_SEN fprintf(fid,'%g\n',abs(str2num(get(MA_ANG,'string'))));

fprintf(fid,'MIN_ANG_SEN\n'); % Save dato MIN_ANG_SEN fprintf(fid,'%g\n',abs(str2num(get(MI_ANG,'string'))));

fprintf(fid,'MAX_MIN_ROT\n'); % Save dato MAX_MIN_ROT fprintf(fid,'%g\n',abs(str2num(get(MA_MI_ROT,'string'))));

MAX_OBST = abs(str2num(get(MAX_O,'string')));

fprintf(fid,'MAXOBSTA\n'); % Save dato MAXOBSTA fprintf(fid,'%g\n',MAX_OBST);

fprintf(fid,'MAXSIOBST\n'); % Save dato MAXSIOBST fprintf(fid,'%g\n',abs(str2num(get(MAX_SI_O,'string'))));

global veloSimula

Anexo A. Programa

294

% Comprobar velocidad de simulación continuada deseada veloSimula = get(veloRun,'value');

% Save dato velocidad de simulación continuada deseada fprintf(fid,'veloSimula\n');

fprintf(fid,'%g\n',veloSimula);

delete(dt_text);

% Cerrar pantalla de configuración variables de la simulación close(DISPLAY4);

axes(WORLD_AXIS);

% Declaración de variables

global OBSTACLES POSITIONS

% Al producirse un cambio en la configuración, es necesario % restructurar los datos ya existentes tanto de los obstáculos como % de los Robots for i = (MAX_OBSTACLES+1):MAX_OBST % Crear patch gráfico para Obstáculo

a =patch('XData',0,'YData',0,'EraseMode','xor','FaceColor','w','EdgeColor','w');

% Guardar los diferentes patch de datos del Obsta. en el vector% state que corresponde a "UserData" del patch del Obsta.

% selfobs = [coordenada"x" coordenada"y" theta radius shape]

selfobs = [0 0 0 0 0];

state = [0 selfobs];

set(a,'UserData',state); OBSTACLES(i) = a;

end; % end for i=(MAX_OBSTACLES+1):MAX_OBST

MAX_SEN2 = MAX_SENSORS;

for i = 1:length(AGENTS)

if AGENTS(i)~= 0 if (MAX_SEN1 ~= MAX_SEN2)

% Datos de estado actual del Robot "i" % self = [xa ya theta radius velolineal veloangular pertur] self = gagent(i,'Self');

% dades_sen(j,:) =[distan detec, ang_detec, ang rotate sensor, % position dis, position ang.] dades_sen = gagent(i,'dades_sen');

Anexo A. Programa

295

% Actualizar tamaño de la matriz de datos de los sensores

for j = 1:MAX_SEN1 da_sen(j,:) = [0 0 0 0 0]; end;

for j = 1:MAX_SEN2 if j <= MAX_SEN1 da_sen(j,:) = dades_sen(j,:); end; end;

% graf_sen(j) = [coordenadas de dibujo sensor "i"] for j = 1:MAX_SEN1 gr_sen(j) = line('EraseMode','xor','Visible','on','XData',

0,'YData', 0,'color','g','UserData',da_sen(j,:)); end;

% Recuperar estructura antes de borrar

[state i self2 goal2 confina2 mast2 redon2 graf_sen2dades_sen2] = strucrob(i);

filename1 = get(confina2,'UserData'); typefile = get(confina2,'Xdata'); filename2 = get(mast2,'UserData');

dagent(i); % Borrar Robot para actualizar su estructura a la% nueva configuración sensores

MAX_SENSORS = MAX_SEN1;

agent(i,self,filename1,typefile,filename2,gr_sen,da_sen);

end; % end if (MAX_SEN1 ~= MAX_SEN2)

redon = gagent(i,'redon');

% Actualizar espacio vital visible, o no? set(redon,'visible',vital); sagent(i,'redon',redon);

% state = get(POSITIONS(i),'UserData'); % Actualizar diana visible, o no? set(POSITIONS(i),'visible',diana);

MAX_SENSORS = MAX_SEN2; % Recuperar valor de MAX_SENSORS

end; % end if AGENTS(i)~= 0 end; % end for i = 1:length(AGENTS) % Función declaración variables globales generales (actualizar) invargen(0);

Anexo A. Programa

296

% Liberar memoria, clear variables de la pantalla de configuración clear DISPLAY4 MAX_A MAX_SI MIN_SI MAX_SE MA_DS MI_DS MA_ANG MI_ANGMAX_SEN1 MAX_OBST MA_MI_ROT MAX_O MAX_SI_O D_T DIN VIT index1 index2MAX_SEN2 dt_text veloRun

global dt_text % actualizar texto Grain Time de la pantalla

dt_text = uicontrol('Visible','on','Position', [.03 .83 .245.03],'HorizontalAlignment', 'Left', 'Style', 'text', 'Units','normalized', 'String', ['Total Grain Time (GT = ' num2str(dt*1000,2) 'ms) = '] ,'BackgroundColor','y','Callback','');

end; % end if op==0

clear quit1clear self MAX_SEN1 MAX_SEN2


Anexo BPresentación de las Pantallas del Simulador

Anexo B. Presentación de las Pantallas del Simulador

298


299


300


301


Anexo CAnexo de Información Técnica

Technology – short and to the point

maxon DC motor

These are some of the technical advantages of maxon DC motors :l no magnetic detentl low rotor inertia results in high

acceleration characteristicsl low electromagnetic interferencel low electrical noisel small inductancel high efficiencyl linear relationship voltage/speedl linear relationship load/speedl linear relationship load/currentl small torque ripple due to multi-

segment commutatorl able to withstand overloadsl very long lifel compact design = small dimensionsl extensive possibilities to combine

motors with gearheads, DC-tachosand encoders

maxon EC motor

The special advantages ofmaxon EC motor .l no mechanical commutationl high life expectancy – limited by

bearing life onlyl no magnetic detentl high speeds even at low voltagesl The maxon winding technology

allows the winding to be optimizedfor specific applications

l good heat dissipation, high over-load capability

l largely linear motor curves, featuring excellent regulation

l high efficiencyl smallest electrical time constant

and low inductancel minimal electrical noise.

S-Motor

Worldwide patents demonstrate thehigh level of technology.

The maxon winding:

A number of different windings areavailable for each motor type, as canbe seen from the motor data pages.This is achieved by selecting a differ-ent wire gauge for a specific numberof turns. Various terminal resistancesare the result.

Specific motor parameters, speedand current are influenced by thesevariations. In turn, the user canselect the optimum motor for hisspecific application.

High terminal resistance = thin wire = low starting current(high resistance winding) many turns low specific speed (rpm/Volt)Low terminal resistance = thick wire = high starting current(low resistance winding) few turns high specific speed (rpm/Volt)

maxon standard windings are made by processing wires ranging from 0.032 to 0.45 mm in diameter.

maxon motor

22

Barquero Barquero


Commutation:

Life expectancy:

maxon DC motor

Precious metal brushes and com-mutator: Our precious metal combi-nation guarantees very consistentand low contact restistance even af-ter prolonged shutdown. Low startingvoltage, low electrical interference,and high efficiency. Ideal for battery-powered applications.CLL-Concept: Arcing is the maincause for commutator and brushwear. CLL effectively suppressesarcing by a substantial degree andmotor life is greatly extended.Graphite brushes: Used in conjunc-tion with copper commutators for themost rigorous start/stop servo appli-cations. Tens of millions of cycleshave been attained repeatedly.

Owing to the large number of factorsinvolved, no general statement as tolife can be made. The low inductancemoving coil rotor found in all maxonmotors ensures maximum brush andcommutator life. Motor life of greaterthan 20’000 hrs is achievable underfavourable conditions, but less than100 hrs of life expectancy could bestated under extremly unfavourableconditions. On average, life expec-tancy is achievable between approx.1’000 to 3’000 hrs.

The following factors affect motorlife:1. Electrical load: The higher thecurrent, the greater the electricalwear. In some cases it may even beadvisable to select a slightly strongermotor. Our specialists will gladlyadvise you.

2. Speed: The higher the speed, thegreater mechanical and electricalwear will be.

3. Operating mode: Extremestart/stop and CW/CCW operationshave a life shortening effect.

4. Environmental influences: Temperature, humidity, vibration,type of installation, etc..

5. The CLL-Concept prolongs life,specifically at high loads, with theadvantages of the precious metalbrushes being fully retained.

6. Graphite brushes in conjunctionwith ball bearings also assure longlife even in extremely rigorous condi-tions.

maxon EC motor

Commutation is accomplishedelectronically.Mechanical parts such as brushesand commutator are replaced with electronic components.In contrast to maxon DC motors:The winding remains stationary whilethe magnet rotates.

Life is virtually only limited by normalball bearing wear. For this reasonand due to our use of industrialgrade electronic components exclu-sively, the EC motor achieves a lifeof several 10’000 hours.

maxon motor

23

Barquero Barquero


Temperature:

At constant operating voltage, speed decrease is proportional to load in-crease. Matching motors to meet desired operating characteristics is possible with thewide range of different windings available. Realizing low output speeds with a gearmotor is often the preferred solutionover a slow running motor.

This is a limiting factor for the operating range of every motor. Heat isgenerated in the motor winding which needs to be carried away via thesurface. The max. rotor temperature shown on the data sheets may not beexceeded, even short term. The motor’s ability to perform is directly relatedto the difference between ambient temperature and maximum permissiblerotor temperature as well as the duty cycle (see also page 36 «Short TermOperation»). Winding resistance rises and magnetic forces decrease astemperature increases, resulting in reduced output performance. These fac-tors must be given appropriate consideration when operating with high con-tinuous loads.Measures such as good air circulation or heat sinking can significantly lowertemperatures.Normal bearing lubricants permit a minimal operating temperature of -20°C.

Comments:

no No load speed (rpm)

MH cold Stall torque at rotortemperature 25°C (mNm)

MH hot Stall torque at rotortemperature >25°C (mNm)

l0 No load current (mA)

IA cold Starting current at rotortemperature 25°C (mA)

IA hot Starting current at rotor temperature >25°C (mA)

Pcold Power output at rotortemperature 25°C (mW)

Phot Power output at rotor temperature >25°C (mW)

Change in operating characteristicswith rising motor temperature atconstant terminal voltage.

maxon DC motor

Maximum rotor temperature for ourhigh-temperature versions is 125°C,and 85°C for all others.

Optimum operating speeds are in therange of 4000 rpm to 9000 rpm, de-pending on motor size. Some special versions permitspeeds of more than 50’000 rpm.

maxon EC motor

The maximum winding temperatureis 125°C.

Speeds higher than 50’000 rpm arecertainly possible.

maxon EC motor

This motor can only develop its trueadvantages in conjunction with ballbearings.

maxon DC motor

Sintered bearings: High quality, lownoise, cost-effective bearings withpermanent lubrication for standard re-quirements.Ball bearings: For extreme loadsand maximum life.

Rotor temperature:

Speed:

Bearings:

maxon motor

24

Barquero Barquero


Constant Voltage:

Variable Voltage/Control:

Control:

maxon motor control

If the maxon DC motor is connectedto a fixed voltage, then shaft speedwill follow the speed/torque gradientwith every load change. Thespeed/torque gradient is a motor-specific constant (see also pages30/31 and 36 to 99, line 5).

If, in an application, excessive speedvariation is experienced during loadchanges, then either a strongermotor or a variable operating voltage(U1...2...3) is required. Speed remainsstable (n/const), speed/torquegradient is displaced in parallelfashion.

Displacement of the speed/torquegradient can be automated electroni-cally. The electronic controller mmcadjusts the motor input voltage rela-tive to the motor load. As a result, the overall system, consisting of motor and controller, appears tohave a changed, flat, speed/torquegradient.

To perform as intended, the control-ler needs two pieces of informationfor comparison: The desired (set)speed info and the actual speed info.Information comparison renders aneither positive or negative deviationfrom desired speed. The controllerthen changes the voltage at the mo-tor terminals until deviation is zero.

The maxon motor control programoffers three methods to gather actualspeed information:

DC Tacho:A tacho provides a speed-propor-tional DC voltage.

Digital Encoder:This unit supplies a stream of pulsesat a frequency that is speed propor-tional. An electronic circuit is used toconvert the frequency into a DC volt-age. Information gathering is virtuallywear-free, an advantage. Upper andlower frequency limits of the fre-quency-to-voltage converter must,however, be taken into consideration.

IxR Compensation: Motor current rises in linear fashionwith rising motor load. The control circuit changes the voltage at the mo-tor terminals in relation to change in motor current.

maxon’s range of motor control pro-ducts includes controllers for DC mo-tors which allow the detection of theactual value with use of a DC tacho,incremental encoder and IxR com-pensation as well as making com-plete 4-quadrant running possible. The Linear Servo Control (LSC) is ideal for use with motors of up to 50Watt and a max. current of 2 Ampère.The PWM servo controllers coverthe range of maxon DC motors up toapprox. 250 Watt.

For the electronic commutation andcontrol of the brushless maxon ECmotors a range of 1- to 4-quadrantcontrollers for motor currents of up to14 A are available.The Hall sensors built in to the motorare used for speed detection. Formore demanding regulation themotor is fitted with an encoder or aresolver which makes an appropri-ately higher definition possible.

maxon motor

26

Barquero Barquero

maxon DC motor – Important considerations

A diagram can be drawn for anymaxon DC motor, providing userswith the most important data.

Shown as an example is motor2326.939 operating at 24 Volt and25°C rotor temperature. This diagramalso shows how the motor would oper-ate at a given load of MB.(Refer to motor data sheet, page 60).

U = 24 volt Nominal voltageno = 8660 rpm No load speedMH = 44.7 mNm Stall torqueIo = 27.2 mA no Load currentIA = 1720 mA Starting currentP2max = 9950 mW Max. power outputηmax = 76.5% Max. efficiencyMB = Operating torquenB = Operating speed at load MBIB = Operating current at load MBP2B = Power output at load MBηB = Efficiency at operating point

Whereas speed, torque, and currentgradients are determined by their endpoints, plotting of output and efficiencycurves is somewhat more laborious.The output power is plotted using thefollowing rule: The peak value occurs

at MH

2.

General rule:

P = π

30 ⋅ M ⋅ n

Due to the proportional relationships,power is easily determined.

Our example:

For 0.9 MH and 0.1 no

P = π

30 ⋅ MH ⋅ no (0.9 x 0.1) = 3.6 W

For 0.8 and 0.2 MH

P = π

30 ⋅ MH ⋅ no (0.8 x 0.2) = 6.5 W

For 0.7 and 0.3 MH

P = π

30 ⋅ MH ⋅ no (0.7 x 0.3) = 8.5 W

For 0.6 and 0.4 MH

P = π

30 ⋅ MH ⋅ no (0.6 x 0.4) = 9.7 W

For 0.5 MH

P = π

30 ⋅ MH ⋅ no (0.5 x 0.5) = 10.1 W

And the efficiency:

η = π

30 ⋅

M ⋅ nU ⋅ I

ηmax = 1 − √Io

IA

2

Although no consideration is given totolerances and other factors, suffi-ciently accurate values are obtainedfor the purpose of a preliminary review.

At U = 24 V, the diagram above showsmotor gradients for speed n, current I,output P, and efficiency η as a func-tion of torque M. The speed/torquegradient ∆n / ∆M (rpm/mNm) is shownto be independent of the operatingvoltage.This gradient therefore provides a use-ful figure of merit about a motor.

Different motors can be directly com-pared. The smaller this number, themore powerful the motor.

It is not possible to operate a motorsimultaneously at maximum powerand maximum efficiency as is clearlydemonstrated by theses gradients.

These motors develop their highesttorque during start-up. It is a multipleof the unit’s normal operating torque.As a consequence, current consump-tion is highest as well.

In our example, values are entered foran operating torque of MB = 10 mNmand a power output of PB = 7.0 W.A speed of nB = 6700 rpm and a cur-rent IB = 411 mA are the result. Effi-ciency ηB is still 71.1%.

The diagrams below show that achange in operating voltage producesa parallel displacement of thespeed/torque gradient and a corre-sponding change in power output.

The diagram shows the most importantmotor and operating characteristics.

maxon motor

30

Barquero Barquero

Selection criteriaThere are cases where a predeter-mined operating point can beachieved with different motors havingdifferent speed/torque gradients.

Example: Achieving a specific opera-ting point with motors of varyingpower capacity.

Motor 2 is the less powerful and lowerpriced motor, and quite satisfactory ina particular application. Motor 1 is astronger unit with power in reserveand, perhaps important, with longerlife expectancy. In addition, power con-sumption is lower. The exact identicaloperating point is not always achiev-able as shown in the diagram. Selec-tion criteria is therefore often dictatedby the specific application.Our sales engineers can also be of as-sistance in this respect.

Pragmatic beginning for a motor determination:If you have a well equipped electroniclaboratory at your disposal, you mayskip the following text.

Our experience has shown that it isoften quite difficult to precisely deter-mine the optimum motor for a specificapplication by calculation. A small trickcan help you to establish a first ap-proximation:Operate your apparatus with a maxonDC motor of estimated size and out-put power. Then vary the operatingvoltage until the appropriate point isreached. Now measure voltage andcurrent. By conveying these valuesand the exact model number of the«measurement-motor», our engineerswill be in a position to inform you ofthe most suitable motor for your appli-cation.

Additional criteria of influence in find-ing the most suitable motor:

– Mass to be accelerated (weight andinertia of that mass)

– Mode of operation (continuous, in-termittent, forward/reverse)

– Ambient conditions (temperature,humidity, media)

– Voltage supply (battery, power supply)

Comparison between two differentmotors at identical torque load

From this we derive that, for the samemotor, speed and power output arenearly proportional to input voltage.This is one of the reasons why it issimply not possible to indicate a fixedoutput power capacity. Our motorscan be operated at voltages otherthan those indicated in the datasheets. Consideration should there-fore be given to max. permissiblespeed and rotor temperature rise. Increased motor load results in in-creased coil temperature and coil re-sistance, leading to a reduction in stalltorque. The speed/torque gradient de-velops an apparent curvature. (See

also «Technology – short and to thepoint» under «Temperature»).

For voltage changes, the followingequations give good approximations:

n2 = n1 U2

U1 speed

P2 = P1 U2

U1 power output at con-

stant torque load

MH2 = MH1 ⋅ U2

U1stall torque

IA2 = IA1 ⋅ U2

U1starting current

Changing a motor’s winding instead ofits operating voltage also results in dif-ferent motor behaviour. Although thespeed/torque gradient remains con-stant. Up to 19 different windings areavailable per motor type.

maxon motor

31

Barquero Barquero

TolerancesTolerances are no longer neglectablewhen it comes to «fine tuning». Mech-anical tolerances can be extractedfrom outline drawings. General motordata sheets include only nominalvalues. The diagram on the rightshows how tolerances affect motorperformance. These tolerances aremainly caused by differences in mag-netic field strength and wire resist-ance, and to a lesser degree by mech-anical factors.

Tolerances indicated in this diagramare simplified and exaggerated for thepurpose of illustration. It is well demon-strated however that deviation fromnominal values is less in the operatingrange than in starting and no load con-ditions. All these details can be foundin our computer sheets. Please re-quest them if necessary.

Commutation displayPlease observe the current trace of amaxon DC motor with precious metalbrushes in an oscilloscope, as fol-lows: Connect the scope leads acrossa resistor having a value approx.1/50th of the motor’s DC resistanceand which is in series to the motor. Ondisplay is the voltage drop across theresistor. In contrast to what is experi-enced with conventional motors, youwill note an even trace, free of electri-cal noise. The combination ofprecious metal commutation andmaxon rotor system provides for veryminimal high frequency noise whichcould, if it were severe, cause prob-lems in electronic circuits. In practice,our motors need no noise suppressionmeasures.

Spikes can be seen when observinggraphite brush commutation. This isone of the distinct characteristics ofthese motors. In spite of the electricalnoise generated by these spikes,graphite brush motors are success-fully used in conjunction with elec-tronic motor controls.Please note that the brush contact re-sistance varies depending on motorcurrent. Finally, a word about price

The F-Program (with ferrite magnets),the maxon A-max motors and the A-Program feature especially favor-able pricing – the S-Program featuresadditional performance and great ver-satility.

Special versions are generally moreexpensive. For the purpose of pin-pointing the most suitable motor fromour vast range for your application, it would be to your advantage to do this in consultation with our sales engineers.

Scope display,graphite brushes

Scope display,precious metal brushes

Tolerance field presentationfor maxon motors

Tolerance for starting current

➀Ripple, effective wavyness, peak topeak

➁Modulation, basically caused byasymmetry in windings and mag-netic field

➂Shape of the signal for one com-plete turn (number of peaks = num-ber of segments times two)

maxon motor

32

Barquero Barquero

maxon DC motor – Comments regarding motor data (pages 37-99)Line 1 Assigned power rating P2T [W]

This figure represents the maximumpower output when operating within therecommended output power range. It isdependent on motor type and can becorrelated with the presentation in ourSelection Guide (Please refer also topages 37-99 under «Operating Ranges»).

Line 2 Nominal voltage U [V]

All nominal values refer to operation atthis voltage. It was chosen such as not toexceed the maximum recommendedspeed in no load condition. Motor appli-cation is of course not limited to this volt-age. To reach the assigned power rating(Line 1) higher operating voltages arepermissible. Maximum power output(Line 12) will increase respectively, (seealso page 30 «Important considerations»- bottom of right hand side).

Line 3 No load speed no [rpm]

This is the speed at which the motor turnsat nominal voltage and without load. Forpractical purposes, this speed is propor-tional to the voltage applied.

Line 4 Stall torque MH [mNm]

This is the torque produced by the motorin a standstill condition, also called start-ing torque. The rapidly rising rotor tem-perature leads to a corresponding de-crease in stall torque (see also «Technol-ogy – short and to the point»).

Line 5 Speed/torque gradient∆n/∆M [rpm/mNm]

This gradient says a lot about the powercapability of a motor. The flatter the gra-dient, the less speed variation is experi-enced during load variations.The speed/torque gradient is calculatedat 25°C winding temperature.

Line 6 No load current Io [mA]

This is the current the unloaded motordraws. It depends on brush and bearingfriction and varies slighty with varyingspeed.

Line 7 Starting current IA [mA], [A]

is the quotient «Voltage at terminals/DC-resistance» and is proportional to stalltorque.

Line 8 Terminal resistance R [Ohm]

is the resistance at the terminals at 25°Cand determines the starting current at agiven voltage.In the case of graphite brushes it must benoted that contact resistance varies de-pending on the load.

Line 9 Maximum permissible speed nmax [rpm]

This speed should not be exceeded dur-ing normal operation. Commutation prob-lems may be expected at higher speeds,in turn, this could lead to premature motorfailure.

Line 10 Maximum continuous current Ipermiss. [mA],[A]

Operating the motor continuously at thiscurrent level and at 25°C ambient willcause the winding to ultimately reach thespecified max. winding temperature. Thisassumes no heat sinking. Dependinghow the motor is mounted, this value canbe increased substantially.

Line 11 Max. continuous torque Mperm [mNm]

is the torque that can be supplied continu-ously or on an average, thereby heatingup the winding to the maximum per-missible temperature; based on an am-bient temperature of 25°C.

Line 12 Maximum power outputPmax [mW], [W]

is the theoretical maximum output at 25°Crotor temperature. Permissible limits arefrequently below this level (see max. con-tinuous current and max. permissiblespeed).

Line 13 Maximum efficiency ηmax [%]

Efficiency is derived from the relationshipbetween no load and starting current.

The higher the efficiency, the nearer to noload speed the operating point will be.Maximum efficiency is usually at approxi-mately 1/7th of stall torque.Operating at this point is not necessarilysynonymous to operating at the motor’soptimum load point.

Line 14 Torque constantkM [mNm/A]

This may also be referred to as «specifictorque» and represents the quotient fromgenerated torque and applicable current.

Line 15 Speed constant kn [rpm/V]

shows the specific speed per volt of ap-plied voltage, disregarding any frictionallosses. The reciprocal of the speed con-stant is known as the voltage constant orback-EMF constant.

Line 16 Mechanical time constantτm [ms]

is the time required by the rotor to accel-erate from standstill to 63% of its no loadspeed. In 4τ the rotor will have reachedmore than 99% of no load speed.

Line 17 Moment of inertia JR [gcm2]

is the polar mass moment of inertia of therotor.

Line 18 Terminal inductance L [mH]

is the winding inductance when stationaryand measured at 1 kHz, sinusoidal.

Line 19 Thermal resistance Rth2 [K/W]

from housing to ambient air. Charac-teristic value of thermal contact resist-ance without additional heat sinking. Thesum of lines 19 and 20 define the max.admissible power loss. Heat sinking maysubstantially reduce this value.

Line 20 Thermal resistance Rth1 [K/W]

from rotor to housing.

Operating Ranges Comments (Example from page 58) Short Term Operation

Recommended operating range

Continuous operationIn observation of above listed thermal resistances(lines 19 and 20) the maximum permissible rotortemperature will be reached during continuousoperation at 25°C ambient.= Thermal limit

Short term operationThe motor may be briefly overloaded (recurring).

Speed (n), torque (M), current (l):The outer edges of the values depicted representlimits for continuous and short term motor oper-ation. Values listed in the tables (lines 3, 4, 6, 7,12 and 13) are valid for operation at nominalvoltage (line 2). These are therefore values whichare only reached when operating the motor athigher voltages.

Assigned Power Rating P2T (W) (Line 1) Starting current IA at nominal voltage

(Line 7) as well as related stall torque

MH (mNm) (Line 4) IA = UR

⋅ 103 (mA)

980 Winding Number with related currentcurve. Low resistance winding(Ref. line 8)

982 Winding Number with related currentcurve. High resistance winding(Ref. line 8)

ON Motor in operationOFF Motor inoperativeÎ max. peak currentIpermiss. max. permissible continuous current

Line 10tON ON timeT cycle time tON + tOFFDCy Duty Cycle in percent of the Cycle

Time T. The motor may be overloaded by the relationship Î/Ipermiss. during X% of the total Cycle Time.

maxon motor

36 maxon DC motor April 1999 edition / subject to change

Barquero Barquero

Barquero Barquero

Barquero Barquero

Barquero Barquero

Motor Calculations, Equations and their Applications

Definitions1. Nominal Voltage UN [ V ]

is that voltage from which the nominal data is taken.

2. Nominal Speed nN [ rpm ]

is the speed which is determined by the nominal voltage and nominal torque.

3. Nominal Torque MN [ mNm ]

is the torque of the shaft at nominal voltage and nominal speed.

4. Nominal Current IN [ mA ]

is the current consumed at nominal torque and after nominal speed isreached.

5. Nominal Electrical Input P 1N [ mW ]

is the product of the nominal voltage and nominal current.

6. Nominal Power P2N [ mW ]

is the mechanical power which is developed at the motor shaft. It is calculatedfrom the nominal torque and nominal speed.

7. Nominal Efficiency ηN [ % ]

is the quotient of the nominal power output and nominal power input.

8. Stall Torque MH [ mNm ]

(also known as stopping torque, short-circuit torque and starting torque)is the torque which, at nominal voltage, causes the motor shaft to stop.(dependent on temperature).

9. No Load Current lO [ mA ]

is the current which at idle running results from bearing and brush frictionand specific torque at a motor temperature of 25°C.

10. No Load Speed nO [ rpm ]

is the speed which is achieved by the unloaded motor with nominal voltageapplied at 25°C.

11. Friction Torque MR [ mNm ]

is the torque requirement of the motor when running idle due to the bearingsand brush friction, measured after a running in period.

12. Specific Current iS

AmNm

is the current change a a constant voltage, which is brought about by adefined torque change. It is one of the motor characteristics.

13. Specific Torque mS

mNmA

is the factor at which the generated motor torque changes with electricalinput. It is one of the motor characteristics.

14. Specific Speed nS [ rpm / V ]

is the speed change per volt, voltage change at constant load. It is one ofthe motor characteristics.

15. Specific Generator Voltage eS

V1000 rpm

is the change in open circuit voltage output at the motor terminals achievedby changing the motor shaft speed by 1000 rpm. It is one of the generatorcharacteristics.

16. Inner Voltage ei [ V ]

is the generated EMF of the motor or generator.

17. Gradient of the Speed/Torque Curve∆n∆M

[ rpm / mNm ]

the gradient of the speed-torque curve gives information on the powercapacity of the motor. It shows the change of speed at particular torquevariations. The smaller the value of this quotient the flatter the curve showingthat the motor is more operationally efficient.

18. Terminal Resistance R [ Ω ]

is the resistance across the motor terminals taken at 25°C winding tempera-ture with a stationary shaft but run-in motor. (incl. brushes and brush contactresistance).

19. Terminal Inductance L [ mH ]

is the inductance at the motor terminals when stationary. The test voltageand test frequency should be given.

20. Electrical Time Constant τe [ ms ]

is the time in which the current increases to 63 % of the final value when theshaft is blocked.

τe = LR

21. Electromechanical Time Constant τm [ ms ]

is the time needed by the rotor, with unloaded shaft, to reach 63 % of the noload speed from standstill at nominal voltage.

22. Full Speed Time th [ ms ]

is the time the rotor takes, with unloaded shaft, to reach virtual no load speedfrom standstill at nominal voltage.(in practice t h ≈ 3 τ m ) .

23. Rotor Moment of Inertia JR [ gcm2 ]

is the mass moment of inertia of the rotor.

24. Maximum Acceleration amax [ rad s-2 ]

is the maximum change of the angular speed, with an unloaded shaft andwithout additional mass moment of inertia, when nominal voltage is applied.It is the quotient of the stall torque MH and the rotor mass moment of inertiaJR . amax =

MH

JR

25. Speed Constant kn

rpmV

is the quotient of the no load speed no and the induced voltage of the motorUi. It is one of the motor characte-ristic constants. kn =

no

Ui

26. Torque Constant kM

mNmA

is the factor at which the generated motor torque changes with electricalinput. It is one of the motor characteristics.

kM = Mi

I

27. Current Constant kl

AmNm

Reciprocal of the torque constant. It is one of the motorcharacteristic constants. kI =

1kM

τττ

maxon motor

208

Barquero Barquero

Barquero Barquero

Barquero Barquero

Barquero Barquero

Barquero Barquero

28. Generator Voltage Constant kE

V

rad s −1

is the quotient of the induced voltage Ui and the angular speed at generatedoperation. It is one of the motorcharacteristic constants. kE =

Ui

ωIn practice kE is given as the speed change of 1000 rpm.

Calculation factor V

1000 rpm = 104.7 rad s-1.

29. Starting Voltage UA [ V ]

is the voltage whereby the stationary rotor starts to move. It is dependent ontemperature and position.

30. Maximum Output P2max [ mW ]

is the mechanical power achieved at the motor shaft with operation atnominal voltage. Limiting conditions should be noted. Operation at maximumvalues normally reduces the life-span.

31. Maximum Speed nmax [ rpm ]

is the maximum permissible speed. (speeds in excess of this value reducelife-span).

32. Maximum Housing Temperature θG [ °C ]

is the maximum permissible continuous temperature on the motor housingsurface.

33. Permissible Power loss PV permiss [ mW ]

is the power which can be dissipated within the motor without overheating.

34. Thermal Resistance Rth1 und Rth2

KW

is the characteristic value of the thermal transition resistance.Rth1 winding to housingRth2 housing to ambientValues are measured when the motor is mounted horizontally on a plasticplate in the open air. By mounting on a heat sink Rth2 can be improved.

35. Maximum Efficiency ηmax [ % ]

is the maximum efficiency which can be reached at nominal voltage. This isnot, in every case, the optimum operation point. The maximum efficiencyoccurs at approx. 1⁄7 of the stall torque.

36. Optimum Torque Mopt. [ mNm ]

is the torque at the maximum efficiency.

37. Optimum Speed nopt. [ rpm ]

is the speed at the maximum efficiency.

38. Starting Current IA [ mA ]

is the current which by calculation is given as the quotient of nominal voltageUN and terminal resistance R.

IA = UN

R39. Permissible Current lpermiss [ mA ]

is the current at which the maximum permissible rotor temperature isreached, taking into consideration the given thermal resistance Rth1 , Rth2

and an ambient temperature of 25°C. On motors with lower resistance rotorsIpermiss is limited further by maximum allowable current density at thebrushes.

40. Permissible Torque Mpermiss [ mNm ]

is the output torque achieved at Ipermiss.

Physical Values and their Unitsamax Max Angular Acceleration . . . . . . . . . . . . rad s-2

I Armature Current . . . . . . . . . . . . . . . . . A

IA Starting Current at δR = 25°C . . . . . . . . . . . . A

IAδ Starting Current at δR . . . . . . . . . . . . . . . A

lB Load Current . . . . . . . . . . . . . . . . . . . A

lo No Load Current . . . . . . . . . . . . . . . . . A

J Moment of Inertia . . . . . . . . . . . . . . . . kg m2

JL Load Inertia . . . . . . . . . . . . . . . . . . kg m2

JR Rotor Inertia . . . . . . . . . . . . . . . . . . kg m2

∆n/∆M Speed torque gradient . . . . . . . . . . . . . rpm/Nm

k Motor Constant . . . . . . . . . . . . . . . Nm W − 1⁄2

kM Torque Constant . . . . . . . . . . . . . . . . Nm/A

M Torque Output . . . . . . . . . . . . . . . . . . Nm

Mb Moment of Acceleration . . . . . . . . . . . . . . Nm

MB Load Torque . . . . . . . . . . . . . . . . . . . Nm

Mi Generated Torque . . . . . . . . . . . . . . . . . Nm

MiH Generated Stall Torque at δR = 25°C . . . . . . . . . Nm

MiHδ Generated Stall Torque at δR . . . . . . . . . . . . Nm

MR Friction Torque . . . . . . . . . . . . . . . . . . Nm

n Speed . . . . . . . . . . . . . . . . . . . . rpm

nB Load Speed . . . . . . . . . . . . . . . . . . rpm

ni Ideal No Load Speed . . . . . . . . . . . . . . rpm

no No Load Speed . . . . . . . . . . . . . . . . . rpm

P1 Electrical Input . . . . . . . . . . . . . . . . . . W

P1B Electrical Inputin Loaded Condition . . . . . . . . . . . . . . . . W

P2

Mechanical Output . . . . . . . . . . . . . . . . . . . . W

P2B Mechanical Outputin Loaded Condition . . . . . . . . . . . . . . . . W

Pi Generated Mechanical Power . . . . . . . . . . . . W

PJ Power Loss in Joule . . . . . . . . . . . . . . . . W

PJB Power Loss in Joulein Loaded Condition . . . . . . . . . . . . . . . . W

R Terminal Resistance at δR = 25°C . . . . . . . . . . Ω

Rδ Terminal Resistance at δR . . . . . . . . . . . . . . ΩRth Thermal Resistance . . . . . . . . . . . . . . . K/W

Rth1 Thermal Resistance Rotor-Housing . . . . . . . . . K/W

Rth2 Thermal Resistance Housing-Ambient . . . . . . . . K/W

t Time . . . . . . . . . . . . . . . . . . . . . . . s

U Terminal Voltage . . . . . . . . . . . . . . . . . V

αcu Thermal Resistance Coefficientfor Copper . . . . . . . . . . . . . . . . . . . . 1/K

η Efficiency . . . . . . . . . . . . . . . . . . . . %

ηB Efficiency in Loaded Condition . . . . . . . . . . . . %

ηmax Max Efficiency . . . . . . . . . . . . . . . . . . %

δR Rotor Temperature . . . . . . . . . . . . . . . . °C

δU Ambient Temperature . . . . . . . . . . . . . . . °C

∆δ Temperature Rise of Winding . . . . . . . . . . . . °C

τ Mechanical Time Constant . . . . . . . . . . . . . . s

τm Electromechanical Time Constantof the Rotor . . . . . . . . . . . . . . . . . . . . s

τth Thermal Time Constant of the Motor . . . . . . . . . . s

ω Angular Speed . . . . . . . . . . . . . . . . . rad/s

ωi Ideal Angular Speed . . . . . . . . . . . . . . . rad/s

ωo No Load Angular Speed . . . . . . . . . . . . . rad/s

maxon motor

209

Barquero Barquero

Barquero Barquero

. . . Graphite Brushes, 1.5 Watt, sleeve bearings, 2 shafts A 118552 118553 118554 118555 118556 118557 118558 118559 118560 118561 118562 118563 118564 118565 118566 . . . Graphite Brushes, 1.5 Watt, sleeve bearings, 2 shafts B 118567 118568 118569 118570 118571 118572 118573 118574 118575 118576 118577 118578 118579 118580 118581

1 Assigned power rating W 1.5 1.5 1.5 1.5 1.5 1.5 1.5 1.5 1.5 1.5 1.5 1.5 1.5 1.5 1.52 Nominal voltage Volt 1.20 1.50 2.40 3.00 3.60 4.20 4.80 6.00 7.20 9.00 12.00 12.00 15.00 18.00 20.003 No load speed rpm 12800 12900 13600 13100 12900 13300 12300 12700 12300 12300 13300 12300 13100 14000 133004 Stall torque mNm 3.03 2.75 3.09 3.04 3.18 3.46 3.20 3.21 3.13 3.11 3.27 3.00 3.22 3.51 3.225 Speed/torque gradient rpm/mNm 4840 5460 5030 4930 4600 4300 4340 4470 4430 4490 4590 4680 4620 4480 46806 No load current mA 473 389 257 196 159 140 110 92.4 73.6 59.1 49.2 44.7 38.8 35.1 29.77 Starting current mA 3880 2880 2090 1590 1360 1290 971 804 633 504 428 367 334 320 2548 Terminal resistance Ohm 0.309 0.522 1.15 1.88 2.65 3.27 4.94 7.46 11.4 17.9 28.1 32.7 44.9 56.2 78.99 Max. permissible speed rpm 16000 16000 16000 16000 16000 16000 16000 16000 16000 16000 16000 16000 16000 16000 16000

10 Max. continuous current mA 720 720 720 657 554 499 405 330 267 213 170 157 134 120 10111 Max. continuous torque mNm 0.563 0.688 1.06 1.26 1.30 1.34 1.34 1.32 1.32 1.31 1.30 1.29 1.29 1.31 1.2912 Max. power output at nominal voltage mW 930 848 1020 968 1000 1120 960 993 936 929 1060 896 1030 1200 104013 Max. efficiency % 44.1 42.1 44.5 44.5 45.6 47.1 46.1 45.9 45.7 45.5 46.0 44.7 45.8 47.1 45.714 Torque constant mNm/A 0.782 0.955 1.48 1.91 2.34 2.69 3.30 3.99 4.95 6.17 7.64 8.16 9.64 10.9 12.715 Speed constant rpm/V 12200 10000 6470 5000 4070 3550 2890 2390 1930 1550 1250 1170 991 873 75316 Mechanical time constant ms 17.4 15.9 14.5 14.1 13.8 13.6 13.5 13.5 13.5 13.4 13.5 13.5 13.5 13.4 13.517 Rotor inertia gcm2 0.343 0.278 0.276 0.274 0.287 0.302 0.298 0.289 0.290 0.286 0.280 0.276 0.279 0.286 0.27518 Terminal inductance mH 0.01 0.01 0.02 0.04 0.05 0.07 0.11 0.16 0.24 0.38 0.58 0.66 0.92 1.19 1.6019 Thermal resistance housing-ambient K/W 46.00 46.00 46.00 46.00 46.00 46.00 46.00 46.00 46.00 46.00 46.00 46.00 46.00 46.00 46.0020 Thermal resistance rotor-housing K/W 14.00 14.00 14.00 14.00 14.00 14.00 14.00 14.00 14.00 14.00 14.00 14.00 14.00 14.00 14.00

maxon DC motorRE ∅13 mm, Graphite Brushes, 1.5 Watt

maxon Modular System

Planetary Gearhead Digital-Magnet-∅13 mm Encoder Ø13 mm0.2-0.35 Nm 16 CTP, 2 chan.Details page 124 Details page 182

Planetary Gearhead∅13 mm0.05-0.15 NmDetails page 125

Motor Type: Order Numbermaxon DC motor RE ∅13 mm . . . Motor Data

A without mounting threads B with mounting threads

B A/B

Operating Range Comments (Details on page 36)

Recommended operating range

Continuous operationIn observation of above listed thermal resistances(lines 19 and 20) the maximum permissible rotortemperature will be reached during continuous oper-ation at 25°C ambient.= Thermal limit.

Short term operationThe motor may be briefly overloaded (recurring).

Stock program

Standard program

Special program (on request!)

Axial play 0.05 - 0.15 mm Max. sleeve bearing loads

axial (dynamic) 0.8 Nradial (5 mm from flange) 1.4 NPress-fit force (static) 15 Nsame as above, shaft supported 140 N

Radial play/sleeve bearings 0.014 mm Ambient temperature range -20/+65°C Max. rotor temperature +85°C Number of commutator segments 7 Weight of motor 15 - 18 g Values listed in the table are nominal.

For applicable tolerances (see page 33)and additional details please request our computer printout.

maxon motor

44 maxon DC motor April 1999 edition / subject to change

Barquero Barquero

Barquero Barquero


Referencias

Referencias

315

Referencias

[1] Manual de usuario de MATLAB. Prentice Hall.

[2] Matshuhiro Ogata. Problemas de Ingeniería de control utilizando MATLAB. Prentice Hall.

[3] Gene F. Franklin, J. David Powel, Abbas Amani-Nacini. Control de Sistemas dinámicos conretroalimentación. Addison-Wesley Iberoamericana.

[4] Benjamin C. Ruo. Sistemas de Control Automático. Prentice Hall.

[5] J. Mª. Angulo, Rafael Avilés. Curso de Robótica. Paraninfo.

[6] Ebwin Wise. Applied Robotics. P. Prompt Plublications.

[7] H. R. Everett. Sensors for Mobile Robots Theory and Application. A.K. Peters ltd.

[8] Francisco J. Fernández, Marcote García Asensio. Estudio cinemático de robots móviles con unnúmero arbitrario de ruedas (Proyecto Final de Carrera). Universidad de Alcalá de Henares.

[9] Manuel Silva, Armando Roy. Inteligencia Artificial y Robótica Industrial. Universidad de Zaragoza.

[10] Física 3. edebé.

[11] Apuntes de la asignatura Control Automático de 2 curso de Ingeniería Técnica Industrial Electrónica

Para obtener más información relativa al Proyecto o referente a los robots móviles,consultar:

HTTP: // [email protected]


in_3 5

out_5

1

out_1

4.948e-

Kb1 M1

1

in_1



+- 0.24e-3s+11.4

1

Armadura M1

3

out_3

4.95e-3

Ki M1Intensidad M1

-+

Velocidad M10.29e-7s+2.155e-6

1

Fricción M1Voltaje M1

7

out_7s

1

Out_3-Out_4 Intensidad (A) de los motores

Out_5-Out_6 Posición de los motores ( N·m)

Out_5-Out_6 Par de los motores (N·m)

Out_1-Out_2 Velocidad (rad/s) de los motores

0.29e-7s+2.155e-61


-+4.95e-3

Ki M2Intensidad M2

4

out_4

0.24e-3s+11.4

1

Armadura M2

+-Voltaje M2

2

out_2


2

in_2

4.948e-

Kb1 M2

6

out_6


8

out_8s

1

0 10 20 30 40 50 60 70 80 90 100 110 120 130

10

20

30

40

50

60

70

80

90

O

SIMULADOR EN MATLAB DEROBOTS MÓVILES

DISEÑO DE ROBOTS CON SENSORES,CONTROLADORES, COMPORTAMIENTOS,OBSTÁCULOS. ANIMACIÓN GRÁFICA


Manual de Usuario

AUTOR: Santos Barquero CáceresDIRECTOR: Albert Oller i Pujol

FECHA: Junio / 2001

Índice del Manual de Usuario

i

1. Índice

Página2. Introducción .............................................................................................................. 2

3. Instalación del Programa ..........................................................................................2

3.1 Requerimientos de Hardware y Software ................................................................................... 23.1.1 El Mouse y Otros Dispositivos para Señalar .......................................................................3

3.2 Mejoras en el Rendimiento ........................................................................................................... 33.3 Instalación ..................................................................................................................................... 3

4. Inicio del simulador ................................................................................................... 4

4.1 Presentación del Entorno de Trabajo del Simulador .................................................................54.1.1 Ventana de Simulación .........................................................................................................54.1.2 Barra de Menús ....................................................................................................................54.1.3 Herramientas de Trabajo en la Simulación ......................................................................... 54.1.4 Área de presentación de Mensajes y Tiempos ..................................................................... 6

5. Configuración de la Simulación ............................................................................... 6

5.1 Presentación de la Pantalla de Configuración ............................................................................ 65.2 Cambio de la Configuración .........................................................................................................75.3 Salir-guardar la Configuración ....................................................................................................7

6. Herramientas para la Simulación ............................................................................ 7

6.1 Introducción ...................................................................................................................................76.2 Seleccionar Tipo Movimiento ....................................................................................................... 7

6.2.1 Grasping ...............................................................................................................................86.2.2 Forward ................................................................................................................................96.2.3 Tracking ............................................................................................................................... 96.2.4 Other .....................................................................................................................................10

6.3 Herramientas del Robot ................................................................................................................106.3.1 Como Crear un Nuevo Robot ...............................................................................................116.3.2 Seleccionar el Índice de un Robot ........................................................................................116.3.3 Borrar un Robot ................................................................................................................... 126.3.4 Posicionar un Robot a Partir del Mouse ............................................................................. 126.3.5 Posicionar un Robot a Partir del Teclado ........................................................................... 136.3.6 Edición del Robot .................................................................................................................14

6.4 Otras Herramientas .......................................................................................................................146.4.1 Logger .................................................................................................................................. 146.4.2 Trail ......................................................................................................................................156.4.3 Clear .....................................................................................................................................15

7. Diseño del Robot ........................................................................................................ 16

7.1 Presentación de la Pantalla de Diseño del Robot ........................................................................177.1.1 Ventana de Diseño del Robot ...............................................................................................177.1.2 Barra de Menús ....................................................................................................................177.1.3 Herramientas para el Diseño del Robot .............................................................................. 17

7.2 Cambio de Tamaño del Robot .....................................................................................................17

Índice del Manual de Usuario

ii

7.3 Sensores .......................................................................................................................................... 187.3.1 Crear un Nuevo Sensor ........................................................................................................ 187.3.2 Seleccionar un Sensor .......................................................................................................... 197.3.3 Actualizar Datos de un Sensor .............................................................................................207.3.4 Borrar un Sensor ..................................................................................................................207.3.5 Programación de la Máquina de Estado ............................................................................. 21

7.4 Modelado del Sistema de Control del Robot (Fichero Simulink) ..............................................247.4.1 Crear un Nuevo fichero Simulink de Control ...................................................................... 247.4.2 Edición de un Fichero Simulink de Control ya Existente .................................................... 267.4.3 Cambio del Fichero Simulink de Control ............................................................................ 27

7.5 Guardar en un Fichero el Diseño del Robot ................................................................................277.6 Recuperar de un Fichero el Diseño del Robot .............................................................................287.7 Salir-cerrar pantalla de diseño del Robot ................................................................................... 29

8. Diseño del Entorno de Obstáculos ............................................................................30

8.1 Presentación de la Pantalla de Diseño de Obstáculos .................................................................308.1.1 Ventana de Diseño de Obstáculos ........................................................................................318.1.2 Barra de Menús ....................................................................................................................318.1.3 Herramientas para el Diseño de Obstáculos ....................................................................... 31

8.2 Crear un Nuevo Obstáculo ........................................................................................................... 318.3 Seleccionar un Obstáculo ..............................................................................................................328.4 Posicionar un Obstáculo a partir del Mouse ...............................................................................328.5 Actualizar Datos de un Obstáculo ................................................................................................338.6 Borrar un Obstáculo ......................................................................................................................348.7 Guardar en un Fichero el Entorno de Obstáculos ......................................................................358.8 Recuperar de un Fichero el Entorno de Obstáculos ...................................................................368.9 Salir-cerrar la Pantalla de Diseño del Entorno de Obstáculos ..................................................37

9. Simulación .................................................................................................................. 38

9.1 Inicio de la Simulación .................................................................................................................. 389.2 Simulación Paso a Paso ................................................................................................................. 399.3 Simulación Continuada .................................................................................................................399.4 Stop Simulación ............................................................................................................................. 39

10. Modo de Presentación y Estudio de los Resultados ................................................40

10.1 Presentación Gráfica de los Resultados ....................................................................................4010.2 Guardar Resultados de la Simulación ...................................................................................... 4210.3 Recuperar Resultados de la Simulación ................................................................................... 43

11. Fin de la Simulación .................................................................................................. 44

11.1 Salir del Simulador ..................................................................................................................... 4411.2 Reset del Simulador ....................................................................................................................44

Simulador en MATLAB de Robots MóvilesManual de Usuario

Manual de Usuario

2

2 IntroducciónEl programa de simulación de Robots móviles basado en el entorno de MATLAB V4.2.1c,

es de gran sencillez de manejo, sin complicadas y ni rebuscadas herramientas para la interacción

con el usuario. En el manual que se presenta a continuación, se exponen las diferentes opciones,

herramientas y métodos para que el usuario utilice este programa didáctico sin necesidad de un

profundo y extenso estudio del mismo, ya que un diseño sencillo de las herramientas, como el que

se ha pretendido realizar, conlleva una utilización fácil y rápida.

En los siguientes apartados, se explica el modo y la forma de utilizar el programa, y así

ayudar al usuario a entender y desarrollar todas las herramientas correctamente. Es importante

entender el funcionamiento del programa para obtener el mayor rendimiento posible, con unos

resultados, que permitan un estudio del modelado del Robot realizado por parte del usuario.

3 Instalación del ProgramaLa instalación del programa de simulación es muy fácil de realizar gracias al CD-ROM

donde se recopila el simulador y la documentación técnica referente a este programa.

3.1 Requerimientos de Hardware y SoftwareLos requisitos básicos del sistema para la instalación y funcionamiento del simulador son

los mismos de MATLAB V4.2.1.c, extraídos del Manual de usuario.

- Un IBM, Compaq, o sistema compatible al 100% con un procesador Intel (o

compatible) 386, 486, o Pentium.

- Microsoft Windows 3.1.

- Instalación de MATLAB V4.2.1c.

- Unidad de disquetes de alta densidad y/o unidad de lector de CD-ROM.

- Monitor y ratón soportado por Microsoft Windows.

- 1 MB de espacio libre en el disco duro.

- 8 MB de memoria extendida.

Los siguientes elementos son los que se recomiendan de forma opcional:

- Memoria adicional.

- Adaptador gráfico y pantalla de 8 bits (para 256 colores simultáneos).

- Tarjeta aceleradora gráfica soportada por Microsoft Windows.

Manual de Usuario

3

3.1.1 El Ratón y Otros Dispositivos para SeñalarEl simulador usa un interfaz gráfico, beneficiándose de menús desplegables y de los push-

buttons que nos permiten realizar selecciones de forma rápida y sencilla. Aunque en la mayoría de

las aplicaciones de MATLAB el ratón no es necesario, en este caso, es imprescindible para el

trabajo con el simulador, ya que por ejemplo los push-buttons sólo pueden utilizarse con el ratón.

3.2 Mejoras en el RendimientoLa gran ventaja y virtud de MATLAB es que aporta un entorno rico y poderoso, de forma

que le permite aprovechar todas las capacidades del hardware. Esto significa que el rendimiento de

MATLAB, y por tanto, de nuestro programa de simulación, depende muy directamente de la

configuración del hardware que se utilice. Entre otros, los siguientes factores afectan a la velocidad

de operación de MATLAB y de la aplicación, que en este caso es nuestro simulador.

- Tipo de procesador y frecuencia del reloj.

- Cantidad de memoria física disponible.

- Velocidad de acceso al disco.

- Tarjeta aceleradora gráfica.

De los factores mencionados anteriormente, MATLAB se beneficia y aumenta su

rendimiento gracias a la mayor disponibilidad de memoria física en la computadora, mejorando la

velocidad de respuesta en las animaciones y de los cálculos, que es lo que afecta al simulador.

3.3 InstalaciónLa instalación del simulador se realiza mediante el CD-ROM que se adjunta en la

documentación técnica, y siguiendo estos pasos:

1) Crear una carpeta en el disco duro para instalar el simulador.

2) Abrir el CD-ROM.

3) Abrir carpeta del CD-ROM denominada “Programa”.

4) Copiar todas las funciones que contiene esta carpeta, y pegarlas en la carpeta que se ha

creado en el disco duro para la instalación del simulador.

5) Finalizada la copia, el programa ya esta instalado y preparado para ser ejecutado desde

el entorno de MATLAB.

Manual de Usuario

4

4 Inicio del SimuladorUna vez instalado el programa, el usuario, para iniciar una sesión de trabajo con el

simulador, debe abrir el programa MATLAB V4.2c.1. Desde el entorno de trabajo de MATLAB, se

entra en el directorio raíz donde se ha instalado el programa de simulación, para ejecutar la función

comensa, que abre y presenta la pantalla de simulación del programa, figura “Physical Robot

Simulator”.

» comensa

La pantalla de simulación, figura “Physical Robot Simulator”, varía debido al tipo de

monitor que posea el usuario. En la Figura 1 se muestra la pantalla de trabajo inicial sobre un

monitor SVGA de 17 pulgadas.

Figura 1. Pantalla de simulación en un monitor de 17 pulgadas

NOTA: Antes de iniciar la sesión de trabajo con el programa, se recomienda cerrar todas

las figuras no necesarias, para no ralentizar el normal funcionamiento del simulador (liberar

memoria).

Barra de títuloBarra de menús

Ventana de simulación

Herramientas de simulación

Herramientas del Robot

Tiempos de la simulación

Herramientas de los obstáculos

Herramientas de resultados

Manual de Usuario

5

4.1 Presentación del Entorno de Trabajo del SimuladorCuando iniciamos la sesión se presenta la pantalla de simulación, que consta de 4 áreas: la

ventana de simulación (zona de movimientos de los Robots, ocupa la mayor parte de la pantalla), la

barra de menús, el área de las herramientas de trabajo (dividida en diferentes funciones, Movement,

Obstacles, etc.) y el área de información al usuario para los mensajes de “Warning” y de

información relacionada con los tiempos de simulación del programa. En conjunto, las cuatro áreas

anteriores se conocen como la Pantalla Principal de Simulación, figura “Physical Robot Simulator”.

4.1.1 Ventana de SimulaciónComo ya hemos mencionado, la ventana de simulación ocupa la mayor parte de la pantalla

de presentación del programa. Esta ventana, dividida en retículas de 10 unidades, tiene unas

coordenadas totales mínimas y máximas [x,y] → MIN[0,0] MAX[130,90]. Los ejes definidos en

esta ventana, nos sirven de referencia para las coordenadas [x,y] durante la simulación.

4.1.2 Barra de MenúsEn la parte superior de la figura de presentación de la simulación se encuentra la barra de

menús. En ella, podemos seleccionar órdenes del entorno propio de MATLAB V4.2.1.c

(presentadas en todas las figuras de MATLAB V4.2.1.c) para trabajar de diferentes formas con la

información de la pantalla de simulación. La barra de menús de MATLAB V4.2.1.c, por defecto,

posee en cualquier figura de: File, Edit, Windows, Help. Pero además, en nuestro caso, y gracias a

la programación, se dispone del menú Options.

4.1.3 Herramientas de Trabajo en la SimulaciónLa parte derecha de la pantalla de simulación, presenta las diferentes herramientas de

trabajo para la simulación y el normal funcionamiento del programa. Esta área se divide en

subáreas, dependiendo de la función realizada por esas herramientas: inicio de la simulación,

selección del tipo de movimiento, etc.

Este manual, se basa en el modo de utilización de las herramientas del programa. Están

clasificadas según: la acción que realizan, la forma de manipulación y la información que aporten

en la simulación (push-buttons, buttons radio, etc.)

Manual de Usuario

6

4.1.4 Área de Presentación de Mensajes y TiemposEsta área, que no puede ser manipulada por el usuario, sirve para comunicar e informar

sobre los errores de manipulación al usuario, además de presentar los tiempos de simulación. Es

importante, que el usuario entienda la información aportada en esta área para que sirva realmente

de ayuda.

5 Configuración de la Simulación

5.1 Presentación de la Pantalla de ConfiguraciónAntes de iniciar la simulación, se recomienda revisar la configuración con la que se ha

abierto el programa, para comprobar o corregir los datos según convenga al usuario.

Para que se muestre la pantalla de configuración, el usuario debe de ir a la barra de menús

en la parte superior de la figura “Physical Robot Simulator”, y en el menú Options, se muestra el

botón (label) Change Data Config, que al pulsarlo, abre la pantalla de configuración (Figura 2).

Figura 2. Pantalla de configuración en un monitor de 17 pulgadas

Número máximo deRobotsTamaño máximo delRobotTamaño mínimo delRobot

Número máximo desensoresDistancia máximade detecciónDistancia mínima dedetecciónÁngulo máximo dedetecciónÁngulo mínimo dedetecciónÁngulo máximo derotación del sensor

Número máximo deobstáculosTamaño máximo delobstáculo

Tiempo de duración delos intervalos de muestreo

Botón de Salida de laconfiguración

Selección de la velocidad de la representacióngráfica en simulación continua (Run).

Manual de Usuario

7

5.2 Cambio de la ConfiguraciónComo se observa en la figura anterior, la pantalla de configuración, figura “Config of Data

Simulator”, está compuesta por diferentes datos presentados en pequeñas casillas de texto, donde el

usuario puede modificar el valor de configuración a su conveniencia, pero siempre dentro de unos

valores acotados y establecidos.

5.3 Salir-Guardar la ConfiguraciónFinalizada la operación de reconfiguración, o simplemente la comprobación, por parte del

usuario de los valores de configuración del programa, para salir (cerrar) la pantalla salvando los

cambios realizados, se debe pulsar el push-button . En el caso de no realizar cam-

bios, o que no se desee guardar las modificaciones, se cierra pulsando el botón Cerrar de la figura.

Es preferible utilizar siempre, haya o no modificaciones, el push-button .

6 Herramientas para la Simulación

6.1 IntroducciónPresentado el entorno de trabajo del programa, en los siguientes apartados se explican las

diferentes herramientas para llevar a cabo todo el proceso de inicialización de la simulación.

6.2 Seleccionar Tipo de MovimientoEl seleccionar un tipo de movimiento u otro, se debe a la necesidad del usuario de realizar

un modo de simulación determinado y concreto para observar y estudiar los resultados obtenidos

ante diferentes situaciones y entornos de simulación del Robot. Por ejemplo:

- Observar-estudiar la respuesta del Robot en un movimiento en línea recta.

- Observar-estudiar la respuesta del Robot en un movimiento hacia unas coordenadas

concretas determinadas por el usuario.

Manual de Usuario

8

6.2.1 GraspingEste movimiento, consiste en transportar un objeto triangular asociado a cada Robot. El

Robot va a buscar el objeto y lo transporta hasta las coordenadas de destino de forma cíclica, es

decir, que una vez transportado el objeto, retorna automáticamente a las coordenadas de origen,

para volver a empezar el ciclo de transporte.

Se activa pulsando sobre el button tipo radio .

Creación del Objeto Grasping

Se creará el objeto grasping asociado, conjuntamente con el Robot, si el button tipo radio

permanece activado cuando se crea éste. En caso contrario, si el Robot ya ha sido

creado, pero en otro tipo de movimiento, al seleccionar el button radio se creará auto-

máticamente el objeto grasping asociado a cada Robot existente.

Borrar el Objeto Grasping

El objeto grasping, se borra conjuntamente con el Robot si se pulsa el botón

(ver apartado 6.3.3 Borrar un Robot). Cuando se realiza un cambio de tipo de movimiento, también

se borra el objeto grasping, pero no se borra el Robot.

Actualizar las coordenadas del Objeto Grasping

Cuando se selecciona un tipo de movimiento a partir de los buttons tipo radio, se activan o

desactivan diferentes herramientas específicas de ese movimiento. En este caso, el botón de

actualizar las coordenadas del objeto grasping:

Los pasos a seguir para la operación de actualizar las coordenadas del objeto grasping son:

1) Seleccionar en el popup de índices del Robot, el índice del Robot asociado

al objeto grasping deseado.

2) Comprobar que el color del popup coincide con el color del objeto deseado.

Manual de Usuario

9

3) Modificar los valores en las casillas de texto, por las nuevas coordenadas.

Estas coordenadas se limitan a [xMIN, yMIN] = [0 + R,0 + R], [xMAX, yMAX] = [130 + R,90 + R].

Siendo R el radio del objeto grasping.

4) Pulsar el botón .

Realizados los pasos anteriores, el objeto grasping ya ha cambiado de posición.

NOTA: No es posible modificar las coordenadas de destino del objeto grasping. Además, no puede

ser superpuesto a otros elementos de la simulación (mensaje de “Warning”).

Warning: Superposition Object "grasping" nº 2 Coordinates x = 53.5 y = 21

6.2.2 ForwardEn la activación de este movimiento, no son necesarias herramientas adicionales como en

otros casos, ya que se “programa” al Robot para ir siempre en línea recta. Este tipo de movimiento,

no tiene mucho sentido si es que no se ha dotado al Robot de un sistema de sensores (Máquina de

Estados). Pulsando sobre el buttton tipo radio , se activa este movimiento, desactivan-

do los otros movimientos, elementos y herramientas no necesarias.

6.2.3 TrackingPulsando sobre el button tipo radio , se activa este movimiento que consiste en

hacer describir de forma automática al setpoint unas figuras determinadas para que el Robot tenga

una consigna de posición variable, pero que puede ser repetida posteriormente por el usuario de

forma exacta, y así poder comparar diferentes controles de posición.

Herramientas activadas en el movimiento Tracking

Forma de la figura a describir

Cuando se activa este tipo de movimiento, se presenta un popup compuesto por:

donde se da la posibilidad de elegir entre diferentes figuras a

describir por el setpoint durante la simulación.

Manual de Usuario

10

Desfase de las figuras

Esta herramienta, permite introducir en la casilla de texto el ángulo de desfase de las

figuras en los casos de: Circular, Arc y Spiral, para variar la forma de éstas ( ).

6.2.4 OtherEste movimiento está activado por defecto en el inicio del programa , permite al

usuario asignar a través de las casillas activadas a tal efecto, unas coordenadas concretas al

setpoint, para que el Robot se dirija a estas coordenadas de destino asignadas.

Los pasos a seguir para la asignación de la consigna de posición (coordenadas de destino)

al Robot son:

1) Seleccionar el índice del Robot .

2) Asignar las nuevas coordenadas en las casillas de texto (setpoint):

3) Validar las nuevas coordenadas con el botón .

Cada Robot, puede tener sus propias coordenadas de destino asignadas a sus respectivos

setpoint’s.

6.3 Herramientas del RobotAnteriormente se ha hablado de las herramientas asociadas al tipo de movimiento, ahora se

presentan las herramientas específicas para el trabajo con el Robot. Estas herramientas se asocian

directamente al trabajo con el Robot en la pantalla de simulación, figura “Physical Robot

Simulator”, o en la pantalla de diseño, figura “Design Robot”, como es el caso de y

.

Manual de Usuario

11

6.3.1 Como Crear un Nuevo RobotEn el entorno de la pantalla de simulación, figura “Physical Robot Simulator”, se muestra

la herramienta necesaria para crear un nuevo Robot. Éste, para ser creado debe tener unos datos

iniciales introducidos por el usuario, como son las coordenadas de posición y el ángulo de rotación.

Además, en la misma operación, se permite diseñar-modelar el Robot, abriendo la pantalla de

diseño preparada a tal efecto (ver apartado 7 Diseño del Robot). Los pasos a seguir para crear un

Robot, consisten en:

1) Introducir las coordenadas, donde se desea crear el nuevo

Robot.

Estas coordenadas se limitan a [xMIN, yMIN] = [0 + R,0 + R], [xMAX, yMAX] = [130 + R,90 + R].

Siendo R el radio del Robot (tamaño establecido en la pantalla de diseño).

2) Introducir el ángulo de rotación para el nuevo Robot en

Limitado a [0º < ...< 360º].


Una vez realizados estos pasos, se crea un Robot, abriendo a continuación la pantalla de

diseño del Robot, figura “Design Robot”.

En el caso de que las coordenadas de inicio del Robot, lo superpongan a otro elemento de

la simulación, o se supere el número máximo de los Robots permitidos, la operación de crear un

nuevo Robot se anula, presentando el pertinente mensaje de “Warning”.

Cuando se crea un Robot, sí el button tipo radio permanece activado, se crea

conjuntamente el objeto grasping asociado.

6.3.2 Seleccionar el Índice de un RobotPara realizar algún tipo de operación sobre cualquier Robot, primero es necesario

seleccionar en el popup de índices, el índice de ese Robot (en este caso índice = 1),

comprobando que el popup tenga el mismo color que el Robot que se desea

seleccionar. Una vez seleccionado el índice, se puede realizar la operación deseada

sobre el Robot.

Manual de Usuario

12

Observar que cuando se selecciona el índice del Robot, en las

casillas de texto se presentan los datos relativos a ese Robot.

6.3.3 Borrar un RobotExiste la posibilidad de borrar un Robot, cuando el usuario lo crea conveniente. Para borrar

el Robot no es necesario ninguna condición ni característica determinada, únicamente, que el Robot

haya sido creado anteriormente.

1) Seleccionar el índice del Robot que se desea borrar .


Realizados los pasos anteriores, el Robot se borra, dejando libre el índice

que ocupaba.

6.3.4 Posicionar un Robot a partir del MouseUna de las herramientas más útiles en la simulación, es la de Setmouse, ya que permite dar

al Robot unas coordenadas de posición y un ángulo de forma rápida, a partir de la utilización del

mouse. En los siguientes pasos, se explica el modo de utilización de esta herramienta:


2) Pulsar botón .

3) En la ventana de simulación, hacer click en las coordenadas exactas donde se desee

colocar el Robot. Estas coordenadas se limitan a [xMIN, yMIN] = [0 + R,0 + R], [xMAX, yMAX] = [130 +

R,90 + R]. Siendo R el radio del Robot (tamaño establecido en la pantalla de diseño).

4) Con un segundo click, a una cierta distancia y con un ángulo respecto al primer click, se

introduce el ángulo de giro del Robot (orientación a [0º < ...< 360º]).

Al finalizar el segundo click, el Robot se redibuja en la nueva

posición. Y en las casillas de texto aparecen los valores de las nuevas

coordenadas.

Manual de Usuario

13

Si las coordenadas introducidas con el primer click superponen el Robot con otro elemento

de la simulación (que no sea su objeto grasping), se anula la operación de Setmouse, presentándose

el mensaje de “Warning” pertinente.

NOTA: Si se desea un ángulo de 0º, simplemente se tienen que realizar los dos clicks en las mismas

coordenadas.

6.3.5 Posicionar un Robot a Partir del TecladoDel mismo modo que en el caso anterior, esta herramienta permite mover el Robot en el

interior de la ventana de simulación, pero en este caso, las coordenadas y el

ángulo de rotación se introducen por parte del usuario a través del teclado

en las casillas de texto. Los pasos a seguir para realizar esta operación son:


2) Introducir coordenadas y ángulo en las casillas de texto.

Estas coordenadas se limitan a [xMIN, yMIN] = [0 + R,0 + R], [xMAX, yMAX]

= [130 + R,90 + R] y orientación a [0º < ...< 360º]. Siendo R el radio

(tamaño establecido en la Pantalla de diseño) del Robot.

3) Pulsar botón .

Al pulsar el botón , el Robot se redibuja automáticamente en las nuevas

coordenadas y con el nuevo ángulo.

Si las coordenadas introducidas por teclado superponen el Robot con otro elemento de la

simulación (que no sea su objeto grasping), se anula la operación de SetKeyboard, presentándose el

mensaje de “Warning” pertinente.

Manual de Usuario

14

6.3.6 Edición del RobotEn la simulación, se permite diseñar el Robot con unas determinadas características, según

convenga al usuario. Cuando se crea el Robot, se presenta la pantalla de diseño del Robot, figura

“Design Robot”, de forma automática. Pero en el caso que el usuario durante la simulación desee

corregir o modificar alguna característica del diseño realizado, debe utilizar la herramienta

siguiendo estos pasos:

1) Seleccionar el índice del Robot

2) Pulsar botón .

Al pulsar el botón se abre la pantalla de diseño del Robot, donde se muestra

el modelado-diseño actual del Robot, además de una serie de herramientas para su edición (ver

apartado 7 Diseño del Robot).

6.4 Otras herramientasEn los apartados siguientes se explican herramientas útiles para la simulación, que afectan

a todos los Robots por igual.

6.4.1 LoggerEsta herramienta, se basa en un button tipo radio , que al ser activado permite

almacenar los resultados de la simulación para que sean representados gráficamente o guardados en

un fichero de texto. En caso de estar desactivado , no se almacenan los resultados de la

simulación, y por tanto no habrá datos para la representación gráfica, ni para ser guardados en un

fichero, y consecuentemente, no se podrá realizar el estudio de los resultados. Por eso, esta opción

es activada por defecto en el inicio de la simulación.

Manual de Usuario

15

6.4.2 TrailEl button tipo radio si permanece activado durante la simulación del Robot, ya

sea Step ó Run, permite que el Robot deje un rastro en su recorrido por las diferentes coordenadas

de la ventana de simulación. En cambio, si esta desactivado , el Robot no deja el rastro

en su recorrido.

Figura de ejemplo del rastro que deja el Robot si .

NOTA: El rastro permanece visible aunque se desactive el

6.4.3 Clear

El botón de nos sirve para borrar todos los resultados acumulados hasta el

momento en la simulación, o para limpiar la ventana de simulación.

a) Si está activado y se pulsa el botón se limpia el rastro de los

Robots de la ventana de simulación.

b) Si está activado y se pulsa el botón se borran todos los resultados

de la simulación almacenados hasta el momento; velocidades, intensidad, etc. Además inicializa a 0

los tiempos de la simulación .

a) Si y están activados, o desactivados , y se

pulsa el botón , se borran de la memoria las funciones cargadas hasta el momento. Esta

opción es muy útil cuando se realiza alguna modificación en la función de programación de la

Máquina de Estados durante la simulación, ya que si se desea actualizar esta función en la memoria

del ordenador sería necesario hacer un Reset del simulador, pero gracias a esta opción que borra

todas las funciones cargadas, ya no es necesario (se obliga al ordenador a leerlas de nuevo del

disco, actualizando la modificación).

Manual de Usuario

16

7 Diseño del RobotEl programa de simulación permite a través de un entorno de trabajo preparado a tal efecto,

diseñar y modelar el Robot según convenga al usuario. Este entorno de trabajo se presenta cuando

se crea un nuevo Robot, o bien desde la pantalla de simulación figura “Physical Robot Simulator”

(Figura 1), pulsando el botón para la reedición de las características del Robot una vez

ya ha sido creado.

7.1 Presentación de la Pantalla de Diseño del RobotEl entorno de trabajo con las diferentes herramientas para el diseño y modelado del Robot,

se recogen en una misma pantalla de diseño, figura “Design Robot”, tal y como se muestra en la

Figura 3.

Figura 3. Pantalla de diseño del Robot en un monitor de 17 pulgadas

Esta pantalla consta de tres áreas: la ventana para el diseño del Robot (que ocupa la mayor

parte de ésta), la barra de menús y las diferentes herramientas de trabajo para diseñar-modelar el

Robot. En conjunto, las tres áreas anteriores se conocen como la pantalla de diseño del Robot,

figura “Design Robot” (Figura 3).

Herramientas de diseñode los sensores

Barra de títuloBarra de menús

Ventana de diseño

Herramientas de diseño del Robot

Herramientas de modelado del Robot

Manual de Usuario

17

7.1.1 Ventana de Diseño del RobotEn esta área de trabajo, se presenta el gráfico ampliado de un Robot (del mismo color que

el Robot editado), sobre unos ejes con origen en el centro del Robot (escalados de 5 en 5 unidades),

para que sirvan de guía al usuario en su diseño, de forma que tenga una referencia en el tamaño.

Será en este espacio, donde el usuario observará las modificaciones de tamaño e instalará

los sensores en el contorno del Robot.

7.1.2 Barra de MenúsLa barra de menús de la pantalla de diseño, presenta las mismas características que todas

las figuras del entorno de MATLAB V4.2.1.c. Pero, en este caso dispone además, del menú

Options compuesto por submenús que realizan las diferentes operaciones que se explican en

apartados posteriores.

7.1.3 Herramientas para el Diseño del RobotEsta área de trabajo, se divide en tres subáreas, dependiendo de la función a realizar:

cambio de tamaño del Robot, diseño de los sensores y modelado del sistema de control del Robot

(fichero simulink). Estas subáreas contienen diferentes tipos de herramientas; casillas de texto,

push-button, buttons tipo radio, para el trabajo de diseño y modelado del usuario.

7.2 Cambio de Tamaño del RobotEl Robot, definido en forma de trapecio para diferenciar la parte delantera (la estrecha) de

la trasera, se aproxima en su espacio vital ocupado, a un circulo de un determinado radio, a partir

del cual se determina su tamaño. El tamaño de este circulo (radio), puede ser modificado por parte

del usuario, siguiendo estos pasos.

1) Introducir el nuevo radio del Robot en .


Al finalizar la operación de cambio de tamaño, el Robot se redibuja, con su nuevo tamaño.

Este cambio, no afecta a la características en la simulación (no aumenta el par de carga sobre los

motores ni lo disminuye), sino que simplemente, afecta a la representación gráfica.

En caso de existir sensores en el momento de realizar el cambio de tamaño, se observará

como la posición de éstos en el contorno del Robot, no se modifica.

Manual de Usuario

18

Siempre que el valor de tamaño introducido esté fuera del margen de MAX/MIN marcado

en la configuración del programa, se corregirá automáticamente dándole el valor máximo ó

mínimo.

7.3 SensoresUna de las características más importantes en el diseño del Robot, es la posibilidad de

dotarlo de un sistema de sensores, a partir de las herramientas que se presentan en estos apartados.

Estos sensores, permitirán la programación de una Máquina de Estados que aporten un “sentido” en

los movimientos del Robot.

Las características de un sensor en el programa, se basan en tres datos:

- Distancia de detección del sensor .

- Directividad del sensor (ángulo de detección) .

- Ángulo de rotación del sensor respecto a su origen en el contorno del Robot.

7.3.1 Crear un Nuevo SensorSe puede añadir un nuevo sensor al Robot, en el mismo momento en que se ha creado, o

posteriormente, utilizando la posibilidad de edición del Robot , eso sí, el sensor se

crea siempre desde la pantalla de diseño del Robot, figura “Design Robot“. En los siguientes puntos

se explica como crear un nuevo sensor, pero desde la pantalla de simulación del Robot, figura

“Physical Robot Simulator” (Figura 3), en el caso que la pantalla de diseño ya este abierta, no será

necesario realizar los dos primeros pasos.



Una vez abierta la pantalla de diseño del Robot (figura “Design Robot“):

3) Introducir datos característicos del nuevo sensor

Los datos característicos están limitados a los valores de MAX/MIN establecidos en la

pantalla de configuración.


Manual de Usuario

19

5) Mover el mouse, hasta el punto del contorno del Robot donde se desea el nuevo sensor.

Si se intenta colocar el sensor fuera del contorno del Robot, se presenta un mensaje de

“Warning”, avisando del error.

Realizados los pasos anteriores, se puede observar como el nuevo sensor se ha creado con

las características de las casillas de texto.

NOTA: Cuando se crea un sensor, se señaliza su índice con un

número en color rojo que aparece en la ventana de diseño, junto al

sensor. Este índice, se asigna siguiendo un orden numérico.

7.3.2 Seleccionar un SensorPara realizar cualquier operación sobre un sensor, como borrarlo o editarlo, es necesario

seleccionar el índice del sensor deseado en el popup .

Cuando se selecciona el índice del sensor, éste

cambia de color, de verde a magenta, y además, se

introducen en las casillas de texto de forma automática, los

datos característicos del sensor.

1

1

2

3

4

5

6

Manual de Usuario

20

7.3.3 Actualizar Datos de un SensorUna vez creado un sensor, es posible cambiar las características de éste (pero no la posición

en el Robot), de forma fácil y rápida, utilizando la orden . El cambio de datos caracte-

rísticos, se realiza siguiendo estos pasos:

1) Seleccionar el índice del sensor .

2) Introducir los nuevos datos del sensor .


El sensor cambia los datos característicos viejos, por los nuevos valores, siempre y cuando

estos no superen el MAX/MIN de la configuración del programa. En caso contrario, el valor se

corrige de forma automática al MAX/MIN de la configuración.

Si alguno de los datos no fuera introducido correctamente, se presentará el pertinente

mensaje de error.

7.3.4 Borrar un SensorLos sensores pueden ser borrados por el usuario, cuando sea necesario, hayan sido creados

por el propio usuario o recuperados de un fichero de configuración del Robot.

1) Seleccionar el índice del sensor .

Observar como el sensor cambia de color, y sus datos característicos se introducen

automáticamente en .


1

2

3

4

5

6

1

2

3

4

5

6

6

5

4

2

3

Manual de Usuario

21

Una vez borrado el sensor, el índice de éste, queda libre de forma que es

posible volver a crear un nuevo sensor, ocupando el espacio que ha quedado vacío.

7.3.5 Programación de la Máquina de EstadosUna máquina de estados, es un sistema de lógica secuencial cuyas salidas dependen de las

entradas previas y actuales (en este caso las entradas corresponden al estado de los sensores); se

diferencia de los procesos que son sólo funciones de entradas actuales, aunque, en nuestro caso,

también podemos tener en cuenta sólo el estado actual de los sensores a la hora de realizar la

programación.

La programación de la Máquina de Estados de los sensores del Robot, debe realizarse en la

función que el usuario designe en (sin extensión). La

función designada por el usuario, tiene que programarse utilizando como plantilla la función de

ayuda maes(...). Es decir, para crearla primero editaremos la función maes(...), pero a la hora de

guardar, cambiaremos el nombre del archivo, por el nombre que se desea para la función de

programación de los sensores. El nuevo nombre, es el que designaremos después en:

En esta función, el usuario, tiene que programar todas las combinaciones lógicas posibles

en el estado de los sensores. Por ejemplo, en un sistema de x sensores, las combinaciones llegan a

ser yx, donde y es el número de estados posibles del sensor, “0” ó “1”, entonces; y = 2 → 2x.

La información necesaria para la programación, se recibe en los parámetros de entrada de

la función, de forma que:

function [selfdia] = newmaes(selfa,selfdia,ESTADO_SENSORES,ESTADO_VIEJO_SENSORES,a)

selfa = [xa ya theta radius velolineal veloangular pertur] vector de estado del RobotParámetros relativos a la posición, velocidades y par de carga del Robot estado.

selfdia = [xdia ydia libre] vector de estado del setpoint asociado al RobotParámetros relativos a la consigna posición (posición [x, y] de la diana-objetivo)Control de la dirección de Robot , el Robot siempre tiene como objetivos esta posición [xdia, ydia].

Manual de Usuario

22

ESTADO_SENSORES vector de estados de los sensores (actual)Este vector contiene el estado actual de los sensores:ESTADO_SENSORES(i) = Estado 0 ó 1 del sensor, siendo igual a 1 cuando el sensor detecta unobstáculo.

ESTADO_ANTERIOR_SENSORESvector de estados de los sensores (anterior)Este vector contiene el estado anterior de los sensores:ESTADO_ANTERIOR_SENSORES(i) = Estado 0 ó 1 del sensor, siendo igual a 1 cuando el sensordetecta un obstáculo.

a índice del Robot

Cuando el usuario, trata una combinación lógica del estado de los sensores, espera poder

controlar la dirección del Robot en función del estado actual y anterior de esos sensores. Por

ejemplo:

function [selfdia] = newmaes(selfa,selfdia,ESTADO_SENSORES,ESTADO_VIEJO_SENSORES,a)... ... ...if ESTADO_SENSORES(i) == 0 & ESTADO_ANTERIOR_SENSORES(i) == 1

acción;end;

La “acción” programada, se ejecutará en este caso, cuando actualmente el sensor número i

no detecte obstáculo, pero anteriormente si lo detectaba.

Ahora bien, una vez que el usuario sabe reconocer el estado de sus sensores, necesita poder

controlar la dirección y movimientos del Robot en función de este estado. Como ya se ha

comentado en anteriores apartados, la consigna de posición, y por tanto su dirección, esta

controlada por el setpoint asociado al Robot. Para cambiar la dirección del Robot, se tiene que

variar la posición actual del setpoint (diana) a través de su vector de estado selfdia (parámetro de

salida de la función designada por el usuario). Por ejemplo, aplicando el caso anterior, deseamos

que la “acción” programada sea girar el robot +45º (π/4), entonces (ver figura 4):

function [selfdia] = newmaes(selfa,selfdia,ESTADO_SENSORES,ESTADO_VIEJO_SENSORES,a)... ... ...% ----------------------------------------------------------------------% Inicio programación de la Máquina de Estados del Robot% ----------------------------------------------------------------------if ESTADO_SENSORES(1) == 0 & ESTADO_ANTERIOR_SENSORES(1) == 1

selfdia(1) = selfa(1) + 10 * cos(selfa(3) + (pi/4)); % Coordenada x del setpointselfdia(2) = selfa(2) + 10 * sin(selfa(3) + (pi/4)); % Coordenada y del setpoint

end;% ------------------------------------------------------------------% Fin programación de la Máquina de Estados del Robot% ------------------------------------------------------------------

donde recordar que, [selfdia(1) selfdia(2)] posición [x y] del setpoint (diana).[selfa(1) sefa(2)] posición [x y] e selfa(3) ángulo de orientación delRobot.

Manual de Usuario

23

x, y

xdia, ydia

45º

theta10

xdia = selfdia(1)ydia = selfdia(2)x = self(1)y = self(2)theta = self(3)

Figura 4. Ejemplo de programación de la consigna de posición del Robot

El set de una nueva posición para el setpoint, cambia la dirección del Robot. Con un

ejemplo como el anterior, podríamos realizar una nueva “acción”, en este caso, parar el Robot (ver

figura 5).

if ESTADO_SENSORES(1) == 0 & ESTADO_ANTERIOR_SENSORES(1) == 1selfdia(1) = selfa(1); % Al asignar como nueva consigna de posición, la posiciónselfdia(2) = selfa(2); % actual del Robot, este se para.

end;

x, y

xdia = x, ydia = y

theta

xdia = selfdia(1)ydia = selfdia(2)x = self(1)y = self(2)theta = self(3)

En este caso,

Figura 5. Ejemplo de programación de la consigna de posición del Robot

La tercera posición del vector de estado del setpoint, selfdia(3), es una posición “libre” para

uso propio del usuario, donde se puede guardar un dato o variable numérica.

Realizado el control de todas las combinaciones posibles, asignando un nueva consigna de

posición según el estado de los sensores, se puede dar por finalizada la programación de la máquina

de estados.

NOTA: En el caso que el usuario introduzca el nombre de un archivo que no existe para realizar el

control del estado de los sensores, el programa presentará un mensaje de aviso en la pantalla de

diseño, figura “Design Robot”, cuando ésta se intente cerrar.

Recordar siempre utilizar como plantilla la función maes(...), ya que nos servirá de ayuda.

Incluso en su interior se dispone de un ejemplo.

Manual de Usuario

24

7.4 Modelado del Sistema de Control del Robot (fichero simulink)El sistema de control del Robot, se modela en el fichero simulink de control referenciado

en . En este fichero, creado por el usuario, se modelan los motores y el

sistema de control del Robot. Durante la simulación, este fichero simulink se ejecuta para obtener

la respuesta del Robot (velocidades, intensidades, etc.). El correcto funcionamiento de la

simulación y la obtención de resultados coherentes, dependerá de si en este fichero se realiza un

buen modelo del sistema de control (PID, calculo de la consigna, etc.) y de los motores del Robot.

Este fichero simulink de control, es ejecutado durante la simulación en intervalos de duración igual

al tiempo de grain time establecido en la configuración del programa. Estos intervalos, “simulan”

un efecto de muestro, ya que de los resultados obtenidos durante estos intervalos de ejecución, solo

el último valor es guardado como resultado, sirviendo de entrada y como condición inicial en el

siguiente intervalo.

7.4.1 Crear un Nuevo Fichero Simulink de ControlPara crear un nuevo fichero simulink de control, el usuario debe de utilizar como base un

fichero “plantilla”, para que le sirva de guía, conservando las variables de entrada y salida, sin

modificar el formato. Este fichero “modelo” dependerá del sistema de calculo de la consigna,

realizado por el usuario. Si el cálculo de la consigna de posición y velocidad se realiza desde el

mismo fichero simulink de control, el fichero “modelo” será defaultc.m, en cambio, si el usuario

simplemente modela los motores y el control PID en el fichero simulink, sin tener en cuenta el

calculo de las consignas, el fichero “modelo” será motors.m, ya que el calculo de las consignas se

realizará a través de código de programa en la función dynamics(...).

1) Indicar, si en el fichero de control se realizará el cálculo de las consignas.

SI cálculo de las consignas en el fichero simulink de control

NO cálculo de las consignas en el fichero simulink de control


Manual de Usuario

25

3) Seleccionar en la caja de diálogo, el nombre del fichero “modelo”:

defaultc.m → SI cálculo de las consignas en el fichero simulink de control

motors.m → NO cálculo de las consignas en el fichero simulink de control

Pulsar el botón .

4) Una vez abierto el fichero “plantilla”, el usuario puede realizar las modificaciones

deseadas y crear un nuevo fichero, sobre la base del fichero “plantilla”.

5) Guardar el nuevo fichero, cambiando el nombre, utilizando

la orden de la barra de menús del programa simulink Save as.

Después cerrar el fichero o minimizarlo.

6) Introducir el nombre del nuevo fichero simulink de control en

(sin extensión).

7) Para que el programa reconozca el nuevo fichero simulink de control, es necesario salir-

cerrar la pantalla de diseño a partir de la orden .

En caso, que el usuario se equivoque e introduzca el nombre de un fichero que no existe, al

intentar salir-cerrar la Pantalla de Diseño del Robot, se presentará un mensaje de “Warning”

avisando del error.

Recordar, que es importante el conservar en el fichero simulink de control, el formato de

las variables de entrada y salida mostrado en los ficheros “modelo” defaultc.m y motors.m.

Manual de Usuario

26

7.4.2 Edición de un Fichero Simulink de Control ya ExistenteEn caso que el usuario desee modificar, o simplemente revisar, el fichero simulink de

control, se realizarán los siguientes pasos:


2) Seleccionar en la caja de diálogo, el nombre del fichero simulink:

3) Guardar el fichero, con el mismo nombre o diferente, en

caso de haber realizado modificaciones.

Después cerrar el fichero o minimizarlo.

4) Introducir el nombre del fichero editado , en caso que se desee

cambiar el fichero de control.

5) Para que el programa reconozca el nuevo fichero simulink de control, es necesario salir-

cerrar la pantalla de diseño a partir de la orden .

Manual de Usuario

27

7.4.3 Cambio del Fichero Simulink de ControlEl cambio del fichero simulink de control, puede realizarse de dos maneras posibles:

a) Editar el fichero que se desea introducir como nuevo fichero de control, aprovechando

que se introduce el nombre del fichero en de forma automática

cuando se edita.

b) Introducir directamente, con el teclado, el nombre del fichero

(sin extensión).

En los dos casos, hay que tener en cuenta el button tipo radio, donde se indica si el fichero

simulink contiene, o no, todo el modelo del sistema de control del Robot (calculo de las consignas,

control PID, motores, etc.).

7.5 Guardar en un Fichero el Diseño del RobotDespués de crear y diseñar un Robot, probablemente el usuario desee guardar la

configuración y el diseño de este Robot, para recuperarlo posteriormente. Cuando se guarda en un

archivo la configuración del Robot, se almacenan todas las características de éste; el tamaño, los

sensores y el nombre del fichero simulink de control.

En el menú Options, podemos encontrar tres órdenes, Save Config Robot, Load Config

Data y Quit. Para guardar el diseño realizado en un nuevo fichero o en

uno ya existente, utilizaremos la orden Save Config Robot. Esta orden se

divide en dos partes:

a) All Config, en el fichero se guarda todo el diseño realizado, incluyendo, el nombre del

fichero simulink.

b) Data Config, en el fichero sólo se guarda la información relativa a los sensores y al tamaño

del Robot sin incluir el fichero simulink de control.

Manual de Usuario

28

Para guardar la configuración, es necesario realizar los siguientes pasos:

1) Pulsar el button tipo label de la barra de menú , escogiendo la

opción de; Data Config y All Config deseada.

2) Aparecerá una caja de diálogo, donde se tiene que introducir el nombre del archivo

deseado con el teclado, o seleccionarlo de entre los

archivos existentes, utilizando las herramientas de la

caja de diálogo. Observar, que la extensión por

defecto en la caja de diálogo es *.cfg. Esta extensión,

se utiliza para diferenciar estos archivos de diseño del

Robot, de otros archivos del programa.

3) Una vez introducido el nombre del archivo, y seleccionado el directorio donde se desea

guardar, pulsaremos el botón . En el caso de introducir en la caja de diálogo, el nombre

de un archivo existente, cuando pulsemos , el programa, nos preguntará si queremos

reemplazar este archivo, y en caso afirmativo, se guardarán los datos

de la configuración del Robot. Pero, en caso negativo, será necesario

introducir un nuevo nombre para el archivo.

Finalizados los pasos anteriores, el programa guarda los datos de la configuración del

Robot en el archivo correspondiente, y presenta un mensaje, para avisar que ha acabado la

operación de guardar, donde se informa del nombre del archivo.

7.6 Recuperar de un Fichero el Diseño del RobotPara recuperar la configuración guardada en un archivo,

seleccionamos la orden Load Config del menú Options. La configuración

del Robot guardada en un archivo se puede recuperar de dos formas posibles:

a) All Config, recuperar los datos del archivo, incluyendo el nombre del fichero simulink

de control.

b) Data Config, recuperar solo los datos relativos al tamaño y los sensores.

Este sistema de recuperar el diseño del Robot nos permite realizar combinaciones, es decir,

recuperar de un archivo sólo los datos característicos de los sensores y el tamaño del Robot, sin

afectar al fichero simulink de control, configurado en ese momento.

Manual de Usuario

29

Si en el archivo de configuración, no se ha guardado el dato relativo al nombre del fichero

simulink de control, ya que se ha utilizado la orden para guardar Data Only, entonces, no afecta el

utilizar un sistema u otro para recuperar el archivo, ya que el resultado obtenido será el mismo en

los dos casos.

Para recuperar un archivo de configuración del Robot, se realizan los siguientes pasos:

1) Pulsar barra de menús, según el usuario desee recuperar toda la

configuración, o no, tal y como se ha explicado anteriormente.

2) Se visualizará la caja de diálogo, donde se

tiene que introducir el nombre del archivo del cual se

desea recuperar la configuración (archivos con

extensión *.cfg).

El archivo, puede ser recuperado desde el mismo directorio raíz del programa o en otro

directorio, utilizando las herramientas de la caja de diálogo.

3) Una vez introducido el nombre del archivo, pulsaremos el botón de la caja de

diálogo. En ese instante el programa leerá los datos del archivo, para después cerrar y volver a abrir

la pantalla de diseño, reinicializando el Robot con la nueva configuración que se acaba de recuperar

del archivo.

El programa al finalizar la operación de Load, presenta un mensaje, para avisar e informar

del nombre del archivo del cual se ha recuperado la configuración.

7.7 Salir-Cerrar la Pantalla de Diseño del RobotLa operación Salir de la pantalla de diseño del Robot, se puede realizar de dos maneras:

a) Utilizar la orden Quit del menú (“Design Robot” Figura 3).

b) Pulsar el botón cerrar de la figura (NO utilizar)

Las dos maneras son válidas y cierran la pantalla de diseño, pero si se utiliza la opción b),

se perderán parte de las modificaciones realizadas, además, de no liberar el espacio de memoria

utilizado durante el diseño del Robot. Por eso, es recomendable utilizar siempre para salir de la

pantalla la orden Quit del menú Options.

Manual de Usuario

30

8 Diseño del Entorno de ObstáculosLa creación de un entorno de obstáculos en la simulación, posibilita el estudio de la

respuesta del Robot ante diferentes situaciones. El entorno de obstáculos, va asociado directamente

a la programación de una Máquina de Estados (sensores), ya que éstos permiten comprobar su

correcto funcionamiento.

Pulsando el botón de la pantalla de simulación, figura “Physical

Robot Simulator”, se abre la pantalla de diseño de obstáculos, figura “Design Obstacles”.

8.1 Presentación de la Pantalla de Diseño de ObstáculosLos obstáculos, se crean en una pantalla preparada a tal efecto, donde se presentan

diferentes herramientas para el diseño. Esta pantalla consta de tres áreas: la ventana para el diseño

de obstáculos (que ocupa la mayor parte de la pantalla), la barra de menús y las diferentes

herramientas de trabajo para diseñar los obstáculos. Como se observa en la Figura 6, estas tres

áreas componen la pantalla de diseño de obstáculos, figura “Design Obstacles”.

Figura 6. Pantalla de diseño de los obstáculos en un monitor de 17 pulgadas

Barra de menúsBarra de título

Ventana de diseño de obstáculos

Herramientas de diseño de obstáculos

Manual de Usuario

31

8.1.1 Ventana de Diseño de ObstáculosEl espacio de trabajo, reservado para la creación y diseño de los obstáculos, es una copia de

la ventana de simulación para crear el entorno de obstáculos en un entorno igual al de simulación

del Robot.

La ventana, definida por unos ejes divididos en retículas de 10 unidades, sirve de guía y de

referencia al usuario para colocar los obstáculos en las coordenadas que el crea conveniente.

8.1.2 Barra de MenúsComo la pantalla de diseño de los Robots y la de simulación, en este caso, para el diseño de

los obstáculos también se dispone de la barra de menús característica del entorno de MATLAB

V4.2.1.c, pero añadiendo el menú Options (ver Figura 6).

8.1.3 Herramientas para el Diseño de ObstáculosEl área definida a la derecha de la ventana de diseño, presenta diferentes herramientas

destinadas a la creación, modificación, datos característicos, etc. de los obstáculos. Estas

herramientas y su utilización se explica en los siguientes apartados.

8.2 Crear un Nuevo ObstáculoUn obstáculo se compone de una serie de datos característicos que el usuario debe

introducir en el programa antes de crearlo. En los siguientes puntos se explica que pasos hay que

seguir para introducir estos datos y crear un nuevo obstáculo.

1) Introducir datos característicos del nuevo obstáculo .

Estas coordenadas se limitan a [xMIN, yMIN] = [0 + R,0 + R], [xMAX,

yMAX] = [130 + R,90 + R]. Siendo R el radio (tamaño establecido en la

Pantalla de diseño) del Robot. La orientación se limita [0º < ...< 360º] y el

tamaño al valor marcado en la configuración del programa (en este caso

< 5 cm.).

Seleccionar forma del obstáculo (cuadrada o circular)

2) Pulsar botón .

En caso de no producirse ningún error como: superar el número máximo de obstáculos

(valor de configuración) o introducir un dato erróneo, el obstáculo se crea en las coordenadas

introducidas siempre y cuando no se superen los márgenes de MAX/MIN establecidos.

Manual de Usuario

32

8.3 Seleccionar un ObstáculoAntes de realizar cualquier modificación de las características de un

obstáculo es necesario seleccionar el índice del obstáculo en el popup . Una vez

seleccionado un índice, se observará como el obstáculo cambia de color y sus

datos característicos se introducen de forma automática en las casillas de texto.

Además de indicarse su forma gracias a los butttons radio .

8.4 Posicionar un Obstáculo a Partir del MouseSi el usuario desea cambiar la posición actual del obstáculo a una nueva posición, de forma

rápida y sencilla, puede utilizar la herramienta de . El modo de realizar esta operación,

se explica en los siguientes puntos.

1) Seleccionar el índice del obstáculo .


3) Hacer click en la ventana de diseño en las coordenadas exactas donde se desee colocar el

obstáculo. Estas coordenadas se limitan a [xMIN, yMIN] = [0 + R,0 + R], [xMAX, yMAX] = [130 + R,90 + R].

Siendo R el radio (tamaño establecido en ) del obstáculo.

4) Con un segundo click, a una cierta distancia y con un ángulo respecto al primer click,

aporta el ángulo de giro al obstáculo (orientación a [0º < ...< 360º]).

Al finalizar el segundo click, el obstáculo se redibuja en la nueva

posición. Y en las casillas de texto, aparecen los nuevos de los datos

característicos del obstáculo

Manual de Usuario

33

Si las coordenadas introducidas con el primer click hacen superponer el obstáculo con otro

elemento de la simulación (que no sea otro obstáculo), se anula la operación de SetMouse,

presentándose el mensaje de “Warning” pertinente.

Figura 7. Ejemplo de mover un obstáculo con el mouse

NOTA: Si se desea un ángulo de 0º, simplemente se tienen que realizar los dos clicks en las mismas

coordenadas.

8.5 Actualizar Datos de un ObstáculoPara corregir o rectificar los datos característicos de un obstáculo, una vez ya ha sido

creado, se utiliza la orden . Esta orden permite cambiar las coordenadas (de forma

exacta) y el ángulo de orientación a través del teclado, además de la forma del obstáculo.


2) Al seleccionar el índice del obstáculo aparecen en las

casillas de texto sus actuales datos característicos. Ahora bien, el

usuario puede modificar cualquiera de estos datos.

También se puede cambiar la forma .

Manual de Usuario

34

Recordar la limitación de: [xMIN, yMIN] = [0 + R,0 + R], [xMAX, yMAX] = [130 + R,90 + R]. Siendo

R el radio (tamaño establecido en ) del obstáculo.


Figura 8. Ejemplo para actualizar los datos de un obstáculo

8.6 Borrar un ObstáculoSi el usuario lo desea, es posible borrar un obstáculo una vez creado, dejando su índice

libre, para ser ocupado por un nuevo obstáculo en caso necesario.



Observar como al finalizar esta operación, se borra el gráfico del

obstáculo, los datos característicos y el índice.

Figura 9. Ejemplo para borrar un obstáculo

Manual de Usuario

35

8.7 Guardar en un Fichero el Entorno de ObstáculosEl usuario puede guardar el entorno de obstáculos creado, para recuperarlo en otra sesión

de trabajo, o para realizar combinaciones con otros entornos. Además de permitir trabajar con el

mismo entorno en diferentes ocasiones y con diferentes configuraciones para el Robot.

El fichero de texto donde se salvan los obstáculos, puede ser editado y modificado por el

usuario desde el exterior de la simulación (extensión *.obs), por si es necesario realizar algún

cambio de los datos característicos.

1) Pulsar la orden de la barra de menú Options.

2) A partir de la caja de diálogo, se introduce

el nombre del fichero donde se desea grabar el

entorno de obstáculos.

Como se puede observar, en la misma caja de diálogo, se tiene por defecto la extensión

*.obs, para diferenciar estos ficheros del resto de los ficheros de la simulación.

3) Introducido el nombre del archivo y seleccionado el directorio donde se desea guardar el

fichero, pulsaremos . En el caso de que el fichero exista, será necesario confirmar si se

quiere reemplazar el nuevo fichero por el actual.

Si los pasos anteriores se han realizado correctamente, el programa almacena los datos

característicos de todos los obstáculos en el fichero de texto nombrado para este ejemplo:

obsta3.obs, y presenta el mensaje de aviso de finalización de la operación de Save.

Si el usuario desea cancelar la operación de Save, dispone del botón , que cierra

la caja de diálogo.

Manual de Usuario

36

8.8 Recuperar de un Fichero el Entorno de ObstáculosEl usuario que ha realizado un entorno de obstáculos y lo ha grabado en un fichero dispone

de la orden Load para recuperar ese entorno. Cuando se recuperan los obstáculos, leyendo los datos

característicos de un fichero de texto, se pueden dar dos posibilidades.

a) Que no existan obstáculos creados anteriormente a la operación de Load, con lo que los

obstáculos del fichero se recuperan sin ninguna traba, siempre y cuando no haya

ningún problema en el fichero.

b) Que existan obstáculos, con lo que el programa, preguntará si se desea reemplazar los

obstáculos existentes por los obstáculo recuperados del fichero, siempre y cuando

coincidan sus índices. O por el contrario, si no se desea reemplazar, se colocarán los

nuevos obstáculos recuperados del fichero, en las posiciones vacías de los índices,

hasta llegar al máximo permitido.

Esto permite realizar numerosas combinaciones de obstáculos, creando diversos y

diferentes entornos.

1) Pulsar la orden de la barra de menú Options.

2) En la caja de diálogo, se introduce el

nombre del fichero de extensión *.obs, que se desea

recuperar.

3) Pulsaremos el botón , de la caja de diálogo.

Recordar que si existen obstáculos, el programa nos preguntará si se desea reemplazar los

obstáculos existentes por los recuperados del fichero.

Yes → todos los obstáculos que tengan un índice que coincida conalguno de los índices recuperados del fichero serán reemplazados.

No → se ocupan solo los índices vacíos, con los nuevos obstáculosdel fichero.

Cancel → Abortar el Load de obstáculos.

Leídos los datos característicos de los obstáculos, el programa reinicializa la pantalla de

diseño, para presentar el nuevo entorno de obstáculos que se acaba de recuperar del fichero.

Manual de Usuario

37

Para indicar que se ha finalizado la operación con éxito, el programa presenta el mensaje de

aviso con el nombre del fichero.

En caso que a la hora de recuperar los obstáculos del fichero, se haya superado el número

máximo permitido en la configuración, se presentará un mensaje de aviso, durante unos segundos.

Ahora bien, en el supuesto que algún obstáculo se superponga a otro elemento de la simulación, el

programa cancelará automáticamente la orden de Load y presentará el mensaje de superposición

pertinente, avisando del error.

8.9 Salir-Cerrar la Pantalla de Diseño del Entorno de ObstáculosAcabadas las diferentes operaciones de trabajo con el entorno de obstáculos, es necesario

cerrar la pantalla de diseño de obstáculos (Figura 6) para continuar la sesión con el simulador.

c) Utilizar la orden Quit del menú (“Design Obstacles” Figura 6).

d) Pulsar el botón cerrar de la figura (NO utilizar)

Como ya se ha comentado en casos anteriores, los dos métodos son válidos, pero siempre

es recomendable utilizar el botón de Quit, ya que se libera memoria del programa.

Manual de Usuario

38

9 SimulaciónCuando el usuario haya creado y diseñado el/los Robot/s y definido el entorno de

obstáculos, puede dar inicio a la simulación en uno de estos dos modos:

Simulación paso a paso

Simulación continuada

En ambos casos, los resultados obtenidos serán los mismos.

9.1 Inicio de la SimulaciónAntes de dar inicio a la simulación, el usuario debe establecer unas condiciones iniciales

mínimas para el Robot, además de configurar las diferentes herramientas de funcionamiento del

programa (como seleccionar el movimiento, activar-desactivar , etc.).

En los siguientes puntos, se presentan los pasos recomendados antes de iniciar la

simulación.

1) Posicionar el Robot (coordenadas iniciales) con las herramientas

2) Seleccionar el tipo de movimiento e inicializar el estado de los

objetos asociados a ese movimiento, como es el caso de Grasping (objeto

triangular) ó de Other (coordenadas del setpoint).

3) Programación de la Máquina de Estado de los sensores (función seleccionada por el

usuario utilizando como plantilla la función maes(...)).

4) Diseñar el entorno de obstáculos.

5) Activar-desactivar el rastro del Robot .

6) Activar-desactivar el registrar los datos .

7) Borrar los resultados de anteriores simulaciones .

Realizados los pasos anteriores, ya se puede dar inicio a la simulación.

Manual de Usuario

39

9.2 Simulación Paso a PasoLa simulación paso a paso, consiste en la pulsación del button . En cada pulsación,

se ejecuta un paso de la simulación, es decir, se realiza una simulación de una duración

determinada por el grain time del programa, almacenando el último resultado de este intervalo de

simulación (condiciones iniciales en el siguiente intervalo).

Este tipo de simulación, nos sirve para observar paso a paso la evolución y los resultados

obtenidos entre cada intervalo de simulación, a la velocidad que nosotros pulsemos el botón

9.3 Simulación ContinuadaLa simulación continuada, activada con una sola pulsación del botón , consiste en

una ejecución continua de simulaciones, en intervalos de duración determinados en el valor del

grain time del programa.

La simulación continuada, es muy apropiada cuando se desea realizar una simulación de un

largo periodo de duración, para obtener unos resultados amplios para un estudio global del

funcionamiento del sistema de control del Robot (velocidades, intensidades, etc.).

9.4 StopLa simulación continuada se para pulsando el botón (en repetidas ocasiones). An-

tes de realizar la visualización de resultados o algún tipo de operación, es necesario parar la

simulación para evitar que el ordenador se sature.

Aunque se haya parado la simulación, siempre se puede volver a reanudar en modo

sin afectar a la respuesta obtenida.

Manual de Usuario

40

10 Modo de Presentación y Estudio de los ResultadosLa principal finalidad de este programa didáctico de simulación es el estudio de el sistema

de control de un Robot móvil. Para poder realizar el estudio, son necesarios la obtención de una

serie de datos, donde se refleje la respuesta del sistema, es decir, obtener los resultados de

velocidad, intensidad, par, etc. de los motores del Robot para estudiarlos y sacar conclusiones

referentes al sistema de control realizado.

Recordar, que para que los resultados de la simulación se fueran almacenando, era

necesario tener activado el . En caso contrario, si se desactiva , se habrían per-

dido los resultados de la simulación. Por eso, este button, se activa por defecto en el inicio de la

simulación.

10.1 Presentación Gráfica de los ResultadosSiempre y cuando existan datos almacenados y la simulación este parada , se pue-

de realizar la presentación gráfica de los resultados pulsando la orden . La respuesta pre-

sentada corresponderá al Robot cuyo índice haya sido seleccionado en el popup (en

este caso Robot nº4). Ver Figuras 10,11 y 12.

Figura 10. Ejemplo de resultados gráficos del simulador (velocidades lineal y angular)

Manual de Usuario

41

Figura 11. Ejemplo de resultado gráfico del simulador (coordenadas de posición del Robot)

Figura 12. Ejemplo de resultados gráficos del simulador (velocidades, intensidades y pares de los motores del Robot)

Manual de Usuario

42

10.2 Guardar Resultados de la SimulaciónComo ya se ha comentado anteriormente, los resultados de la simulación son muy

importantes, por eso, se ha creído conveniente facilitar al usuario una herramienta para poder

guardar los resultados de la simulación en un fichero. Este fichero puede ser recuperado en

posteriores sesiones de trabajo con el simulador, continuando la simulación y el estudio si se cree

conveniente, o ser editado para realizar un estudio dato a dato.

La orden para guardar los resultados, se encuentra en el menú Options de la pantalla de

simulación, figura “Physical Robot Simulator”.

1) Seleccionar el índice del Robot que desean guardar los resultados .

2) Pulsar la orden .

3) En la caja de diálogo presentada, se debe

introducir el nombre del archivo donde se desea

guardar los datos de la simulación (extensión por

defecto *.dat).

4) Pulsar una vez introducido el nombre del archivo. En el caso que el archivo

ya exista anteriormente, el programa nos preguntará si deseamos reemplazarlo, o no. Si se pulsa

reemplazar, entonces el archivo será reemplazado con los nuevos datos. Al pulsar

reemplazar, el programa volverá a pedir un nuevo nombre para el archivo.

Al presentar el mensaje de aviso con el nombre del fichero donde se han guardado los

datos, se da por finalizada la operación de guardar. Ahora existe un nuevo archivo de texto *.dat

con los resultados de la simulación.

NOTA: Si se intenta realizar esta operación de guardar datos, sin que existan previamente, el

programa presentará un mensaje de error.

Manual de Usuario

43

10.3 Recuperar Resultados de la SimulaciónSi el usuario, anteriormente ha guardado los resultados de una simulación y ahora desea

recuperarlos, debe tener en cuenta dos puntos:

a) Que exista previamente un Robot creado, al cual asignar estos datos recuperados

(seleccionar en el popup el Robot).

b) Una vez recuperados los datos, la simulación puede ser reanudada donde se dejo.

Observar que el Robot se posiciona en las últimas coordenadas guardadas en el fichero,

para reiniciar la simulación.

Establecidas estas premisas, se pueden recuperar los datos de la simulación siguiendo estos

pasos.

1) Seleccionar índice del Robot, al cual se desean asignar los datos

recuperados del fichero.

2) Pulsar la orden del menú Options.

3) En la caja de diálogo presentada, se tiene

que introducir el nombre del archivo (*.dat). Recordar

que sólo es posible recuperar ficheros de texto con un

determinado formato en su interior.

4) Pulsar , de la caja de diálogo una vez introducido el nombre del archivo. Si no

ha existido ningún problema en la lectura del fichero, el programa recupera los datos para que

puedan ser tratados gráficamente (obtener gráficas) o para continuar la simulación.

NOTA: El programa cuando recupera los datos de una anterior simulación, reemplaza los datos

que pudieran existir anteriormente.

Manual de Usuario

44

11 Fin de la Simulación

11.1 Salir del SimuladorAcabada la sesión de trabajo con el simulador, el usuario puede salir del programa y volver

al entorno de MATLAB V4.2.1.c utilizando dos maneras posibles:

e) Utilizar la orden (“Physical Robot Simulator” Figura 1).

f) Pulsar el botón cerrar de la figura “Physical Robot Simulator” (NO utilizar).

El segundo método expuesto, no es ni mucho menos válido, ya que no libera la memoria

del programa MATLAB V4.2.1.c, además de permitir que sigan abiertas otras figuras. El primer

método es el válido y aconsejable ya que libera totalmente la memoria de variables y datos, además

de cerrar todas las figuras del simulador.

11.2 Reset SimuladorCuando el usuario lo crea conveniente (para iniciar una nueva sesión de trabajo con el

simulador), puede reinicializar (liberar totalmente la memoria de MATLAB y cerrar todas las

figuras abiertas) y volver a empezar la sesión de trabajo con el simulador, pulsando la orden

mostrada en la parte superior derecha de la pantalla de simulación, figura “Physical Robot

Simulator” (Figura 1).

proyecto final de carrera simulador en matlab de robots...

Documents