Download Report

Construcción de plataforma software
para el diseño y la simulación de
robots móviles
Carlos Sosa Marrero
Tutor: Santiago Torres Álvarez
Cotutor: Antonio L. Morell González
Julio de 2016
A Elena, Miguel y Alejandro,
porque su cariño será siempre
lo mejor de estos cuatro años.
ÍNDICE
Carlos Sosa Marrero
Índice
1. Introducción
1.1. Objetivos . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.2. Antecedentes . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.3. Herramientas software utilizadas . . . . . . . . . . . . . . . . .
7
7
7
8
2. Descripción general
10
2.1. Arquitectura de la plataforma . . . . . . . . . . . . . . . . . . 10
2.2. Comunicación entre los módulos . . . . . . . . . . . . . . . . . 10
3. Descripción de la interfaz
13
4. Descripción de los robots móviles
4.1. Diferencial . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.1.1. Restricciones geométricas para huella circular . . .
4.1.2. Restricciones geométricas para huella cuadrangular
4.2. Triciclo . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.2.1. Restricciones geométricas para huella circular . . .
4.2.2. Restricciones geométricas para huella cuadrangular
4.3. Cuatriciclo . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.3.1. Restricciones geométricas para huella circular . . .
4.3.2. Restricciones geométricas para huella cuadrangular
4.4. Sensores . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.5. Propiedades dinámicas . . . . . . . . . . . . . . . . . . . .
4.6. Adecuación del modelo al software de simulación . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
24
24
25
26
27
29
31
33
35
38
40
41
41
.
.
.
.
.
.
44
44
45
50
54
58
59
5. Descripción de los experimentos
5.1. Cámaras . . . . . . . . . . . . .
5.2. Obstáculos . . . . . . . . . . . .
5.3. Circuito . . . . . . . . . . . . .
5.4. Balizas . . . . . . . . . . . . . .
5.5. Escena vacı́a . . . . . . . . . . .
5.6. Escena creada por el usuario . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
6. Conclusiones y lı́neas abiertas
60
6.1. Conclusiones . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
6.2. Lı́neas abiertas . . . . . . . . . . . . . . . . . . . . . . . . . . 60
Construcción de plataforma software para el diseño y la simulación...
1
ÍNDICE
A. Código de Matlab
A.1. Start.m . . . . . . . . .
A.2. SelectScene.m . . . . . .
A.3. PreView.m . . . . . . . .
A.4. SelectRobot.m . . . . . .
A.5. CreaDiffCir.m . . . . . .
A.6. CreaDiffQuad.m . . . . .
A.7. CreaTricycleCir.m . . . .
A.8. CreaTricycleQuad.m . .
A.9. CreaQuadricycleCir.m .
A.10.CreaQuadricycleQuad.m
A.11.SelectItem.m . . . . . .
A.12.PosMarker.m . . . . . .
A.13.CreaObs.m . . . . . . .
A.14.PosObs.m . . . . . . . .
A.15.Simulation.m . . . . . .
Carlos Sosa Marrero
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
63
63
66
71
74
79
86
93
100
108
116
124
127
133
137
142
B. Código Lua
167
B.1. create . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 167
Construcción de plataforma software para el diseño y la simulación...
2
ÍNDICE DE FIGURAS
Carlos Sosa Marrero
Índice de figuras
1.
2.
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.
16.
17.
18.
19.
20.
21.
22.
23.
24.
25.
26.
27.
28.
29.
30.
31.
32.
33.
34.
35.
Aplicación para la simulación de un robot Roomba . . . . . . .
Modo sı́ncrono . . . . . . . . . . . . . . . . . . . . . . . . . .
Estructura de la interfaz . . . . . . . . . . . . . . . . . . . . .
Ventana de la interfaz desarrollada sobre ventana de V-REP .
Ventana de inicio . . . . . . . . . . . . . . . . . . . . . . . . .
Selector de escenas . . . . . . . . . . . . . . . . . . . . . . . .
Vista previa de cada una de las escenas . . . . . . . . . . . . .
Selector de huella y modelo de robot . . . . . . . . . . . . . .
Generador de robot diferencial con huella circular . . . . . . .
Generador de robot diferencial con huella cuadrangular . . . .
Generador de robot de tipo triciclo con huella circular . . . . .
Generador de robot de tipo triciclo con huella cuadrangular . .
Generador de robot de tipo cuatriciclo con huella circular . . .
Generador de robot de tipo cuatriciclo con huella cuadrangular
Selector de ı́tem . . . . . . . . . . . . . . . . . . . . . . . . . .
Posicionador de balizas . . . . . . . . . . . . . . . . . . . . . .
Generador de obstáculos . . . . . . . . . . . . . . . . . . . . .
Posicionador de obstáculos . . . . . . . . . . . . . . . . . . . .
Ventana de simulación . . . . . . . . . . . . . . . . . . . . . .
Robot diferencial . . . . . . . . . . . . . . . . . . . . . . . . .
Robot diferencial en la escena de V-REP . . . . . . . . . . . .
Robot diferencial con huella circular . . . . . . . . . . . . . . .
Robot diferencial con huella cuadrangular . . . . . . . . . . .
Robot de tipo bicicleta . . . . . . . . . . . . . . . . . . . . . .
Robot de tipo triciclo . . . . . . . . . . . . . . . . . . . . . . .
Robot de tipo triciclo en la escena de V-REP . . . . . . . . .
Robot de tipo triciclo con huella circular . . . . . . . . . . . .
Rueda delantera con ángulo θdmax . . . . . . . . . . . . . . . .
Robot de tipo triciclo con huella cuadrangular . . . . . . . . .
Rueda delantera con ángulo θxmax . . . . . . . . . . . . . . . .
Rueda delantera con ángulo θymax . . . . . . . . . . . . . . . .
Robot de tipo cuatriciclo . . . . . . . . . . . . . . . . . . . . .
Robot de tipo cuatriciclo en la escena de V-REP . . . . . . . .
Robot de tipo cuatriciclo con huella circular . . . . . . . . . .
Rueda delantera derecha con ángulo θddmax . . . . . . . . . . .
Construcción de plataforma software para el diseño y la simulación...
8
11
13
14
15
15
16
16
17
18
18
19
19
20
20
21
21
22
23
24
25
26
26
27
28
28
29
30
31
32
33
33
34
35
36
3
ÍNDICE DE FIGURAS
36.
37.
38.
39.
40.
41.
42.
43.
44.
45.
46.
47.
48.
49.
50.
51.
52.
Carlos Sosa Marrero
Rueda delantera derecha con ángulo θdymin . . . . . . . .
Robot de tipo cuatriciclo con huella cuadrangular . . . .
Rueda delantera derecha con ángulo θdxmax . . . . . . . .
Rueda delantera derecha con ángulo θdymax . . . . . . . .
Sensor de proximidad . . . . . . . . . . . . . . . . . . . .
Sensor de visión . . . . . . . . . . . . . . . . . . . . . . .
Robot con formas puras . . . . . . . . . . . . . . . . . .
Jerarquı́a de un robot con rueda directriz . . . . . . . . .
Página 1+5 . . . . . . . . . . . . . . . . . . . . . . . . .
Página de vistas . . . . . . . . . . . . . . . . . . . . . . .
Escena con obstáculos . . . . . . . . . . . . . . . . . . .
Simulación de robot diferencial en escena con obstáculos
Escena con circuito . . . . . . . . . . . . . . . . . . . . .
Simulación de robot diferencial en escena con circuito . .
Escena con balizas . . . . . . . . . . . . . . . . . . . . .
Simulación de robot diferencial en escena con balizas . .
Escena vacı́a . . . . . . . . . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
Construcción de plataforma software para el diseño y la simulación...
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
37
38
39
39
40
41
42
42
44
45
46
48
50
52
55
56
58
4
ÍNDICE DE CUADROS
Carlos Sosa Marrero
Índice de cuadros
1.
2.
3.
4.
5.
6.
7.
8.
9.
Robot diferencial con huella circular verificado en escena con
obstáculos . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Robot de tipo triciclo con huella cuadrangular verificado en
escena con obstáculos . . . . . . . . . . . . . . . . . . . . . .
Robot de tipo cuatriciclo con huella circular verificado en escena con obstáculos . . . . . . . . . . . . . . . . . . . . . . .
Robot diferencial con huella cuadrangular verificado en escena
con circuito . . . . . . . . . . . . . . . . . . . . . . . . . . .
Robot de tipo triciclo con huella circular verificado en escena
con circuito . . . . . . . . . . . . . . . . . . . . . . . . . . .
Robot de tipo cuatriciclo con huella cuadrangular verificado
en escena con circuito . . . . . . . . . . . . . . . . . . . . . .
Robot diferencial con huella circular verificado en escena con
balizas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Robot de tipo triciclo con huella cuadrangular verificado en
escena con balizas . . . . . . . . . . . . . . . . . . . . . . . .
Robot de tipo cuatriciclo con huella circular verificado en escena con balizas . . . . . . . . . . . . . . . . . . . . . . . . .
Construcción de plataforma software para el diseño y la simulación...
. 47
. 49
. 50
. 52
. 53
. 54
. 56
. 57
. 58
5
Resumen
Esta memoria describe las caracterı́cticas y posibilidades de Koala, una
plataforma software para el diseño y testeo de robots móviles. Se detalla,
asimismo, el trabajo realizado para su creación.
Koala está compuesta por dos módulos. El primero consiste en una aplicación desarrollada en el entorno de interfaces de usuario GUIDE de Matlab.
Permite al usuario realizar de forma sencilla las labores de diseño de robots
tipo diferencial, triciclo y cuatriciclo con huellas circulares o cuadrangulares.
Este módulo se encarga, además, del control del robot en un determinado
escenario.
El segundo módulo hace uso de la plataforma para la experimentación con
robots V-REP. Es responsable de crear el robot a partir de la información
recabada por el primer módulo y mostrar en pantalla su comportamiento en
una serie de escenarios.
Abstract
This report describes the features and possibilities of Koala, a software
platform for the design and testing of mobile robots. It also explains the work
carried out for its creation.
Koala is composed of two modules. The first one consists in an application
developed in the Matlab user inferfaces environment GUIDE. It allows the
user to perform in a simple way the design tasks of differential, tricycle and
quadricycle robots with circular and quadrangular footprints. This module
is also in charge of controlling the robot on a particular scene.
The second module uses the robots experimentation platform V-REP. It
is responsible for creating the robot based on the information gathered by
the first module and dispaying its behaviour in a series of scenes.
1 INTRODUCCIÓN
1.
1.1.
Carlos Sosa Marrero
Introducción
Objetivos
El objetivo del presente Trabajo de Fin de Grado es la construcción de
Koala, una plataforma software para el diseño y la simulación de robots
móviles basada en el entorno de desarrollo de interfaces de usuario GUIDE
de Matlab que facilite el diseño de un robot móvil a partir de parámetros introducidos por el usuario (configuración de las ruedas, forma y dimensiones).
Asimismo, se persigue ofrecer al usuario una herramienta para el testeo
y visualización en pantalla del comportamiento del robot en determinados
entornos predefinidos o creados previamente por el usuario con el software
de simulación V-REP.
De esta forma, se pretende conseguir un producto con un gran rango de
aplicación en docencia e investigación que simplifique y agilice las labores de
diseño y testeo de robots móviles y la programación necesaria para el control
integral del mismo.
1.2.
Antecedentes
Koala surge como evolución de una aplicación desarrollada en el marco de
la asignatura Sistemas Robotizados para la simulación del comportamiento
de un robot diferencial Roomba en Matlab. Esta aplicación solo permitı́a al
usuario escoger el radio del cuerpo del robot.
Además, como se muestra en la figura 1, la visualización del comportamiento del robot se realizaba a través de un gráfico en dos dimensones, una
opción limitada y que, además, hacı́a necesaria la existencia de funciones destinadas a graficar el robot en cada instante de tiempo y que fuera el propio
Matlab el encargado de obtener la posición y orientación del robot en todo
momento mediante una aproximación discreta.
Asimismo, la aplicación carecı́a de una interfaz gráfica de usuario, por lo
que el radio del robot y el comportamiento deseado del mismo eran introducidos por lı́nea de comandos.
Construcción de plataforma software para el diseño y la simulación...
7
1 INTRODUCCIÓN
Carlos Sosa Marrero
Figura 1: Aplicación para la simulación de un robot Roomba
1.3.
Herramientas software utilizadas
Matlab en su versión R2010a. Se trata de un software matemático con
múltiples aplicaciones en los campos del control, la robótica, la monitorización de la salud, el procesamiento de imágenes o las comunicaciones. Entre
sus múltiples prestaciones destacan la manipulación y el tratamiento de datos y funciones y la implementación de algoritmos. Asimismo, incluye un
editor de interfaces de usuario GUIDE que se utilizará para la creación de la
interfaz de la plataforma desarrollada en el presente Trabajo.
El código desarrollado en Matlab es integrable con otros lenguajes. Esto
facilitarı́a la exportación de la solución de control conseguida hacia algún
tipo de sistema embebido (Arduino, Raspberry PI, etc.) si se optase por ello
en una futura lı́nea de trabajo.
V-REP (Virtual Robot Experimental Platform) en su versión educativa
3.3.1, la útlima lanzada hasta la fecha. Se trata de una plataforma software
desarrollada por Coppelia Robotics para la experimentación con robots. Permite crear, editar, simular y evaluar cualquier sistema robótico creado por
el usuario o escogido de su librerı́a de modelos. Está basado en una arquitectura de control distribuido, lo que implica que los programas de control
son enlazados directamente a los objetos de la escena. Asimismo, puede usarConstrucción de plataforma software para el diseño y la simulación...
8
1 INTRODUCCIÓN
Carlos Sosa Marrero
se como un programa independiente o integrarse de forma sencilla con otra
aplicación, como es el caso de la plataforma objeto de este Trabajo.
Construcción de plataforma software para el diseño y la simulación...
9
2 DESCRIPCIÓN GENERAL
2.
2.1.
Carlos Sosa Marrero
Descripción general de la plataforma
Arquitectura de la plataforma
La plataforma software desarrollada se compone de dos grandes módulos
que se detallan a continuación.
Módulo de la aplicación de Matlab La aplicación desarrollada en Matlab
se encarga, a través de la GUI, de recoger las elecciones por parte del usuario
en lo que respecta al escenario en el que ha de tener lugar la simulación,
ası́ como en lo referente a las caracterı́sticas del robot cuyo comportamiento
se desea testear. Idealmente, este módulo se encargarı́a también de la generación del robot y posibles objetos adicionales a partir de la información
recabada. Sin embargo, la API remota de V-REP para Matlab aún carece de
las funciones especı́ficas para la creación de cuerpos geométricos y sensores.
Por este motivo, se hace uso de la función genérica simxCallScriptFunction
que permite llamar a una determinada función de un script asociado a un
objeto de V-REP.
Asimismo, este módulo se encarga del control del robot simulado. Gracias
a las funciones de la API remota, que a este respecto sı́ se encuentran ya
implementadas, es posible obtener la información de los sensores del robot y
aplicar la conveniente consigna a los motores.
Módulo de V-REP El módulo de V-REP es el encargado de llevar a
cabo y mostrar en pantalla la simulación del robot diseñado por el usuario
en el entorno que ha estimado conveniente. Además, como se ha explicado
previamente, la creación de objetos solo puede aún llevarse a cabo a través
de la interfaz del propio V-REP o, de forma programática, a través de la API
regular. Por ello, es preciso que cada escena de V-REP en la que se desee
llevar a cabo una simulación cuente con un script en Lua que contenga las
funciones necesarias para la creación de las diferentes partes del robot y a
las que se llamará a través de simxCallScriptFunction desde la aplicación de
Matlab.
2.2.
Comunicación entre los módulos
La comunicación entre ambos módulos se establece, como se ha mencionado, gracias a la API remota con la que cuenta V-REP. A través de un
Construcción de plataforma software para el diseño y la simulación...
10
2 DESCRIPCIÓN GENERAL
Carlos Sosa Marrero
socket, se produce la interacción entre las dos aplicaciones, siendo V-REP el
servidor y la aplicación desarrollada en Matlab, el cliente. La API remota se
activa por parte de V-REP gracias al plugin v repExtRemoteApi.dll, cargado
por defecto. Para habilitar la API remota en la aplicación de Matlab, se hace
uso de la función vrep.simxStart, tras haber cargado la librerı́a remoteApi.dll.
Para más información a este respecto, se recomienda consultar el manual de
usuario de V-REP [1].
La comunicación establecida puede ser sı́ncrona o ası́ncrona. En este caso se ha optado por el primer tipo, que se activa del lado del cliente con
la función simxSynchronous() y permite que cada paso de la simulación se
ejecute cuando reciba un trigger desde la API remota gracias a la función
simxSynchronousTrigger(). En la figura 2 se ilustra este procedimiento.
Figura 2: Modo sı́ncrono
Además, la mayorı́a de las funciones de la API remota exigen como argumentos el identificador clientID proporcionado por simxStart y un modo de
operación (simx opmode oneshot, simx opmode blocking, simx opmode streaming,
Construcción de plataforma software para el diseño y la simulación...
11
2 DESCRIPCIÓN GENERAL
Carlos Sosa Marrero
simx opmode buffer, etc.) que define qué ocurre con la petición y la respuesta
asociadas a cada llamada. A este respecto, se ha optado en todos los casos
por el modo de operación recomendado por el manual de usuario de V-REP
[1] para cada función.
Construcción de plataforma software para el diseño y la simulación...
12
3 DESCRIPCIÓN DE LA INTERFAZ
3.
Carlos Sosa Marrero
Descripción de la interfaz
De la misma forma que el marsupio del koala permite a las crı́as dar sus
primeros pasos en un entorno que de otro modo podrı́a resultar hostil, Koala
ofrece al usuario la posibilidad de iniciarse en el diseño y la simulación del
comportamiento de robots móviles sin verse abrumado por las posibilidades
que a este respecto ofrecen V-REP y otras plataformas similares. La interfaz
desarrollada en el entorno GUIDE de Matlab permite ejecutar todas las fases de diseño y simulación del comportamiento de robots móviles de forma
sencilla. Su estructura se esquematiza en la figura 3.
Inicio
Selector de escena
Vista previa
Selector de huella y modelo
Generador de robot
(según huella y modelo)
Posicionador de balizas
Generador de obstáculos
Posicionador de obstáculos
Simulación
Figura 3: Estructura de la interfaz
Todas las ventanas de la interfaz se posicionan en la esquina superior
derecha de la pantalla, de forma que interfieran los menos posible con la
ventana de V-REP en la que el usuario observa los resultados del proceso de
Construcción de plataforma software para el diseño y la simulación...
13
3 DESCRIPCIÓN DE LA INTERFAZ
Carlos Sosa Marrero
creación del robot y la simulación, como se aprecia en la figura 4.
Figura 4: Ventana de la interfaz desarrollada sobre ventana de V-REP
El proceso de creación del robot y generación de posibles obstáculos o balizas puede ser cancelado por el usuario en cualquier momento bien utilizando
los botones Cancelar dispuestos para tal fin o bien cerrando la ventana, en
cuyo caso se solicitará confirmación.
A continuación, se describe cada una de las ventanas que componen la
interfaz. Una primera ventana, que se muestra en la figura 5, da la bienvenida
al usuario y le permite entablar la conexión con V-REP, que ha de haber sido
iniciado previamente.
Seguidamente, en la ventana que se aprecia en la figura 6, se ofrece al
usuario la posibilidad de elegir la escena en la que desea llevar a cabo la simulación, pudiendo escoger entre cuatro entornos predefinidos o cargar una
escena propia creada previamente con V-REP. En este úlimo caso, recomendado para los usuarios más avanzados, ha de verificarse que la escena contenga un dummy con el script en Lua create asociado, como ya incluyen las
escenas predefinidas, a fin de poder llevar a cabo la creación del robot y
posibles obstáculos y/o balizas adicionales de forma satisfactoria.
Construcción de plataforma software para el diseño y la simulación...
14
3 DESCRIPCIÓN DE LA INTERFAZ
Carlos Sosa Marrero
Figura 5: Ventana de inicio
Figura 6: Selector de escenas
Asimismo, a fin de facilitar la elección, se ofrece al usuario una vista
previa y descripción del comportamiento que se espera del robot en cada una
de las escenas en la ventanta que se muestra en la figura 7
Construcción de plataforma software para el diseño y la simulación...
15
3 DESCRIPCIÓN DE LA INTERFAZ
Carlos Sosa Marrero
Figura 7: Vista previa de cada una de las escenas
A continuación, el usuario ha de seleccionar en la ventana que se aprecia
en la figura 8 el tipo de huella (circular o cuadrangular) y el modelo de robot
móvil (diferencial, triciclo o cuatriciclo) que desea crear y simular.
Figura 8: Selector de huella y modelo de robot
Construcción de plataforma software para el diseño y la simulación...
16
3 DESCRIPCIÓN DE LA INTERFAZ
Carlos Sosa Marrero
En función de la elección del usuario, el proceso de creación del robot continuará con una de las seis ventanas asociadas a cada combinación de huella
y modelo. Para cualquier caso, se comprobará que los valores introducidos
por el usuario sean numéricos y positivos. De forma adicional, se asegurará
que las dimensiones especificadas cumplan con las restricciones geométricas
de cada modelo y tipo de huella. En caso de que esto no sea ası́, un mensaje de error avisará al usuario, indicándole qué restricción no se cumple y le
sugerirá cambiar los valores de las dimensiones implicadas. Asimismo, todas
las ventanas ofrecen la posibilidad, en caso de equivocación por parte del
usuario, de regresar a la ventana de selección de huella y modelo y escoger
unas nuevas caracterı́sticas.
Robot diferencial con huella circular Se solicita al usuario, tal y como
se muestra en la figura 9, el radio y la altura del cuerpo del robot, la distancia
entre ejes y el radio y el ancho de las ruedas.
Figura 9: Generador de robot diferencial con huella circular
Robot diferencial con huella cuadrangular Se solicita al usuario, tal y
como se aprecia en la ventana de la figura 10, el largo, el ancho y la altura del
cuerpo del robot, la distancia entre ejes y el radio y el ancho de las ruedas.
Construcción de plataforma software para el diseño y la simulación...
17
3 DESCRIPCIÓN DE LA INTERFAZ
Carlos Sosa Marrero
Figura 10: Generador de robot diferencial con huella cuadrangular
Robot de tipo triciclo con huella circular Se solicita al usuario, tal y
como se aprecia en la ventana de la figura 11, el radio y la altura del cuerpo
del robot, las distancias entre los ejes izquierdo-derecho y delantero-trasero
y el radio y el ancho de las ruedas.
Figura 11: Generador de robot de tipo triciclo con huella circular
Robot de tipo triciclo con huella cuadrangular Se solicita al usuario,
tal y como se muestra en la ventana de la figura 12, el largo, el ancho y la
altura del cuerpo del robot, las distancias entre los ejes izquierdo-derecho y
delantero-trasero y el radio y el ancho de las ruedas.
Construcción de plataforma software para el diseño y la simulación...
18
3 DESCRIPCIÓN DE LA INTERFAZ
Carlos Sosa Marrero
Figura 12: Generador de robot de tipo triciclo con huella cuadrangular
Robot de tipo cuatriciclo con huella circular Se solicita al usuario, tal
y como se aprecia en la ventana de la figura 13, el radio y la altura del cuerpo
del robot, las distancias entre los ejes izquierdo-derecho y delantero-trasero
y el radio y el ancho de las ruedas.
Figura 13: Generador de robot de tipo cuatriciclo con huella circular
Robot de tipo cuatriciclo con huella cuadrangular Se solicita al usuario, tal y como se aprecia en la ventana de la figura 14, el radio y la altura del
cuerpo del robot, las distancias entre los ejes izquierdo-derecho y delanterotrasero y el radio y el ancho de las ruedas.
Construcción de plataforma software para el diseño y la simulación...
19
3 DESCRIPCIÓN DE LA INTERFAZ
Carlos Sosa Marrero
Figura 14: Generador de robot de tipo cuatriciclo con huella cuadrangular
A continuación, se ofrece al usuario la posibilidad de añadir balizas u
obstáculos a la escena, como se muestra en la figura 15, (en el caso de haber
elegido probar el robot en las escenas Balizas, Escena Vacı́a o haber cargado
una escena creada previamente) o solo obstáculos (para cualquiera de los
casos restantes).
Figura 15: Selector de ı́tem
Si se elige añadir una baliza, su forma cilindrı́ca y sus dimensiones (radio
de la base de 0,125 m y 0,5 m de altura) y, especialmente, su color verde
(R=0, G=1, B=0), fundamental para su detección por parte del robot, se
consideran fijos.
Por defecto, cada nueva baliza se crea a ras de suelo justo a 1 m enfrente
del robot. No obstante, como se muestra en la figura 16, el usuario puede
modificar a su conveniencia la posición de la baliza, bien introduciendo las
coordenadas cartesianas deseadas o bien, si lo encuentra más intuitivo, arrastrando directamente la baliza hasta la posición que estima conveniente en la
ventana de V-REP. En este último caso, haciendo clic en el botón Actualizar
coordenadas se muestran en la ventana las nuevas coordenadas cartesianas
Construcción de plataforma software para el diseño y la simulación...
20
3 DESCRIPCIÓN DE LA INTERFAZ
Carlos Sosa Marrero
de la baliza. El usuario puede añadir tantas balizas como desee haciendo clic
en Crear una nueva baliza.
Figura 16: Posicionador de balizas
Si se elige añadir un obstáculo, se pregunta en primer lugar al usuario,
como se aprecia en la ventana de la figura 17, por su forma (prismática,
cilı́ndrica o cónica) y sus dimensiones (lado o radio de la base, según el caso
y altura).
Figura 17: Generador de obstáculos
Construcción de plataforma software para el diseño y la simulación...
21
3 DESCRIPCIÓN DE LA INTERFAZ
Carlos Sosa Marrero
Al igual que en el caso de las balizas, cada nuevo obstáculo se crea por
defecto a ras de suelo justo a 1 m enfrente del robot. No obstante, el usuario
puede, nuevamente, introducir las coordenadas cartesianas que estime convenientes para el obstáculo o arrastrarlo en la ventana de V-REP hasta la
posición deseada (como se muestra en la figura 18). De nuevo, haciendo clic
en en el botón Actualizar coordenadas se muestran en la ventana las actuales
coordenadas cartesianas del obstáculo. El usuario puede repetir la secuencia
de generación y posicionamiento de obstáculos tantas veces como desee.
Figura 18: Posicionador de obstáculos
Tras haber añadido las balizas y/o los obstáculos que el usuario haya
estimado conveniente, una útlima ventana, que se muestra en la figura 19
permite iniciar, pausar, parar y retomar la simulación, ası́ como seleccionar y
cambiar tantas veces como se desee la velocidad lineal del robot dentro de un
intervalo de 0 a 1 m/s que se ha estimado razonable para las dimensiones de
los recintos en los que tiene lugar el testeo. Al parar la simulación, se ofrece
la posibilidad de probar un nuevo robot en una nueva escena, para lo cual
el usuario es remitido a la ventana Selector de escenas, o finalizar Koala de
forma definitiva.
Construcción de plataforma software para el diseño y la simulación...
22
3 DESCRIPCIÓN DE LA INTERFAZ
Carlos Sosa Marrero
Figura 19: Ventana de simulación
Construcción de plataforma software para el diseño y la simulación...
23
4 DESCRIPCIÓN DE LOS ROBOTS MÓVILES
4.
Carlos Sosa Marrero
Descripción de los robots móviles
En este capı́tulo se describen los modelos de robots móviles que permite
crear Koala.
4.1.
Diferencial
Un robot de tipo diferencial cuenta, como se aprecia en la figura 20,
con dos ruedas motrices situadas a ambos lados de su cuerpo, ası́ como dos
esferas deslizantes ubicadas en las partes frontal y trasera que sirven de punto
de apoyo y cuyas dimensiones no influyen, idealmente, en la cinemática del
robot.
Figura 20: Robot diferencial
El avance, retroceso o giro del robot se produce asignando una determinada velocidad angular a cada uno de los motores de las ruedas. La velocidad
lineal del robot viene dada por (1).
vi + vd
(1)
2
donde vi y vd son, respectivamente, las velocidades lineales de las ruedas
izquierda y derecha calculadas según (2) y (3) a partir de las velocidades
angulares de las ruedas ωi y ωd y el radio de las mismas r.
v=
vi = ω i ∗ r
Construcción de plataforma software para el diseño y la simulación...
(2)
24
4 DESCRIPCIÓN DE LOS ROBOTS MÓVILES
Carlos Sosa Marrero
vd = ωd ∗ r
(3)
La velocidad angular del robot viene dada por (4)
vi − vd
(4)
d
donde d es la distancia entre los ejes.
El estado del robot estará caracterizado por su posición x e y y su orientación α obtenidas a partir de (6), (7) y (5), respectivamente.
ω=
α̇(t) = ω(t)
(5)
ẋ(t) = v(t) ∗ cos [α(t)]
(6)
ẏ(t) = v(t) ∗ sen [α(t)]
(7)
En la figura 21, se muestra un robot diferencial con huella circular creado
en la escena de simulación de V-REP.
Figura 21: Robot diferencial en la escena de V-REP
4.1.1.
Restricciones geométricas para huella circular
Un robot diferencial con huella circular presenta el diseño que se muestra
en la figura 22.
Construcción de plataforma software para el diseño y la simulación...
25
4 DESCRIPCIÓN DE LOS ROBOTS MÓVILES
Carlos Sosa Marrero
R
a
r
d
Figura 22: Robot diferencial con huella circular
Las ruedas no han de sobresalir del cuerpo del robot ni tampoco superponerse. Esto se verifica si se cumplen, respectivamente, (8) y (9).
s
2
d+a
+ r2 ≤ R
(8)
2
a<d
(9)
Además, se tiene que en ningún caso la distancia entre ejes puede ser
mayor que el diámetro del robot.
4.1.2.
Restricciones geométricas para huella cuadrangular
Un robot diferencial con huella cuadrangular presenta el diseño que se
muestra en la figura 23
b
a
r
l
d
Figura 23: Robot diferencial con huella cuadrangular
Construcción de plataforma software para el diseño y la simulación...
26
4 DESCRIPCIÓN DE LOS ROBOTS MÓVILES
Carlos Sosa Marrero
Las ruedas no han de sobresalir del cuerpo del robot ni por la partes
frontal y trasera ni por los laterales, lo cual se verifica si se cumplen, respectivamente, (10) y (11). Además, se ha de garantizar que las ruedas no se
superpongan, para lo cual sigue siendo válida (9).
r≤
l
2
d+a≤b
4.2.
(10)
(11)
Triciclo
El modelo del triciclo surge como una evolución en busca de una mayor
estabilidad del modelo de la bicicleta. Un robot de este último tipo, descartado en el desarrollo de Koala por considerarse carente de interés una vez
implementado el modelo del triciclo, posee una rueda motriz y una directriz,
como se aprecia en la figura 24.
θ
Figura 24: Robot de tipo bicicleta
El modelo del triciclo, que se muestra en la figura 25, añade una segunda
rueda motriz, para obtener, de esta manera, los tres puntos de apoyo que
garantizan la estabilidad del robot.
Construcción de plataforma software para el diseño y la simulación...
27
4 DESCRIPCIÓN DE LOS ROBOTS MÓVILES
Carlos Sosa Marrero
θ
Figura 25: Robot de tipo triciclo
El avance, retroceso o giro de un robot de tipo triciclo se consigue asignando una misma velocidad angular a los motores de ambas ruedas motrices
y un ángulo a la rueda directriz. La velocidad lineal del robot coincide con
la de las ruedas traseras y su velocidad angular viene dada por (12)
v ∗ tan(θ)
(12)
ddt
donde ddt es la distancia entre los ejes delantero y trasero.
El estado del robot estará nuevamente caracterizado por su posición x e
y y su orientación α obtenidas, como en el caso de un robot diferencial, a
partir de (6), (7) y (5), respectivamente.
En la figura 26, se muestra un robot de tipo triciclo con huella circular
creado en la escena de simulación de V-REP.
ω=
Figura 26: Robot de tipo triciclo en la escena de V-REP
Construcción de plataforma software para el diseño y la simulación...
28
4 DESCRIPCIÓN DE LOS ROBOTS MÓVILES
4.2.1.
Carlos Sosa Marrero
Restricciones geométricas para huella circular
Un robot de tipo triciclo con huella circular presenta el diseño que se
muestra en la figura 27.
R
ddt
a
r
did
Figura 27: Robot de tipo triciclo con huella circular
Las ruedas no han de sobresalir del cuerpo del robot. Para tal fin, en el
caso de las ruedas traseras, ha de cumplirse (13).
s
2
2 did + a
ddt
+r ≤R
(13)
+
2
2
En el caso de la rueda delantera, ha de tenerse en cuenta que esta gira de
-π/6 a π/6 y no ha de sobresalir del cuerpo del robot en ningún
momento.
a
Para ello, se calcula en primer lugar el ángulo θdmax = arctg 2r para el que
la distancia de P1 al centro del robot es máxima. Este ángulo es tal que la
diagonal del rectángulo que forma la vista cenital de la rueda es coincidente
con el radio del robot, como se aprecia en la figura 28.
Sin embargo, puede que la rueda nunca alcance dicho ángulo durante su
giro. Por este motivo, se halla el mı́nimo entre π/6 y θdmax . Teniendo en
cuenta que d es estrictamente creciente desde 0 hasta θdmax , de esta forma
se obtiene el ángulo para el que d es máxima dentro del arco de giro de la
rueda.
Construcción de plataforma software para el diseño y la simulación...
29
4 DESCRIPCIÓN DE LOS ROBOTS MÓVILES
Carlos Sosa Marrero
P1
θdmax
Figura 28: Rueda delantera con ángulo θdmax
A continuación, se calculan según (14) y (15) las coordenadas cartesianas
de P1 con respecto al centro del cuerpo del robot y se verifica finalmente que
la rueda no sobresalga en este el caso más desfavorable dentro de su arco de
giro (16).
r a 2
π
2r
, θdmax ∗
+ min
+ r2
(14)
x = −cos arctg
a
6
2
a
π
i r a 2
ddt
y = cos arctg
− min
, θdmax ∗
+ r2 +
2r
6
2
2
h
p
x2 + y 2 ≤ R
(15)
(16)
En cualquier caso, las distancias entre ejes no han de ser mayores que el
diámetro del robot.
Asimismo, las ruedas tampoco han de superponerse. Para ello, se comprueba en primer lugar que las ruedas traseras no se superpongan entre sı́,
para lo cual ha de cumplirse (17).
a < did
(17)
Además, se verifica que la rueda delantera no se superponga en su giro
con las traseras. Para tal fin, se comprueba que o bien la rueda directriz se
encuentre a la suficiente distancia de las ruedas motrices para que sea cual
sea su giro nunca lleguen a superponerse, para lo cual ha de cumplirse (18)
Construcción de plataforma software para el diseño y la simulación...
30
4 DESCRIPCIÓN DE LOS ROBOTS MÓVILES
Carlos Sosa Marrero
o (19), o bien la distancia sea menor pero suficiente para que hasta el ángulo
máximo de giro de π/6 no se produzca superposición entre las ruedas (20).
r a 2
+ r2 < ddt − r
(18)
2
r a 2
did − a
(19)
+ r2 <
2
2
r 2r
π
a 2
did − a
cos arctg
−
∗
(20)
+ r2 <
a
6
2
2
4.2.2.
Restricciones geométricas para huella cuadrangular
Un robot de tipo triciclo con huella cuadrangular presenta el diseño que
se muestra en la figura 29.
b
ddt
l
a
r
did
Figura 29: Robot de tipo triciclo con huella cuadrangular
Las ruedas no han de sobresalir del cuerpo del robot. Para ello, en el caso
de las ruedas traseras, han de cumplirse tanto (21) como (22) .
did + a ≤ b
Construcción de plataforma software para el diseño y la simulación...
(21)
31
4 DESCRIPCIÓN DE LOS ROBOTS MÓVILES
Carlos Sosa Marrero
ddt
l
+r ≤
(22)
2
2
En el caso de la rueda delantera, se comprueba que esta no sobresalga
por los laterales del cuerpo del robot (24). Para ello, se calcula en primer
lugar según (23) la coordenada x de P2 con respecto al centro del robot para
el mı́nimo entre π/6
y el ángulo para el que la coordenada x es máxima
2r
θxmax = arctg a . Este ángulo es tal que la diagonal del rectángulo que
forma la vista cenital de la rueda es paralela al ancho del robot, como se
aprecia en la figura 30.
θxmax
P2
Figura 30: Rueda delantera con ángulo θxmax
Además, sigue siendo válido lo expuesto en las restricciones para un robot
de tipo triciclo de huella circular, según lo cual puede que la rueda no alcance
θxmax en su arco de giro.
r a 2
π
2r
, θxmax ∗
+ r2
(23)
− min
x = cos arctg
a
6
2
b
(24)
2
A fin de verificar que la rueda delantera no sobresalga por la parte frontal
(26), se halla según (25) la coordenada y de P1 con respecto al centro del
robot para el mı́nimo entre
π/6 y el ángulo para el que la coordenada y es
a
máxima θymax = arctg 2r
. Este ángulo es tal que la diagonal del rectángulo
que forma la vista cenital de la rueda es paralela al largo del robot, como se
aprecia en la figura 31.
x≤
Construcción de plataforma software para el diseño y la simulación...
32
4 DESCRIPCIÓN DE LOS ROBOTS MÓVILES
Carlos Sosa Marrero
P1
θymax
Figura 31: Rueda delantera con ángulo θymax
i r a 2
h
a
π
ddt
y = cos arctg
− min
, θymax ∗
+ r2 +
2r
6
2
2
(25)
l
(26)
2
Asimismo, se verifica que las ruedas no se superpongan entre sı́, para lo
cual sigue siendo válido lo expuesto para el caso de un robot tipo triciclo con
huella circular.
y≤
4.3.
Cuatriciclo
Un robot móvil de tipo cuatriciclo cuenta con dos ruedas motrices y dos
ruedas directrices dispuestas como se muestra en la figura 32.
θi
θd
CIR
Figura 32: Robot de tipo cuatriciclo
Construcción de plataforma software para el diseño y la simulación...
33
4 DESCRIPCIÓN DE LOS ROBOTS MÓVILES
Carlos Sosa Marrero
Sus velocidades lineal y angular se obtienen de igual forma que para un
robot de tipo triciclo, con la particularidad de que, en este caso, θ no se
corresponde al ángulo de ninguna de las dos ruedas directrices. A fin de
evitar deslizamiento, el robot ha de poseer un único centro instantáneo de
rotación. Para ello, es preciso que la rueda interior (en el caso que se muestra
en la figura 32, la derecha) adquiera durante el giro un ángulo ligeramente
mayor que la exterior (en este caso, la izquierda). Estos ángulos θi y θd se
calculan, respectivamente, con (27) y (28).
ddi
(27)
θi = arccotg cotg(θ) −
2ddt
ddi
θd = arccotg cotg(θ) +
(28)
2ddt
donde ddi es la distancia entre los ejes derecho e izquierdo y ddt , entre los
ejes delantero y trasero.
El estado del robot vuelve a estar caracterizado por su posición x e y y
su orientación α obtenidas, como en los dos casos anteriores, a partir de (6),
(7) y (5), respectivamente.
En la figura 33, se muestra un robot de tipo cuatriciclo con huella cuadrangular creado en la escena de simulación de V-REP.
Figura 33: Robot de tipo cuatriciclo en la escena de V-REP
Construcción de plataforma software para el diseño y la simulación...
34
4 DESCRIPCIÓN DE LOS ROBOTS MÓVILES
4.3.1.
Carlos Sosa Marrero
Restricciones geométricas para huella circular
Un robot de tipo cuatriciclo con huella circular presenta el diseño que se
muestra en la figura 34.
R
ddt
a
r
did
Figura 34: Robot de tipo cuatriciclo con huella circular
Nuevamente, las ruedas no han de sobresalir del cuerpo del robot. En lo
que respecta a las ruedas traseras, sigue siendo válido lo expuesto para el
caso de un robot de tipo triciclo y huella circular.
En el caso de las ruedas delanteras, se estudia la rueda directriz derecha, siendo los resultados obtenidos válidos también para la rueda directiz
izquierda dada lah simetrı́a del robot.
Ha de tenerse
i
h en cuenta queiesta gira
ddi
ddi
π
y no ha
de θd1 = arccotg cotg( 6 ) + 2ddt a θd2 = arccotg cotg(− π6 ) + 2d
dt
de sobresalir del cuerpo del robot en ningún
Para ello, se calcula
momento.
ddi
a
en primer lugar el ángulo θddmax = arctg ddt − arctg 2r
para el que la
distancia d de P2 al centro del robot es máxima. Este ángulo es tal que la
diagonal del rectángulo que forma la vista cenital de la rueda es coincidente
con el radio del robot, como se aprecia en la figura 35.
Sin embargo, nuevamente puede que la rueda nunca alcance dicho ángulo
durante su giro. Por este motivo, se halla el mı́nimo entre θd2 y θddmax . Teniendo en cuenta que d es estrictamente creciente desde 0 hasta θddmax , de
esta forma se obtiene el ángulo para el que d es máxima dentro del arco de
giro de la rueda.
Construcción de plataforma software para el diseño y la simulación...
35
4 DESCRIPCIÓN DE LOS ROBOTS MÓVILES
Carlos Sosa Marrero
P2
θddmax
Figura 35: Rueda delantera derecha con ángulo θddmax
A continuación, se calculan según (29) y (30) las coordenadas cartesianas
de P2 con respecto al centro del cuerpo del robot y se verifica finalmente que
la rueda no sobresalga en este el caso más desfavorable dentro de su arco de
giro (31).
r did
a 2
+ r2 +
− min (−θd2 , θddmax ) ∗
2
2
(29)
a
i r a 2
ddt
y = cos arctg
+ min (−θd2 , θddmax ) ∗
+ r2 +
2r
2
2
(30)
x = cos arctg
2r
a
h
p
x2 + y 2 ≤ R
(31)
Asimismo, las ruedas tampoco han de superponerse. Para ello, se comprueba en primer lugar que las ruedas traseras no se superpongan entre
sı́, para lo cual sigue siendo válida (17). Además, se verifica que las ruedas delanteras no se superpongan en su giro con las traseras. De nuevo,
se estudia la mitad derecha del robot. Como expuesto
anteriormente,
la
h
i
ddi
π
rueda delantera derecha gira desde θd1 = arccotg cotg( 6 ) + 2ddt a θd2 =
Construcción de plataforma software para el diseño y la simulación...
36
4 DESCRIPCIÓN DE LOS ROBOTS MÓVILES
Carlos Sosa Marrero
i
h
ddi
y no ha de superponerse con la rueda trasera dearccotg cotg(− π6 ) + 2d
dt
recha en ningún momento.
Para ello, se calcula en primer lugar el ángulo
a
θdymin = arctg 2r
para el que la coordenada y de P3 es mı́nima, que coincide con el ángulo para el que la coordenada y de P1 es máxima. Este ángulo
es tal que la diagonal del rectángulo que forma la vista cenital de la rueda
es paralela a la distancia entre los ejes delantero y trasero, como se muestra
en la figura 36
θdymin
P3
Figura 36: Rueda delantera derecha con ángulo θdymin
De nuevo, puede que la rueda no alcance dicho ángulo durante su giro,
por lo que se halla el mı́nimo entre θd2 y θdymin y se calcula la coordenada y
de P3 con respecto al centro del robot según (32).
i r a 2
a
ddt
y = −cos arctg
− min (−θd2 , θdymin ) ∗
+ r2 +
2r
2
2
h
(32)
Finalmente se comprueba que las ruedas no se superpongan con (33).
−ddt
+r
(33)
2
Para verificar que las ruedas traseras no se superpongan entre sı́ sigue
siendo válida (17).
y>
Construcción de plataforma software para el diseño y la simulación...
37
4 DESCRIPCIÓN DE LOS ROBOTS MÓVILES
4.3.2.
Carlos Sosa Marrero
Restricciones geométricas para huella cuadrangular
Un robot de tipo cuatriciclo con huella cuadrangular presenta el diseño
que se muestra en la figura 37.
b
ddt
l
a
r
did
Figura 37: Robot de tipo cuatriciclo con huella cuadrangular
Como para todos los casos anteriores, las ruedas no han de sobresalir del
cuerpo del robot. En lo que respecta a las ruedas traseras, sigue siendo válido
lo expuesto para un robot de tipo triciclo y huella cuadrangular.
En el caso de las ruedas delanteras, se sigue un razonamiento similar al
planteado para el caso de un robot de tipo triciclo y huella cuadrangular
particularizado para la rueda directiz derecha, que se muestra en las figuras
38 y 39, siendo los resultados obtenidos nuevamente válidos para la rueda
izquierda, dada la simetrı́a del robot.
A fin de verificar que la rueda no sobresalga por el lateral del robot con
(24), se halla la coordenada x máxima que puede alcanzar P2 durante su giro
desde θd1 hasta θd2 según (34).
r did
a 2
2r
− min (−θd2 , θdxmax ) ∗
+ r2 +
(34)
x = cos arctg
a
2
2
Construcción de plataforma software para el diseño y la simulación...
38
4 DESCRIPCIÓN DE LOS ROBOTS MÓVILES
Carlos Sosa Marrero
θxmax
P2
Figura 38: Rueda delantera derecha con ángulo θdxmax
Para comprobar que la rueda delantera no sobresalga por la parte frontal
(26), se halla según (35) la coordenada y máxima de P1 durante su giro.
i r a 2
a
ddt
− min (−θd2 , θdymax ) ∗
+ r2 +
y = cos arctg
2r
2
2
h
(35)
P1
θdymax
Figura 39: Rueda delantera derecha con ángulo θdymax
De igual modo, las ruedas tampoco han de superponerse entre sı́, para lo
cual sigue siendo válido todo lo expuesto para el caso de un robot de tipo
cuatriciclo y huella circular.
Construcción de plataforma software para el diseño y la simulación...
39
4 DESCRIPCIÓN DE LOS ROBOTS MÓVILES
4.4.
Carlos Sosa Marrero
Sensores
Sea cual sea el modelo escogido, el robot móvil creado cuenta con los
siguientes sensores, vitales para su correcto comportamiento en los experimentos que se detallarán en el siguiente capı́tulo.
Sensor de proximidad El robot cuenta con un sensor de ultrasonidos,
como el que se muestra en la figura 40. Está situado en el centro de la parte
frontal del robot y se encarga de detectar cualquier obstáculo situado enfrente
del mismo.
Figura 40: Sensor de proximidad [1]
Es de tipo disco, con un ángulo fijo de π/2 , un radio de 0,1 m y un rango
que varı́a de forma que el sensor cubra todo el diámetro (36) o el ancho (37),
según el caso, del cuerpo del robot.
rango =
rango =
R
cos π4
b
2 ∗ cos
π
4
(36)
(37)
Se aprovecha el radio de 0,1 m para que, sumado al rango obtenido, actúe
de margen de seguridad de que el sensor cubra todo el cuerpo del robot.
Sensores de visión El robot dispone de tres sensores de visión como los
que pueden apreciarse en la figura 41.
Construcción de plataforma software para el diseño y la simulación...
40
4 DESCRIPCIÓN DE LOS ROBOTS MÓVILES
Carlos Sosa Marrero
Figura 41: Sensor de visión [1]
Dos de ellos se encargan de detectar el circuito que ha de seguir el robot
en uno de los posibles experimentos. Estos están situados en el borde inferior
de la parte frontal del robot, a 0’1 m de su eje de simetrı́a y apuntando hacia
el suelo. Poseen una proyección ortogonal de 0,01 m x 0,01 m, una resolución
de un pixel y un far clipping plane de 3 veces el radio de las ruedas que
garantiza que el sensor siempre detecte el suelo.
Asimismo, el robot cuenta con un tercer sensor de visión situado en el
mismo punto que el sensor de proximidad encargado de detectar las balizas
hacia las que ha de dirigirse en otro de los posibles experimentos. Posee una
proyección ortogonal
√ de 0,01 m x 0,01 m, una resolución de un pixel y un far
clipping plane de 52 + 52 m que garantiza que el sensor detecte cualquier
baliza situada en la escena de dimensiones 5 m x 5 m.
4.5.
Propiedades dinámicas
Masa La masa de cada uno de los robots se obtiene a partir de unos valores
de densidad fijos de 0,1 g/cm3 para el cuerpo y 0,5 g/cm3 para las ruedas.
Materiales Los robots poseen los materiales predefinidos por V-REP rest
stack grasp material, en el cuerpo; wheelMaterial, en las ruedas y noFrictionMaterial, en las esferas deslizantes en el caso de los robots diferenciales.
4.6.
Adecuación del modelo al software de simulación
A fin de obtener una simulación más rápida y estable en V-REP, se opta
por usar solamente formas puras tanto en el cuerpo (cilı́ndrico o prismátiConstrucción de plataforma software para el diseño y la simulación...
41
4 DESCRIPCIÓN DE LOS ROBOTS MÓVILES
Carlos Sosa Marrero
co) como en las ruedas (cilı́ndricas) del robot. De este modo, y aun siendo
conscientes de que en la realidad el cuerpo del robot contarı́a con unas hendiduras en las que encajarı́an las ruedas, se opta por elevar el cuerpo del robot
una altura de 2 ∗ r, como se aprecia en la figura 42, de forma que las ruedas
puedan girar sin colisionar con el cuerpo.
Figura 42: Robot con formas puras
Por otra parte, las ruedas directrices de los modelo triciclo y cuatriciclo
cuentan con dos ejes de rotación, uno que permite que la rueda gire libremente
y no deslice por el suelo y otro alrededor del cual la rueda adquiere el ángulo
deseado para un determinado giro del robot. Sin embargo, la jerarquı́a de
V-REP no permite encadenar dos articulaciones. Por este motivo, se opta
por la creación, por cada rueda directriz, de una rueda invisible que sirva de
nexo entre ambos ejes, como se aprecia en la jerarquı́a de la figura 43.
Figura 43: Jerarquı́a de un robot con rueda directriz
Construcción de plataforma software para el diseño y la simulación...
42
4 DESCRIPCIÓN DE LOS ROBOTS MÓVILES
Carlos Sosa Marrero
Esta contará con las propiedades respondable, collidable, measurable, detectable y renderable a 0, a fin de que no interfiera con ningún otro cuerpo
durante la simulación y sea, por tanto, inexistente a tales efectos.
Construcción de plataforma software para el diseño y la simulación...
43
5 DESCRIPCIÓN DE LOS EXPERIMENTOS
5.
Carlos Sosa Marrero
Descripción de los experimentos
En este capı́tulo se describen las escenas predefinidas en el software VREP y el comportamiento que se espera del robot en cada una de ellas. Todas
las escenas poseen unas dimensiones de 5 m x 5 m y están delimitadas por
paredes de color grisáceo (R=0.95, G=0.95, B=0.95) y 0’5 m de alto.
Dado el alto grado de personalización que permite Koala en lo que respecta al diseño de los robots y aunque se presume que cualquier combinación
de parámetros introducida por el usuario y verificada por el programa darı́a
lugar a un robot que se comporte de forma adecuada en cada uno de los
experimentos previstos, resulta imposible comprobar todas las posibles combinaciones. Por ello, se facilita para cada experimento tres tablas (una por
modelo) con las dimensiones de robots móviles cuyo correcto comportamiento
ha sido verificado.
5.1.
Cámaras
A fin de poder observar correctamente cualquier detalle de la simulación,
se han dispuesto cuatro cámaras adicionales a las que por defecto proporciona
V-REP en cualquier escena. Las vistas obtenidas por estas cámaras se han
dispuesto en una página 1+5 como la esquematizada en la figura 44.
Figura 44: Página 1+5
Se ha asociado la vista de la cámara principal MainCamera, situada en
Construcción de plataforma software para el diseño y la simulación...
44
5 DESCRIPCIÓN DE LOS EXPERIMENTOS
Carlos Sosa Marrero
el borde inferior del recinto, al recuadro 0. Los recuadros 3 y 4 muestran
las vistas de las cámaras auxiliares 1 y 2, ubicadas, respectivamente en las
esquinas superiores izquierda y derecha del escenario. El recuadro 2 muestra
la vista cenital del recinto proporcionada por DefaultZViewCamera y en el
recuadro 5 se aprecia la vista obtenida por TrackingCamera. Esta cámara se
ofrece a seguir el objeto que el usuario le indique (presumiblemente el propio
robot) haciendo clic en su icono y editando sus propiedades. Idealmente, esta
acción deberı́a ser realizada de forma programática, pero V-REP aún carece
de las funciones necesarias para tal fin. No se ha asociado ninguna vista al
recuadro superior derecho de la página, pues sobre él se sitúa la ventana de
la interfaz desarrollada en Matlab. En la figura 45, se muestra la página de
vistas de uno de los posibles experimentos a modo de ejemplo.
Figura 45: Página de vistas
5.2.
Obstáculos
Esta escena cuenta, como se aprecia en la figura 46, con doce obstáculos
cilı́ndricos de color grisáceo (R=0.95, G=0.95, B=0.95), base de 0,125 m de
radio y 0,5 m de altura. El usuario puede añadir obstáculos con la forma y
dimensiones deseadas en la posición que estime conveniente.
Construcción de plataforma software para el diseño y la simulación...
45
5 DESCRIPCIÓN DE LOS EXPERIMENTOS
Carlos Sosa Marrero
Figura 46: Escena con obstáculos
El robot ha de moverse libremente por ella con la velocidad lineal indicada por el usuario esquivando los obstáculos. Para ello, avanzará en lı́nea recta
hasta que el sensor de proximidad situado en la parte frontal del mismo detecte un obstáculo. En este momento, el robot retrocederá girando ligeramente
durante un determinado intervalo de tiempo y posteriormente realizará una
maniobra de recuperación avanzando y girando ligeramente hacia el sentido
contrario del retroceso. Se desprende que los sensores de visión no resultan
necesarios para el control del robot en esta escena. Se opta, pues, por ocultarlos a fin de obtener una simulación lo más representativa posible.
El comportamiento descrito se consigue de distintas maneras según el tipo
de robot móvil simulado.
Diferencial En el caso de un robot de tipo diferencial el retroceso y ligero
giro del robot se consigue asignando al motor izquierdo una velocidad angular
de -4/5 ω y al motor derecho, de -6/5 ω. El instante de tiempo en el que el
robot ha de finalizar la maniobra de retroceso depende de la velocidad lineal
deseada y se obtiene según
tb = t +
50
v
Construcción de plataforma software para el diseño y la simulación...
(38)
46
5 DESCRIPCIÓN DE LOS EXPERIMENTOS
Carlos Sosa Marrero
donde t es el instante de tiempo actual; 50, una constante obtenida empı́ricamente y v, la velocidad lineal del robot deseada.
Tras el retroceso, el robot ejecuta la maniobra de recuperación asignando
al motor izquierdo una velocidad angular de 6/5 ω y al motor derecho, de
4/5 ω. El instante de tiempo en el que el robot ha de finalizar la maniobra
de recuperación depende también de la velocidad lineal deseada y se obtiene
según
ts = tb +
50
v
(39)
Finalmente, el avance normal del robot se consigue asignando a ambos
motores una velocidad angular de ω. En cualquiera de los tres casos (retroceso, recuperación o avance normal) se consigue que la velocidad lineal del
robot móvil sea la deseada por el usuario.
En el cuadro 1, se recogen las dimensiones de un robot con huella circular
cuyo correcto comportamiento ha sido comprobado para una velocidad lineal
de 0,33 m/s.
Dimensiones
[m]
Radio del cuerpo del robot 0.15
Altura del cuerpo del robot 0.1
Distancia entre ejes
0.2
Radio de las ruedas
0.04
Ancho de las ruedas
0.04
Cuadro 1: Robot diferencial con huella circular verificado en escena con
obstáculos
En la figura 47, se muestra un instante de la simulación llevada a cabo
con dicho robot.
Construcción de plataforma software para el diseño y la simulación...
47
5 DESCRIPCIÓN DE LOS EXPERIMENTOS
Carlos Sosa Marrero
Figura 47: Simulación de robot diferencial en escena con obstáculos
Triciclo En el caso de un robot de tipo triciclo, el retroceso se consigue
aisgnando a los motores de ambas ruedas motrices una velocidad angular de
-ω y un ángulo de π/6 a la rueda directriz.
El instante de tiempo en el que el robot ha de finalizar la maniobra de
retroceso se calcula de forma análoga al caso de un robot de tipo diferencial
pero con una constante mayor, dada la menor maniobrabilidad de los robots
tipo triciclo.
200
(40)
v
Tras el retroceso, el robot ejecuta la maniobra de recuperación asignando
a los motores de ambas ruedas motrices una velocidad angular de ω y un
ángulo de -π/6 a la rueda directriz. El instante de tiempo en el que el robot
ha de finalizar la maniobra de recuperación se obtiene igualmente como
tb = t +
200
(41)
v
Finalmente, el avance normal del robot se consigue asignando a los motores de las ruedas motrices una velocidad angular de ω y un ángulo de 0 a
la rueda directriz. En cualquiera de las tres maniobras se consigue, de nuevo,
que la velocidad lineal del robot móvil sea la deseada por el usuario.
ts = tb +
Construcción de plataforma software para el diseño y la simulación...
48
5 DESCRIPCIÓN DE LOS EXPERIMENTOS
Carlos Sosa Marrero
En el cuadro 2, se recogen las dimensiones de un robot con huella cuadrangular cuyo correcto comportamiento ha sido comprobado para una velocidad
lineal de 0,33 m/s.
Dimensiones
[m]
Largo del cuerpo del robot
Ancho del cuerpo del robot
Altura del cuerpo del robot
Radio de las ruedas
Ancho de las ruedas
Distancia entre ejes izquierdo-derecho
Distancia entre ejes delantero-trasero
0.3
0.3
0.1
0.05
0.05
0.18
0.18
Cuadro 2: Robot de tipo triciclo con huella cuadrangular verificado en escena
con obstáculos
Cuatriciclo En el caso de un robot de tipo cuatriciclo, el retroceso se
consigue asignando a los motores de ambas ruedas motrices una velocidad
angular de -ω , un ángulo θi a la rueda directriz izquierda y un ángulo θd a
la rueda directriz derecha, siendo θi y θd los ángulos que permiten un giro
del robot de π/6 sin que se produzca deslizamiento en las ruedas.
Tras el retroceso, el robot ejecuta la maniobra de recuperación asignando
a los motores de ambas ruedas motrices una velocidad angular de ω , un
ángulo de θi0 a la rueda directriz izquierda y un ángulo θd0 a la rueda directriz
derecha, a fin de conseguir un giro de -π/6 sin que se produzca deslizamiento
en las ruedas. Los instantes de tiempo en los que el robot ha de finalizar las
maniobras de retroceso y recuperación se calculan de igual forma que para un
robot de tipo triciclo, ya que ambos modelos comparten una maniobrabilidad
más reducida.
Finalmente, el avance normal del robot se consigue asignando a los motores de las ruedas directrices una velocidad angular de ω y un ángulo de 0 a
las rueda directrices. Nuevamente, se garantiza que, en cualquiera de las tres
maniobras, la velocidad lineal del robot móvil sea la deseada por el usuario.
En el cuadro 3, se recogen las dimensiones de un robot con huella circular
cuyo correcto comportamiento ha sido comprobado para una velocidad lineal
de 0,33 m/s.
Construcción de plataforma software para el diseño y la simulación...
49
5 DESCRIPCIÓN DE LOS EXPERIMENTOS
Carlos Sosa Marrero
Dimensiones
[m]
Radio del cuerpo del robot
Altura del cuerpo del robot
Radio de las ruedas
Ancho de las ruedas
Distancia entre ejes izquierdo-derecho
Distancia entre ejes delantero-trasero
0.2
0.1
0.05
0.05
0.18
0.18
Cuadro 3: Robot de tipo cuatriciclo con huella circular verificado en escena
con obstáculos
5.3.
Circuito
Esta escena cuenta, como se aprecia en la figura 48, con un circuito de
color negro y 0,15 m de ancho dispuesto en el suelo. El usuario puede añadir
obstáculos con la forma y dimensiones deseadas en la posición que estime
conveniente.
Figura 48: Escena con circuito
El robot ha de incorporarse al circuito a partir de su posición inicial en el
centro de la escena y seguirlo indefinidamente. Para ello, el robot avanzará
Construcción de plataforma software para el diseño y la simulación...
50
5 DESCRIPCIÓN DE LOS EXPERIMENTOS
Carlos Sosa Marrero
en lı́nea recta hasta que uno de sus dos sensores de vision situados en la parte
frontal del mismo detecte el circuito.
En este momento, el robot girará en el sentido del sensor en cuestión hasta
que, presumiblemente, los dos sensores detecten el circuito. En dicho instante
el robot girará en el sentido contrario al que lo venı́a haciendo hasta dicho
instante hasta que, de nuevo, solo uno de los dos sensores detecte circuito.
En tal caso, el robot volverá a girar en el sentido del sensor en cuestión.
Una vez que el robot se haya incorporado al circuito, este comportamiento
seguirá siendo válido. El robot girará a la izquierda si dicho sensor detecta
circuito (consecuencia de una curva a la izquierda) y a la derecha si sucede
lo contrario. Se considera que un sensor detecta el circuito, de color negro,
cuando el valor medio de intensidad obtenido es inferior a 0,3.
Asimismo, el robot continuará siendo capaz de esquivar posibles obstáculos que el usuario añada a la escena, retrociendo y girando ligeramente y
recuperándose tal y como hacı́a en la escena anterior. Se desprende que el
sensor de visión central no resulta necesario para el control del robot en esta
escena. Se opta, pues, por ocultarlo a fin de obtener una simulación lo más
representativa posible.
El comportamiento descrito se consigue de distintas maneras según el tipo
de robot móvil simulado.
Diferencial En el caso de un robot de tipo diferencial, el giro a la izquierda
se consigue asignando al motor izquierdo una velocidad angular de 4/5 ω y
al motor derecho, de 6/5 ω. De forma análoga, el robot gira a la derecha
asignando al motor izquierdo una velocidad angular de 6/5 ω y de 4/5 ω al
motor derecho.
Las maniobras de retroceso y recuperación al detectar un obstáculo se
llevan a cabo de igual forma que en la escena anterior, si bien en esta ocasión
los ligeros giros durante estas maniobras no son fijos. Durante el retroceso,
se asigna una velocidad angular de de -4/5 ω al motor izquierdo y de -6/5 ω
al motor derecho si el robot giró por última vez a la derecha y de -6/5 ω al
motor izquierdo y de -4/5 ω al motor derecho, si lo hizo a la izquierda. En
la maniobra de recuperación, el robot gira ligeramente en sentido contrario
al que lo hiciera durante el retroceso. Los instantes de tiempo en los que el
robot ha de finalizar sendas maniobras se calculan de igual forma que para la
escena con obstáculos. En cualquier caso, se garantiza que la velocidad lineal
del robot sea la indicada por el usuario.
Construcción de plataforma software para el diseño y la simulación...
51
5 DESCRIPCIÓN DE LOS EXPERIMENTOS
Carlos Sosa Marrero
En el cuadro 4, se recogen las dimensiones de un robot con huella cuadrangular cuyo correcto comportamiento ha sido comprobado para una velocidad
lineal de 0,33 m/s.
Dimensiones
[m]
Largo del cuerpo del robot 0.4
Ancho del cuerpo del robot 0.3
Altura del cuerpo del robot 0.1
Radio de las ruedas
0.05
Ancho de las ruedas
0.05
Distancia entre ejes
0.2
Cuadro 4: Robot diferencial con huella cuadrangular verificado en escena con
circuito
En la figura 49, se aprecia un instante de la simulación llevada a cabo con
dicho robot.
Figura 49: Simulación de robot diferencial en escena con circuito
Triciclo En el caso de un robot de tipo triciclo, el giro a la izquierda se
consigue manteniendo la velocidad angular de ω a los motores de ambas
Construcción de plataforma software para el diseño y la simulación...
52
5 DESCRIPCIÓN DE LOS EXPERIMENTOS
Carlos Sosa Marrero
ruedas motrices y asignando un ángulo de π/6 a la rueda directriz. De forma
análoga, el robot gira a la derecha manteniendo la velocidad angular de las
ruedas motrices y asignando a la rueda directiz un ángulo de -π/6.
Las maniobras de retroceso y recuperación se llevan a cabo de forma
similar al caso de la escena con obstáculos, si bien en esta ocasión los ángulos
de la rueda directriz no son fijos. Durante el retroceso, este será igual a -π/6
si el robot giró por última vez a la izquierda y a π/6 , si lo hizo a la derecha.
Por su parte durante la maniobra de recuperación, el ángulo de la rueda
directriz será de π/6 si el último giro realizado por el robot tuvo lugar hacia
la izquierda y de -π/6, si fue hacia la derecha. Los instantes de tiempo en los
que el robot ha de finalizar sendas maniobras se calculan de igual forma que
para la escena con obstáculos. En cualquier caso, se consigue, de nuevo, que
la velocidad lineal del robot móvil sea la deseada por el usuario.
En el cuadro 5, se recogen las dimensiones de un robot con huella circular
cuyo correcto comportamiento ha sido comprobado para una velocidad lineal
de 0,25 m/s.
Dimensiones
[m]
Radio del cuerpo del robot
Altura del cuerpo del robot
Radio de las ruedas
Ancho de las ruedas
Distancia entre ejes izquierdo-derecho
Distancia entre ejes delantero-trasero
0.2
0.1
0.05
0.05
0.15
0.15
Cuadro 5: Robot de tipo triciclo con huella circular verificado en escena con
circuito
Cuatriciclo En el caso de un robot de tipo cuatriciclo, el giro a la izquierda
se consigue manteniendo la velocidad angular de ω a los motores de ambas
ruedas motrices y asignando un ángulo de θi a la rueda directriz izquierda y
un ángulo de θd a la rueda directriz derecha, a fin de conseguir un giro de π/6
sin que se produzca deslizamiento en las ruedas. De forma análoga, el robot
gira a la derecha manteniendo la velocidad angular de las ruedas motrices y
asignando un ángulo de θi0 a la rueda directriz izquierda y un ángulo de θd0 a
la rueda directriz derecha, consiguiendo un giro de -π/6 sin que se produzca
deslizamiento en las ruedas
Construcción de plataforma software para el diseño y la simulación...
53
5 DESCRIPCIÓN DE LOS EXPERIMENTOS
Carlos Sosa Marrero
Las maniobras de retroceso y recuperación se llevan a cabo de forma
similar al caso de la escena con obstáculos, si bien en esta ocasión los ángulos
de giro no son fijos. Durante el retroceso, este será igual a -π/6 si el robot giró
por última vez a la izquierda y a π/6 , si lo hizo a la derecha. Por su parte
durante la maniobra de recuperación, el ángulo será de π/6 si el último giro
realizado por el robot tuvo lugar hacia la izquierda y de -π/6, si fue hacia la
derecha. Los instantes de tiempo en los que el robot ha de finalizar sendas
maniobras se calculan de igual forma que para la escena con obstáculos.
Nuevamente, se consigue que, en todo momento la velocidad lineal del robot
sea la deseada por el usuario.
En el cuadro 6, se recogen las dimensiones de un robot con huella cuadrangular cuyo correcto comportamiento ha sido comprobado para una velocidad
lineal de 0,25 m/s.
Dimensiones
[m]
Largo del cuerpo del robot
Ancho del cuerpo del robot
Altura del cuerpo del robot
Radio de las ruedas
Ancho de las ruedas
Distancia entre ejes izquierdo-derecho
Distancia entre ejes delantero-trasero
0.3
0.3
0.1
0.05
0.05
0.15
0.15
Cuadro 6: Robot de tipo cuatriciclo con huella cuadrangular verificado en
escena con circuito
5.4.
Balizas
Esta escena cuenta, como se distingue en la figura 50, con ocho balizas
cilı́ndricas de color verde (R=0, G=1, B=0), base de 0,125 m de radio y 0,5
m de altura y tres obstáculos cúbicos, de color grisáceo (R=0.95, G=0.95,
B=0.95) y 0,4 m de lado, situados entre la posición inicial del robot en el
centro de la escena y tres de las balizas. El usuario puede añadir balizas y
obstáculos, estos últimos con la forma y dimensiones deseadas, en la posición
que estime conveniente.
Construcción de plataforma software para el diseño y la simulación...
54
5 DESCRIPCIÓN DE LOS EXPERIMENTOS
Carlos Sosa Marrero
Figura 50: Escena con balizas
El robot ha de girar hasta que el sensor de visión situado en la parte frontal
del mismo detecte un objeto de color verde. En dicho momento, avanzará en
lı́nea recta hasta que el sensor de proximidad detecte la baliza. El robot
retrocederá entonces girando ligeramente y comenzará la búsqueda de un
nuevo objetivo. Se considera que el sensor detecta una baliza, de color verde,
cuando los valores de R y B obtenidos sean inferiores a 0.2 y el valor de G,
superior a 0.8.
Asimismo, el robot seguirá siendo capaz de esquivar posibles obstáculos
que el usuario añada a la escena. Se desprende que los sensores de visión
izquierdo y derecho no resultan necesarios para el control del robot en esta
escena. Se opta, pues, por ocultarlos a fin de obtener una simulación lo más
representativa posible.
El comportamiento descrito se consigue de distiintas maneras según el
tipo de robot móvil simulado.
Diferencial En el caso de un robot de tipo diferencial, el giro sobre sı́
mismo en busca de una baliza se consigue asignado al motor izquerdo una
velocidad angular de 1/30 ω y al motor derecho, de -1/30 ω. Estas han de
ser tan reducidas para impedir que el robot pase por alto alguna baliza. Las
Construcción de plataforma software para el diseño y la simulación...
55
5 DESCRIPCIÓN DE LOS EXPERIMENTOS
Carlos Sosa Marrero
maniobras de avance y retroceso se ejecutan de igual forma que en las escenas
anteriores.
En el cuadro 7, se recogen las dimensiones de un robot con huella circular
cuyo correcto comportamiento ha sido comprobado para una velocidad lineal
de 0,33 m/s.
Dimensiones
[m]
Radio del cuerpo del robot 0.15
Altura del cuerpo del robot 0.1
Distancia entre ejes
0.18
Radio de las ruedas
0.04
Ancho de las ruedas
0.04
Cuadro 7: Robot diferencial con huella circular verificado en escena con balizas
En la figura 51, se muestra un instante de la simulación llevada a cabo
con dicho robot.
Figura 51: Simulación de robot diferencial en escena con balizas
Construcción de plataforma software para el diseño y la simulación...
56
5 DESCRIPCIÓN DE LOS EXPERIMENTOS
Carlos Sosa Marrero
Triciclo En el caso de un robot de tipo triciclo, el giro en busca de una
baliza se consigue asignado a los motores de ambas ruedas motrices una
velocidad angular de ω y un ángulo de π/6 a la rueda directriz. Las maniobras
de avance y retroceso se ejecutan de igual forma que en las escenas anteriores.
En el cuadro 8, se recogen las dimensiones de un robot con huella cuadrangular cuyo correcto comportamiento ha sido comprobado para una velocidad
lineal de 0,33 m/s.
Dimensiones
[m]
Largo del cuerpo del robot
Ancho del cuerpo del robot
Altura del cuerpo del robot
Radio de las ruedas
Ancho de las ruedas
Distancia entre ejes izquierdo-derecho
Distancia entre ejes delantero-trasero
0.3
0.3
0.1
0.05
0.05
0.18
0.18
Cuadro 8: Robot de tipo triciclo con huella cuadrangular verificado en escena
con balizas
Cuatriciclo En el caso de un robot de tipo cuatriciclo, el giro en busca
de una baliza se consigue asignando a los motores de ambas ruedas motrices
una velocidad angular de ω, un ángulo de θi a la rueda directriz izquierda y
un ángulo θd a la rueda directriz derecha, a fin de conseguir un giro de π/6
sin que se produzca deslizamiento en las ruedas. Las maniobras de avance y
retroceso se ejecutan de igual forma que en las escenas anteriores.
En el cuadro 9, se recogen las dimensiones de un robot con huella circular
cuyo correcto comportamiento ha sido comprobado para una velocidad lineal
de 0,33 m/s.
Construcción de plataforma software para el diseño y la simulación...
57
5 DESCRIPCIÓN DE LOS EXPERIMENTOS
Carlos Sosa Marrero
Dimensiones
[m]
Radio del cuerpo del robot
Altura del cuerpo del robot
Radio de las ruedas
Ancho de las ruedas
Distancia entre ejes izquierdo-derecho
Distancia entre ejes delantero-trasero
0.2
0.1
0.05
0.05
0.18
0.18
Cuadro 9: Robot de tipo cuatriciclo con huella circular verificado en escena
con balizas
5.5.
Escena vacı́a
Esta escena, vacı́a inicialmente como se muestra en la figura 52, permite
añadir en las posiciones indicadas por el usuario obstáculos con la forma y
dimensiones deseadas y balizas.
Figura 52: Escena vacı́a
El usuario eligirá qué tipo de comportamiento (dirigirse hacia las balizas
esquivando los obstáculos, o simplemente evitar estos útlimos) desea que
Construcción de plataforma software para el diseño y la simulación...
58
5 DESCRIPCIÓN DE LOS EXPERIMENTOS
Carlos Sosa Marrero
muestre el robot.
5.6.
Escena creada por el usuario
El usuario eligirá qué tipo de comportamiento (seguir un circuito esquivando los obstáculos, dirigirse hacia las balizas esquivando los obstáculos, o
simplemente evitar estos útlimos) desea que muestre el robot en una escena
creada previamente.
Construcción de plataforma software para el diseño y la simulación...
59
6 CONCLUSIONES Y LÍNEAS ABIERTAS
6.
6.1.
Carlos Sosa Marrero
Conclusiones y lı́neas abiertas
Conclusiones
De acuerdo a los objetivos del Trabajo expuestos al comienzo de esta
memoria, se ha desarrollado una plataforma que permite el diseño y la simulación de robots móviles. Su correcto funcionamiento se admite probado por
los experimentos descritos en el capı́tulo anterior.
Se considera que la elección de integrar en la plataforma el software de
experimentación V-REP, cuyo funcionamiento se desconocı́a antes de iniciar
este Trabajo, se ha antojado acertada. Sus extensas funcionalidades han permitido desarrollar posibilidades que se hubieran visto restringidas de haber
limitado el trabajo realizado a herramientas software que resultaban familiares como era el caso de Matlab.
En lo que respecta a las posibles aplicaciones del trabajo realizado, se ha
buscado en todo momento que la interfaz desarrollada fuera clara y accesible a
cualquier usuario sin experiencia en el manejo de sofware avanzado. Además,
V-REP permite ofrecer simulaciones visualmente muy atractivas. Por este
motivo, se aprecia la utilidad que la plataforma podrı́a tener en entornos
universitarios o, incluso, en niveles educativos inferiores a fin de despertar la
curiosidad por la robótica en potenciales ingenieros.
6.2.
Lı́neas abiertas
Actualmente, Koala permite probar las tres configuraciones de robots
móviles más extendidas en unos escenarios predefinidos que se han considerado los más adecuados para testear el funcionamiento básico del robot.
Sin embargo, la estructura de la plataforma permitirı́a añadir en un futuro
de forma sencilla cualquier otra configuración del robot o escenario que se
estimen oportunos.
No obstante, resulta necesario tener en cuenta que cualquier añadido al
trabajo realizado vendrı́a condicionado por la existencia o no de funciones
para tal fin en las API (preferiblemente, la remota) que ofrece V-REP. A este
respecto, se recomendarı́a modificar, en cuanto se desarrollen las funciones de
la API remota adecuadas, el método de creación del robot y demás objetos en
la escena para que esta tuviera lugar desde la propia aplicación de Matlab.
De esta forma, no serı́a ya necearia la inclusión del script create en cada
escena. Se aconsejarı́a, además asociar la cámara TrackingCamera al robot
Construcción de plataforma software para el diseño y la simulación...
60
6 CONCLUSIONES Y LÍNEAS ABIERTAS
Carlos Sosa Marrero
de forma programática en cuanto se desarrolle una función de la API que lo
permita.
Asimismo, durante el desarrollo del trabajo, se consideró que el robot
dibujara su trayectoria en el suelo del escenario. Dada la inexistencia de funciones para tal fin en la API remota, se realizó una tentativa con funciones
de la API regular llamadas desde la aplicación de Matlab que resultó ineficiente en términos de velocidad de simulación. Nuevamente, se recomendarı́a
no desistir de esta idea y analizar las posibilidades que a este respecto se
ofrezcan en un futuro.
Por otra parte, ya a más largo plazo, se propondrı́a la construcción fı́sica
de un robot (controlado por el mismo código desarrollado en Matlab exportado a algún tipo de sistema embebido) y un escenario de las caracterı́sticas
de los simulados que permitiera corroborar los resultados de la simulación en
un entorno real.
Construcción de plataforma software para el diseño y la simulación...
61
REFERENCIAS
Carlos Sosa Marrero
Referencias
[1] Coppelia Robotics, V-REP User Manual. Version 3.3.1, 2016
Disponible en: http://www.coppeliarobotics.com/helpFiles/
[2] G. Dudek, M. Jenkin, Computational Principles of Mobile Robotics,
2000
[3] A. Ollero Baturone, Robótica:manipuladores y robots móviles, 1991
[4] R. Siegwart, Introduction to Autonomous Mobile Robots, 2004
Construcción de plataforma software para el diseño y la simulación...
62
A CÓDIGO DE MATLAB
A.
A.1.
1
Carlos Sosa Marrero
Código de Matlab
Start.m
function varargout = Start ( varargin )
2
3
4
5
6
7
8
9
10
11
12
13
% Comienzo d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
gui Singleton = 1;
mfilename , . . .
g u i S t a t e = s t r u c t ( ’ gui Name ’ ,
’ gui Singleton ’ , gui Singleton , . . .
’ gui OpeningFcn ’ , @Start OpeningFcn , . . .
’ gui OutputFcn ’ , @Start OutputFcn , . . .
’ gui LayoutFcn ’ ,
[] , ...
’ gui Callback ’ ,
[]) ;
i f n a r g i n && i s c h a r ( v a r a r g i n { 1 } )
g u i S t a t e . gui Callback = s t r 2 f u n c ( varargin {1}) ;
end
14
15
16
17
18
19
20
i f nargout
[ varargout {1: nargout } ] = gui mainfcn ( gui State , varargin { : } )
;
else
gui mainfcn ( gui State , varargin {:}) ;
end
% Fin d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
21
22
23
24
25
26
%−−− Se e j e c u t a j u s t o a n t e s de que S t a r t s e haga v i s i b l e .
f u n c t i o n S t ar t O pe n i ng F c n ( hObject , ev en td at a , h a n d l e s , v a r a r g i n )
g l o b a l next
next =0;
27
28
29
30
31
32
33
34
35
% Se p o s i c i o n a l a ventana en l a e s q u i n a s u p e r i o r d e r e c h a de l a
pantalla .
s c r s z=g e t ( 0 , ’ S c r e e n S i z e ’ ) ;
p o s a c t=g e t ( g c f , ’ P o s i t i o n ’ ) ;
xr=s c r s z ( 3 )−p o s a c t ( 3 ) ;
xp=round ( xr / 2 ) ;
yr=s c r s z ( 4 )−p o s a c t ( 4 ) ;
yp=round ( yr / 2 ) ;
s e t ( g c f , ’ p o s i t i o n ’ , [ xp , yp , p o s a c t ( 3 ) , p o s a c t ( 4 ) ] ) ;
36
37
38
h a n d l e s . output = hObject ;
g u i d a t a ( hObject , h a n d l e s ) ;
39
Construcción de plataforma software para el diseño y la simulación...
63
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
40
41
42
43
%−−− Las s a l i d a s de e s t a f u n c i ó n s e d e v u e l v e n por l a lı́ n e a de
comandos .
f u n c t i o n v a r a r g o u t = Start OutputFcn ( hObject , ev en td at a , h a n d l e s
)
v a r a r g o u t {1} = h a n d l e s . output ;
44
45
46
47
48
49
50
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Connect .
f u n c t i o n C o n n e c t C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
g l o b a l vrep
global clientID
g l o b a l next
51
52
53
54
55
56
57
58
59
60
61
vrep=remApi ( ’ remoteApi ’ ) ;
vrep . s i m x F i n i s h ( −1) ;
c l i e n t I D=vrep . s i m x S t a r t ( ’ 1 2 7 . 0 . 0 . 1 ’ , 1 9 9 9 7 , t r u e , t r u e
,5000 ,5) ;
i f ( c l i e n t I D ˜=−1)
next =1;
close Start
SelectScene
else
e r r o r d l g ( ’No ha s i d o p o s i b l e c o n e c t a r con e l s e r v i d o r .
Compruebe que V−REP s e e n c u e n t r e a b i e r t o . ’ , ’ERROR ’ )
end
62
63
64
65
66
%−−− Función que s e e j e c u t a cuando s e i n t e n t a c e r r a r l a ventana
.
f u n c t i o n f i g u r e 1 C l o s e R e q u e s t F c n ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
67
68
69
70
71
72
73
74
75
76
% S i l a p e t i c i ó n de c i e r r e de l a ventana no e s c o n s e c u e n c i a d e l
normal t r á n s i t o d e l programa de una ventana a o t r a
% s i n o que d e r i v a d e l c l i c por p a r t e d e l u s u a r i o en e l i c o n o de
c i e r r e , s e e x i g e una c o n f i r m a c i ó n .
i f next==0
opc=q u e s t d l g ( ’ ¿Desea s a l i r d e l programa ? ’ , ’ S a l i r ’ , ’ Sı́ ’ , ’No ’ ,
’No ’ ) ;
i f strcmp ( opc , ’No ’ )
return
end
end
next =0;
Construcción de plataforma software para el diseño y la simulación...
64
A CÓDIGO DE MATLAB
77
Carlos Sosa Marrero
d e l e t e ( hObject ) ;
Construcción de plataforma software para el diseño y la simulación...
65
A CÓDIGO DE MATLAB
A.2.
1
2
3
4
5
6
7
8
9
10
11
12
Carlos Sosa Marrero
SelectScene.m
function varargout = SelectScene ( varargin )
% Comienzo d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
gui Singleton = 1;
g u i S t a t e = s t r u c t ( ’ gui Name ’ ,
mfilename , . . .
’ gui Singleton ’ , gui Singleton , . . .
’ gui OpeningFcn ’ , @SelectScene OpeningFcn , . . .
’ gui OutputFcn ’ , @SelectScene OutputFcn , . . .
[] , ...
’ gui LayoutFcn ’ ,
’ gui Callback ’ ,
[]) ;
i f n a r g i n && i s c h a r ( v a r a r g i n { 1 } )
g u i S t a t e . gui Callback = s t r 2 f u n c ( varargin {1}) ;
end
13
14
15
16
17
18
19
i f nargout
[ varargout {1: nargout } ] = gui mainfcn ( gui State , varargin { : } )
;
else
gui mainfcn ( gui State , varargin {:}) ;
end
% Fin d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
20
21
22
23
24
25
26
27
28
29
30
31
%−−− Se e j e c u t a j u s t o a n t e s de que S e l e c t S c e n e s e haga v i s i b l e .
f u n c t i o n S e l e c t S c e n e O p e n i n g F c n ( hObject , e ve nt da ta , h a n d l e s ,
varargin )
% Se p o s i c i o n a l a ventana en l a e s q u i n a s u p e r i o r d e r e c h a de l a
pantalla .
s c r s z=g e t ( 0 , ’ S c r e e n S i z e ’ ) ;
p o s a c t=g e t ( g c f , ’ P o s i t i o n ’ ) ;
xr=s c r s z ( 3 )−p o s a c t ( 3 ) ;
xp=round ( xr / 2 ) ;
yr=s c r s z ( 4 )−p o s a c t ( 4 ) ;
yp=round ( yr / 2 ) ;
s e t ( g c f , ’ p o s i t i o n ’ , [ xp , yp , p o s a c t ( 3 ) , p o s a c t ( 4 ) ] ) ;
32
33
34
h a n d l e s . output = hObject ;
g u i d a t a ( hObject , h a n d l e s ) ;
35
36
37
38
%−−− Las s a l i d a s de e s t a f u n c i ó n s e d e v u e l v e n por l a lı́ n e a de
comandos .
f u n c t i o n v a r a r g o u t = S e l e c t S c e n e O u t p u t F c n ( hObject , e ve ntd at a ,
handles )
Construcción de plataforma software para el diseño y la simulación...
66
A CÓDIGO DE MATLAB
39
Carlos Sosa Marrero
v a r a r g o u t {1} = h a n d l e s . output ;
40
41
42
43
44
45
46
47
48
%−−− Función que s e e j e c u t a a l p u l s a r e l botón ’ O b s t a c l e s ’
e n c a r g a d a de a b r i r l a e s c e n a a s o c i a d a en V−REP.
f u n c t i o n O b s t a c l e s C a l l b a c k ( hObject , ev e ntd at a , h a n d l e s )
g l o b a l vrep
global clientID
g l o b a l next
g l o b a l num obs
global scene
49
50
51
s c e n e =1;
num obs =12;
52
53
54
55
56
f i l e n a m e= ’ O b s t a c l e s . t t t ’ ;
path=pwd ;
vrep . simxLoadScene ( c l i e n t I D , [ path , ’ \ ’ , f i l e n a m e ] , 1 , vrep .
simx opmode blocking ) ;
vrep . simxSynchronous ( c l i e n t I D , t r u e ) ;
57
58
59
60
next =1;
close SelectScene
SelectRobot
61
62
63
%−−− Función que s e e j e c u t a a l p u l s a r e l botón ’ C i r c u i t ’
e n c a r g a d a de a b r i r l a e s c e n a a s o c i a d a en V−REP.
64
65
66
67
68
69
70
f u n c t i o n C i r c u i t C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
g l o b a l vrep
global clientID
g l o b a l next
g l o b a l num obs
global scene
71
72
73
s c e n e =2;
num obs =0;
74
75
76
77
78
f i l e n a m e= ’ C i r c u i t . t t t ’ ;
path=pwd ;
vrep . simxLoadScene ( c l i e n t I D , [ path , ’ \ ’ , f i l e n a m e ] , 1 , vrep .
simx opmode blocking ) ;
vrep . simxSynchronous ( c l i e n t I D , t r u e ) ;
79
Construcción de plataforma software para el diseño y la simulación...
67
A CÓDIGO DE MATLAB
80
81
82
Carlos Sosa Marrero
next =1;
close SelectScene
SelectRobot
83
84
85
86
87
88
89
90
91
92
%−−− Función que s e e j e c u t a a l p u l s a r e l botón ’ Markers ’
e n c a r g a d a de
% a b r i r l a e s c e n a a s o c i a d a en V−REP.
f u n c t i o n M a r k e r s C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
g l o b a l vrep
global clientID
g l o b a l next
g l o b a l num obs
g l o b a l num marker
global scene
93
94
95
96
s c e n e =3;
num obs =3;
num marker =8;
97
98
99
100
101
f i l e n a m e= ’ Markers . t t t ’ ;
path=pwd ;
vrep . simxLoadScene ( c l i e n t I D , [ path , ’ \ ’ , f i l e n a m e ] , 1 , vrep .
simx opmode blocking ) ;
vrep . simxSynchronous ( c l i e n t I D , t r u e ) ;
102
103
104
105
next =1;
close SelectScene
SelectRobot
106
107
108
109
110
111
112
113
114
115
%−−− Función que s e e j e c u t a a l p u l s a r e l botón ’ Empty ’
e n c a r g a d a de
% a b r i r l a e s c e n a a s o c i a d a en V−REP.
f u n c t i o n Empty Callback ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l vrep
global clientID
g l o b a l next
g l o b a l num obs
g l o b a l num marker
global scene
116
117
118
119
s c e n e =4;
num obs =0;
num marker =0;
120
121
f i l e n a m e= ’ Empty . t t t ’ ;
Construcción de plataforma software para el diseño y la simulación...
68
A CÓDIGO DE MATLAB
122
123
124
Carlos Sosa Marrero
path=pwd ;
vrep . simxLoadScene ( c l i e n t I D , [ path , ’ \ ’ , f i l e n a m e ] , 1 , vrep .
simx opmode blocking ) ;
vrep . simxSynchronous ( c l i e n t I D , t r u e ) ;
125
126
127
128
next =1;
close SelectScene
SelectRobot
129
130
131
132
133
134
135
136
137
138
%−−− Función que s e e j e c u t a a l p u l s a r e l botón ’ Load ’ e n c a r g a d a
de
% a b r i r en V−REP l a e s c e n a e l e g i d a por e l u s u a r i o y comprobar
que c o n t e n g a
% e l dummy ’ c r e a t e ’ .
f u n c t i o n L o a d C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l vrep
global clientID
g l o b a l next
global scene
139
140
s c e n e =5;
141
142
143
144
[ f i l e n a m e , path ]= u i g e t f i l e ( ’ ∗ . t t t ’ , ’ S e l e c c i o n e una e s c e n a ’ ) ;
vrep . simxLoadScene ( c l i e n t I D , [ path , ’ \ ’ , f i l e n a m e ] , 1 , vrep .
simx opmode blocking ) ;
r e t=vrep . simxGetObjectHandle ( c l i e n t I D , ’ c r e a t e ’ , vrep .
simx opmode blocking ) ;
145
146
147
148
149
150
i f ( r e t ˜=0)
e r r o r d l g ( ’ La e s c e n a s e l e c c i o n a d a no e s v á l i d a . Compruebe que
c o n t i e n e e l dummmy c r e a t e con e l s c r i p t en Lua que puede
e n c o n t r a r en e s t a misma c a r p e t a . ’ , ’ERROR ’ )
vrep . s i m x C l o s e S c e n e ( c l i e n t I D , vrep . s i m x o p m o d e b l o c k i n g ) ;
else
vrep . simxSynchronous ( c l i e n t I D , t r u e ) ;
151
next =1;
close SelectScene
SelectRobot
152
153
154
155
end
156
157
158
159
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Cancel .
f u n c t i o n C a n c e l C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
Construcción de plataforma software para el diseño y la simulación...
69
A CÓDIGO DE MATLAB
160
161
Carlos Sosa Marrero
g l o b a l vrep
global clientID
162
163
164
vrep . s i m x F i n i s h ( c l i e n t I D ) ;
close SelectScene
165
166
167
168
169
%−−− Función que s e e j e c u t a a l p u l s a r e l botón PreviousView .
f u n c t i o n P r e v i o u s V i e w C a l l b a c k ( hObject , ev e ntd at a , h a n d l e s )
PreView
170
171
172
173
174
%−−− Función que s e e j e c u t a cuando s e i n t e n t a c e r r a r l a ventana
.
f u n c t i o n f i g u r e 1 C l o s e R e q u e s t F c n ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
175
176
177
178
179
180
181
182
183
184
185
% S i l a p e t i c i ó n de c i e r r e de l a ventana no e s c o n s e c u e n c i a d e l
normal t r á n s i t o d e l programa de una ventana a o t r a
% s i n o que d e r i v a d e l c l i c por p a r t e d e l u s u a r i o en e l i c o n o de
c i e r r e , s e e x i g e una c o n f i r m a c i ó n .
i f next==0
opc=q u e s t d l g ( ’ ¿Desea s a l i r d e l programa ? ’ , ’ S a l i r ’ , ’ Sı́ ’ , ’No ’ ,
’No ’ ) ;
i f strcmp ( opc , ’No ’ )
return
end
end
next =0;
d e l e t e ( hObject ) ;
Construcción de plataforma software para el diseño y la simulación...
70
A CÓDIGO DE MATLAB
A.3.
1
2
3
4
5
6
7
8
9
10
11
12
Carlos Sosa Marrero
PreView.m
f u n c t i o n v a r a r g o u t = PreView ( v a r a r g i n )
% Comienzo d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
gui Singleton = 1;
g u i S t a t e = s t r u c t ( ’ gui Name ’ ,
mfilename , . . .
’ gui Singleton ’ , gui Singleton , . . .
’ gui OpeningFcn ’ , @PreView OpeningFcn , . . .
’ gui OutputFcn ’ , @PreView OutputFcn , . . .
[] , ...
’ gui LayoutFcn ’ ,
’ gui Callback ’ ,
[]) ;
i f n a r g i n && i s c h a r ( v a r a r g i n { 1 } )
g u i S t a t e . gui Callback = s t r 2 f u n c ( varargin {1}) ;
end
13
14
15
16
17
18
19
i f nargout
[ varargout {1: nargout } ] = gui mainfcn ( gui State , varargin { : } )
;
else
gui mainfcn ( gui State , varargin {:}) ;
end
% Fin d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
20
21
22
23
24
25
26
27
28
29
30
31
%−−− Se e j e c u t a j u s t o a n t e s de que PreView s e haga v i s i b l e .
f u n c t i o n PreView OpeningFcn ( hObject , e ve nt da ta , h a n d l e s ,
varargin )
% Se p o s i c i o n a l a ventana en l a e s q u i n a s u p e r i o r d e r e c h a de l a
pantalla .
s c r s z=g e t ( 0 , ’ S c r e e n S i z e ’ ) ;
p o s a c t=g e t ( g c f , ’ P o s i t i o n ’ ) ;
xr=s c r s z ( 3 )−p o s a c t ( 3 ) ;
xp=round ( xr / 2 ) ;
yr=s c r s z ( 4 )−p o s a c t ( 4 ) ;
yp=round ( yr / 2 ) ;
s e t ( g c f , ’ p o s i t i o n ’ , [ xp , yp , p o s a c t ( 3 ) , p o s a c t ( 4 ) ] ) ;
32
33
34
35
36
37
38
39
% Por d e f e c t o , s e muestra l a v i s t a p r e v i a y l a d e s c r i p c i ó n de l a
escena
% ’ O b s t a c l e s ’ a l a b r i r l a ventana .
axes ( handles . axes1 )
path=pwd ;
image=imread ( [ path , ’ \ O b s t a c l e s . j p g ’ ] ) ;
image=i m r e s i z e ( image , [ 2 5 0 , NaN ] ) ;
axis off
Construcción de plataforma software para el diseño y la simulación...
71
A CÓDIGO DE MATLAB
40
41
Carlos Sosa Marrero
imshow ( image )
s e t ( h a n d l e s . D e s c r i p t i o n , ’ s t r i n g ’ , ’ El r o b o t s e moverá por l a
e s c e n a e s q u i v a n d o l o s o b s t á c u l o s . ’ ) ;
42
43
44
h a n d l e s . output = hObject ;
g u i d a t a ( hObject , h a n d l e s ) ;
45
46
47
48
49
%−−− Las s a l i d a s de e s t a f u n c i ó n s e d e v u e l v e n por l a lı́ n e a de
comandos .
f u n c t i o n v a r a r g o u t = PreView OutputFcn ( hObject , e ve nt da ta ,
handles )
v a r a r g o u t {1} = h a n d l e s . output ;
50
51
52
53
54
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Back .
f u n c t i o n B a c k C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
55
56
57
next =1;
c l o s e PreView
58
59
60
61
62
63
64
%−−− Funnción que s e e j e c u t a cuando cambia l a s e l e c c i ó n e l e l
p a n e l Scene e n c a r g a d a de m o s t r a r l a v i s t a p r e v i a y l a
d e s c r i p c i ó n de l a e s c e n a e l e g i d a .
f u n c t i o n S c e n e S e l e c t i o n C h a n g e F c n ( hObject , ev en tda ta , h a n d l e s )
i f ( hObject==h a n d l e s . O b s t a c l e s )
f i l e n a m e= ’ O b s t a c l e s . j p g ’ ;
s e t ( h a n d l e s . D e s c r i p t i o n , ’ s t r i n g ’ , ’ El r o b o t s e moverá por l a
e s c e n a e s q u i v a n d o l o s o b s t á c u l o s . ’ ) ;
65
66
67
68
e l s e i f ( hObject==h a n d l e s . C i r c u i t )
f i l e n a m e= ’ C i r c u i t . j p g ’ ;
s e t ( h a n d l e s . D e s c r i p t i o n , ’ s t r i n g ’ , ’ El r o b o t s e g u i r á e l
c i r c u i t o y e s q u i v a r á l o s o b s t á c u l o s que podrá a ñ a d i r
posteriormente . ’ ) ;
69
70
71
72
e l s e i f ( hObject==h a n d l e s . Markers )
f i l e n a m e= ’ Markers . j p g ’ ;
s e t ( h a n d l e s . D e s c r i p t i o n , ’ s t r i n g ’ , ’ El r o b o t s e d i r i g i r á h a c i a
l a s b a l i z a s de c o l o r v e r d e y e s q u i v a r á l o s o b s t á c u l o s . ’ )
;
73
74
e l s e i f ( hObject==h a n d l e s . Empty )
Construcción de plataforma software para el diseño y la simulación...
72
A CÓDIGO DE MATLAB
f i l e n a m e= ’ Empty . j p g ’ ;
s e t ( h a n d l e s . D e s c r i p t i o n , ’ s t r i n g ’ , ’ El r o b o t s e moverá por l a
e s c e n a seg ún e l comportamiento que e s p e c i f i q u e
posteriormente . ’ ) ;
75
76
77
Carlos Sosa Marrero
end
78
79
80
81
82
83
84
axes ( handles . axes1 )
path=pwd ;
image=imread ( [ path , ’ \ ’ , f i l e n a m e ] ) ;
image=i m r e s i z e ( image , [ 2 5 0 , NaN ] ) ;
axis off
imshow ( image )
85
86
87
88
%−−− Función que s e e j e c u t a cuando s e i n t e n t a c e r r a r l a ventana
.
f u n c t i o n f i g u r e 1 C l o s e R e q u e s t F c n ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
89
90
91
92
93
94
95
96
97
98
99
% S i l a p e t i c i ó n de c i e r r e de l a ventana no e s c o n s e c u e n c i a d e l
normal t r á n s i t o d e l programa de una ventana a o t r a
% s i n o que d e r i v a d e l c l i c por p a r t e d e l u s u a r i o en e l i c o n o de
c i e r r e , s e e x i g e una c o n f i r m a c i ó n .
i f next==0
opc=q u e s t d l g ( ’ ¿Desea s a l i r d e l programa ? ’ , ’ S a l i r ’ , ’ Sı́ ’ , ’No ’ ,
’No ’ ) ;
i f strcmp ( opc , ’No ’ )
return
end
end
next =0;
d e l e t e ( hObject ) ;
Construcción de plataforma software para el diseño y la simulación...
73
A CÓDIGO DE MATLAB
A.4.
1
2
3
4
5
6
7
8
9
10
11
12
Carlos Sosa Marrero
SelectRobot.m
function varargout = SelectRobot ( varargin )
% Comienzo d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
gui Singleton = 1;
g u i S t a t e = s t r u c t ( ’ gui Name ’ ,
mfilename , . . .
’ gui Singleton ’ , gui Singleton , . . .
’ gui OpeningFcn ’ , @SelectRobot OpeningFcn , . . .
’ gui OutputFcn ’ , @SelectRobot OutputFcn , . . .
[] , ...
’ gui LayoutFcn ’ ,
’ gui Callback ’ ,
[]) ;
i f n a r g i n && i s c h a r ( v a r a r g i n { 1 } )
g u i S t a t e . gui Callback = s t r 2 f u n c ( varargin {1}) ;
end
13
14
15
16
17
18
19
i f nargout
[ varargout {1: nargout } ] = gui mainfcn ( gui State , varargin { : } )
;
else
gui mainfcn ( gui State , varargin {:}) ;
end
% Fin d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
20
21
22
23
24
25
%−−− Se e j e c u t a j u s t o a n t e s de que S e l e c t R o b o t s e haga v i s i b l e .
f u n c t i o n Sel ect Rob ot Op eni ngFc n ( hObject , ev en td at a , h a n d l e s ,
varargin )
global footprint
g l o b a l model
26
27
28
29
% Se i n i c i a l i z a n l a s v a r i a b l e s a s o c i a d a s a l a h u e l l a y e l modelo
.
f o o t p r i n t =0;
model =0;
30
31
32
33
34
35
36
37
38
% Se p o s i c i o n a l a ventana en l a e s q u i n a s u p e r i o r d e r e c h a de l a
pantalla .
s c r s z=g e t ( 0 , ’ S c r e e n S i z e ’ ) ;
p o s a c t=g e t ( g c f , ’ P o s i t i o n ’ ) ;
xr=s c r s z ( 3 )−p o s a c t ( 3 ) ;
xp=round ( xr / 2 ) ;
yr=s c r s z ( 4 )−p o s a c t ( 4 ) ;
yp=round ( yr / 2 ) ;
s e t ( g c f , ’ p o s i t i o n ’ , [ xp , yp , p o s a c t ( 3 ) , p o s a c t ( 4 ) ] ) ;
39
Construcción de plataforma software para el diseño y la simulación...
74
A CÓDIGO DE MATLAB
40
41
Carlos Sosa Marrero
h a n d l e s . output = hObject ;
g u i d a t a ( hObject , h a n d l e s ) ;
42
43
44
45
46
%−−− Las s a l i d a s de e s t a f u n c i ó n s e d e v u e l v e n por l a lı́ n e a de
comandos .
f u n c t i o n v a r a r g o u t = SelectRobot OutputFcn ( hObject , e ve nt da ta ,
handles )
v a r a r g o u t {1} = h a n d l e s . output ;
47
48
49
50
51
52
%−−− Funnción que s e e j e c u t a cuando cambia l a s e l e c c i ó n en e l
panel
% Footprint .
f u n c t i o n F o o t p r i n t S e l e c t i o n C h a n g e F c n ( hObject , ev en td at a ,
handles )
global footprint
53
54
55
i f ( hObject==h a n d l e s . C i r c u l a r )
f o o t p r i n t =1;
56
57
58
59
e l s e i f ( hObject==h a n d l e s . Quadrangular )
f o o t p r i n t =2;
end
60
61
g u i d a t a ( hObject , h a n d l e s ) ;
62
63
64
65
66
67
%−−− Funnción que s e e j e c u t a cuando cambia l a s e l e c c i ó n en e l
panel
% Model .
f u n c t i o n M o d e l S e l e c t i o n C h a n g e F c n ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l model
68
69
70
i f ( hObject==h a n d l e s . D i f f e r e n t i a l )
model =1;
71
72
73
e l s e i f ( hObject==h a n d l e s . T r i c y c l e )
model =2;
74
75
76
e l s e i f ( hObject==h a n d l e s . Q u a d r i c y c l e )
model =3;
77
78
end
79
Construcción de plataforma software para el diseño y la simulación...
75
A CÓDIGO DE MATLAB
80
Carlos Sosa Marrero
g u i d a t a ( hObject , h a n d l e s ) ;
81
82
83
84
85
86
87
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Back .
f u n c t i o n B a c k C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l vrep
global clientID
g l o b a l next
88
89
90
91
92
vrep . s i m x C l o s e S c e n e ( c l i e n t I D , vrep . s i m x o p m o d e b l o c k i n g ) ;
next =1;
c l o s e SelectRobot
SelectScene
93
94
95
96
97
98
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Cancel .
f u n c t i o n C a n c e l C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
g l o b a l vrep
global clientID
99
100
101
102
103
vrep . s i m x C l o s e S c e n e ( c l i e n t I D , vrep . s i m x o p m o d e b l o c k i n g ) ;
vrep . simxGetPingTime ( c l i e n t I D ) ;
vrep . s i m x F i n i s h ( c l i e n t I D ) ;
c l o s e SelectRobot
104
105
106
107
108
109
110
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Next e n c a r g a d a
de t r a n s i t a r a l a ventana g e n e r a d o r a de r o b o t c o r r e s p o n d i e n t e
.
f u n c t i o n N e x t C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
global footprint
g l o b a l model
111
112
113
114
115
116
117
118
119
120
121
122
s w i t c h model
case 1
switch f o o t p r i n t
case 1
next =1;
c l o s e SelectRobot
CreaDiffCir
case 2
next =1;
c l o s e SelectRobot
CreaDiffQuad
Construcción de plataforma software para el diseño y la simulación...
76
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
otherwise
e r r o r d l g ( ’ S e l e c c i o n e un t i p o de h u e l l a . ’ , ’ERROR ’
)
123
124
end
125
126
case 2
switch f o o t p r i n t
case 1
next =1;
c l o s e SelectRobot
CreaTricycleCir
case 2
next =1;
c l o s e SelectRobot
CreaTricycleQuad
otherwise
e r r o r d l g ( ’ S e l e c c i o n e un t i p o de h u e l l a . ’ , ’ERROR ’
)
end
127
128
129
130
131
132
133
134
135
136
137
138
139
140
case 3
switch f o o t p r i n t
case 1
next =1;
c l o s e SelectRobot
CreaQuadricycleCir
case 2
next =1;
c l o s e SelectRobot
CreaQuadricycleQuad
otherwise
e r r o r d l g ( ’ S e l e c c i o n e un t i p o de h u e l l a . ’ , ’ERROR ’
)
end
otherwise
e r r o r d l g ( ’ S e l e c c i o n e un modelo . ’ , ’ERROR ’ )
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
end
158
159
160
161
162
%−−− Función que s e e j e c u t a cuando s e i n t e n t a c e r r a r l a ventana
.
f u n c t i o n f i g u r e 1 C l o s e R e q u e s t F c n ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
163
Construcción de plataforma software para el diseño y la simulación...
77
A CÓDIGO DE MATLAB
164
165
166
167
168
169
170
171
172
173
Carlos Sosa Marrero
% S i l a p e t i c i ó n de c i e r r e de l a ventana no e s c o n s e c u e n c i a d e l
normal t r á n s i t o d e l programa de una ventana a o t r a
% s i n o que d e r i v a d e l c l i c por p a r t e d e l u s u a r i o en e l i c o n o de
c i e r r e , s e e x i g e una c o n f i r m a c i ó n .
i f next==0
opc=q u e s t d l g ( ’ ¿Desea s a l i r d e l programa ? ’ , ’ S a l i r ’ , ’ Sı́ ’ , ’No ’ ,
’No ’ ) ;
i f strcmp ( opc , ’No ’ )
return
end
end
next =0;
d e l e t e ( hObject ) ;
Construcción de plataforma software para el diseño y la simulación...
78
A CÓDIGO DE MATLAB
A.5.
1
2
3
4
5
6
7
8
9
10
11
12
Carlos Sosa Marrero
CreaDiffCir.m
function varargout = CreaDiffCir ( varargin )
% Comienzo d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
gui Singleton = 1;
g u i S t a t e = s t r u c t ( ’ gui Name ’ ,
mfilename , . . .
’ gui Singleton ’ , gui Singleton , . . .
’ gui OpeningFcn ’ , @CreaDiffCir OpeningFcn ,
...
’ gui OutputFcn ’ , @CreaDiffCir OutputFcn , . . .
’ gui LayoutFcn ’ ,
[] , ...
[]) ;
’ gui Callback ’ ,
i f n a r g i n && i s c h a r ( v a r a r g i n { 1 } )
g u i S t a t e . gui Callback = s t r 2 f u n c ( varargin {1}) ;
end
13
14
15
16
17
18
19
i f nargout
[ varargout {1: nargout } ] = gui mainfcn ( gui State , varargin { : } )
;
else
gui mainfcn ( gui State , varargin {:}) ;
end
% Fin d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
20
21
22
23
24
25
26
27
28
29
30
31
%−−− Se e j e c u t a j u s t o a n t e s de que C r e a D i f f C i r s e haga v i s i b l e .
f u n c t i o n C r e a D i f f C i r O p e n i n g F c n ( hObject , ev en td at a , h a n d l e s ,
varargin )
% Se p o s i c i o n a l a ventana en l a e s q u i n a s u p e r i o r d e r e c h a de l a
pantalla .
s c r s z=g e t ( 0 , ’ S c r e e n S i z e ’ ) ;
p o s a c t=g e t ( g c f , ’ P o s i t i o n ’ ) ;
xr=s c r s z ( 3 )−p o s a c t ( 3 ) ;
xp=round ( xr / 2 ) ;
yr=s c r s z ( 4 )−p o s a c t ( 4 ) ;
yp=round ( yr / 2 ) ;
s e t ( g c f , ’ p o s i t i o n ’ , [ xp , yp , p o s a c t ( 3 ) , p o s a c t ( 4 ) ] ) ;
32
33
34
h a n d l e s . output = hObject ;
g u i d a t a ( hObject , h a n d l e s ) ;
35
36
37
38
%−−− Las s a l i d a s de e s t a f u n c i ó n s e d e v u e l v e n por l a lı́ n e a de
comandos .
f u n c t i o n v a r a r g o u t = C r e a D i f f C i r O u t p u t F c n ( hObject , ev en td at a ,
Construcción de plataforma software para el diseño y la simulación...
79
A CÓDIGO DE MATLAB
39
Carlos Sosa Marrero
handles )
v a r a r g o u t {1} = h a n d l e s . output ;
40
41
42
43
44
45
46
47
48
49
50
51
%−−− Función que l e e e l v a l o r d e l r a d i o d e l c u e r p o d e l r o b o t y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n BodyRad Callback ( hObject , e ve nt da ta , h a n d l e s )
body rad=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( body rad ) | | s i g n ( body rad ) ˜=1)
e r r o r d l g ( ’ El r a d i o d e l c u e r p o d e l r o b o t ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
body rad =0;
s e t ( hObject , ’ s t r i n g ’ , body rad ) ;
end
h a n d l e s . BodyRad=body rad ;
g u i d a t a ( hObject , h a n d l e s ) ;
52
53
54
55
56
57
58
59
60
61
62
63
64
%−−− Función que l e e e l v a l o r de l a a l t u r a d e l c u e r p o d e l r o b o t
y verifica
% que s e a un número p o s i t i v o .
f u n c t i o n B o d y H e i g h t C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
b o d y h e i g h t=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( b o d y h e i g h t ) | | s i g n ( b o d y h e i g h t ) ˜=1)
e r r o r d l g ( ’ La a l t u r a d e l c u e r p o d e l r o b o t ha de s e r un
número p o s i t i v o . ’ , ’ERROR ’ )
b o d y h e i g h t =0;
s e t ( hObject , ’ s t r i n g ’ , b o d y h e i g h t ) ;
end
h a n d l e s . BodyHeight=b o d y h e i g h t ;
g u i d a t a ( hObject , h a n d l e s ) ;
65
66
67
68
69
70
71
72
73
74
75
76
%−−− Función que l e e e l v a l o r d e l r a d i o de l a s r u e d a s y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n WheelsRad Callback ( hObject , ev en td at a , h a n d l e s )
global wheels rad
w h e e l s r a d=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( w h e e l s r a d ) | | s i g n ( w h e e l s r a d ) ˜=1)
e r r o r d l g ( ’ El r a d i o de l a s r u e d a s ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
w h e e l s r a d =0;
s e t ( hObject , ’ s t r i n g ’ , w h e e l s r a d ) ;
end
h a n d l e s . WheelsRad=w h e e l s r a d ;
Construcción de plataforma software para el diseño y la simulación...
80
A CÓDIGO DE MATLAB
77
Carlos Sosa Marrero
g u i d a t a ( hObject , h a n d l e s ) ;
78
79
80
81
82
83
84
85
86
87
88
89
90
%−−− Función que l e e e l v a l o r d e l ancho de l a s r u e d a s y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n WheelsWidth Callback ( hObject , ev en tda ta , h a n d l e s )
w h e e l s w i d t h=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( w h e e l s w i d t h ) | | s i g n ( w h e e l s w i d t h ) ˜=1)
e r r o r d l g ( ’ El ancho de l a s r u e d a s ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
w h e e l s w i d t h =0;
s e t ( hObject , ’ s t r i n g ’ , w h e e l s w i d t h ) ;
end
h a n d l e s . WheelsWidth=w h e e l s w i d t h ;
g u i d a t a ( hObject , h a n d l e s ) ;
91
92
93
94
95
96
97
98
99
100
101
102
103
%−−− Función que l e e e l v a l o r de l a d i s t a n c i a e n t r e e j e s y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n A x i s D i s t C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
a x i s d i s t=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( a x i s d i s t ) | | s i g n ( a x i s d i s t ) ˜=1)
e r r o r d l g ( ’ La d i s t a n c i a e n t r e e j e s ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
a x i s d i s t =0;
s e t ( hObject , ’ s t r i n g ’ , a x i s d i s t ) ;
end
h a n d l e s . A x i s D i s t=a x i s d i s t ;
g u i d a t a ( hObject , h a n d l e s ) ;
104
105
106
107
108
109
110
111
%−−− Función que s e e j e c u t a a l p u l s a r e l botón C r e a t e .
f u n c t i o n C r e a t e C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l vrep
global clientID
global scene
g l o b a l next
112
113
114
115
116
% Se r e a l i z a n l a s c o m p r o b a c i o n e s n e c e s a r i a s en l o que r e s p e c t a a
las
% r e s t r i c c i o n e s g e o mé t r i c a s d e l r o b o t .
i f ( h a n d l e s . A x i s D i s t >2∗ h a n d l e s . BodyRad )
e r r o r d l g ( ’ La d i s t a n c i a e n t r e e j e s no puede s e r en ning ún
Construcción de plataforma software para el diseño y la simulación...
81
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
c a s o mayor que e l d i á m e t r o d e l c u e r p o d e l r o b o t . ’ , ’ERROR ’
)
117
118
119
e l s e i f ( s q r t ( ( h a n d l e s . A x i s D i s t /2+ h a n d l e s . WheelsWidth / 2 ) ˆ2+(
h a n d l e s . WheelsRad ) ˆ 2 )>h a n d l e s . BodyRad )
e r r o r d l g ( ’ Las r u e d a s s o b r e s a l e n d e l c u e r p o d e l r o b o t . Pruebe
a r e d u c i r l a d i s t a n c i a e n t r e e j e s , e l r a d i o o e l ancho
de l a s r u e d a s o aumente e l r a d i o d e l c u e r p o d e l r o b o t . ’ , ’
ERROR ’ )
120
121
122
e l s e i f ( h a n d l e s . A x i s D i s t<=h a n d l e s . WheelsWidth )
e r r o r d l g ( ’ Las r u e d a s s e superponen . Pruebe a aumentar l a
d i s t a n c i a e n t r e e j e s o r e d u z c a e l ancho de l a s r u e d a s . ’ , ’
ERROR ’ )
123
124
125
126
127
128
129
130
else
% Se c a l c u l a n l a s d i m e n s i o n e s , l a masa y l a p o s i c i ó n d e l
cuerpo del
% r o b o t a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
b o d y s i z e =[2∗ h a n d l e s . BodyRad , 2 ∗ h a n d l e s . BodyRad , h a n d l e s .
BodyHeight ] ;
b o d y d e n s i t y =100;
body mass=b o d y d e n s i t y ∗ p i ∗ h a n d l e s . BodyRadˆ2∗ h a n d l e s .
BodyHeight ;
body pos = [ 0 , 0 , 2 ∗ h a n d l e s . WheelsRad+h a n d l e s . BodyHeight / 2 ] ;
131
132
133
134
135
136
137
138
% Se c a l c u l a n l a s d i m e n s i o n e s , l a masa y l a p o s i c i ó n de l a s
ruedas
% a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
w h e e l s s i z e =[2∗ h a n d l e s . WheelsRad , 2 ∗ h a n d l e s . WheelsRad , h a n d l e s
. WheelsWidth ] ;
w h e e l s d e n s i t y =500;
w h e e l s m a s s=w h e e l s d e n s i t y ∗ p i ∗ h a n d l e s . WheelsRad ˆ2∗ h a n d l e s .
WheelsWidth ;
l e f t w h e e l p o s =[− h a n d l e s . A x i s D i s t / 2 , 0 , h a n d l e s . WheelsRad ] ;
r i g h t w h e e l p o s =[ h a n d l e s . A x i s D i s t / 2 , 0 , h a n d l e s . WheelsRad ] ;
139
140
141
142
143
% Se c a l c u l a n e l rango y l a p o s i c i ó n d e l s e n s o r de
proximidad
% a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
p r o x s e n s o r r a n g e=h a n d l e s . BodyRad/ c o s ( p i / 4 ) ;
p r o x s e n s o r p o s =[0 , h a n d l e s . BodyRad , 2 ∗ h a n d l e s . WheelsRad+
h a n d l e s . BodyHeight / 2 ] ;
144
145
% Se c a l c u l a l a p o s i c i ó n de l o s s e n s o r e s de f u e r z a
Construcción de plataforma software para el diseño y la simulación...
82
A CÓDIGO DE MATLAB
146
147
148
Carlos Sosa Marrero
% a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
f r o n t f o r c e s e n s o r p o s =[0 , h a n d l e s . BodyRad−h a n d l e s . WheelsRad
, 2 ∗ h a n d l e s . WheelsRad ] ;
b a c k f o r c e s e n s o r p o s =[0,− h a n d l e s . BodyRad+h a n d l e s . WheelsRad
, 2 ∗ h a n d l e s . WheelsRad ] ;
149
150
151
152
153
154
155
% Se c a l c u l a n l a s d i m e n s i o n e s , l a masa y l a p o s i c i ó n de l a s
esferas
% d e s l i z a n t e s a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l
usuario .
s l i d e r s s i z e =[2∗ h a n d l e s . WheelsRad , 2 ∗ h a n d l e s . WheelsRad , 2 ∗
h a n d l e s . WheelsRad ] ;
s l i d e r s m a s s =0.01;
f r o n t s l i d e r p o s =[0 , h a n d l e s . BodyRad−h a n d l e s . WheelsRad ,
h a n d l e s . WheelsRad ] ;
b a c k s l i d e r p o s =[0,− h a n d l e s . BodyRad+h a n d l e s . WheelsRad ,
h a n d l e s . WheelsRad ] ;
156
157
158
159
160
161
162
% Se c a l c u l a n e l ’ f a r c l i p p i n g plane ’ y l a p o s i c i ó n de l o s
s e n s o r e s de
% v i s i ó n a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
f a r c l i p p i n g p l a n e =3∗ h a n d l e s . WheelsRad ;
l e f t v i s i o n s e n s o r p o s =[ −0.1 , s q r t ( h a n d l e s . BodyRad ˆ2 −0.1ˆ2)
+0.005 ,2∗ h a n d l e s . WheelsRad ] ;
r i g h t v i s i o n s e n s o r p o s = [ 0 . 1 , s q r t ( h a n d l e s . BodyRad ˆ2 −0.1ˆ2)
+0.005 ,2∗ h a n d l e s . WheelsRad ] ;
m i d d l e v i s i o n s e n s o r p o s =[0 , h a n d l e s . BodyRad , 2 ∗ h a n d l e s .
WheelsRad+h a n d l e s . BodyHeight / 2 ] ;
163
164
vrep . s i m x S t a r t S i m u l a t i o n ( c l i e n t I D , vrep . simx opmode oneshot ) ;
165
166
167
168
169
170
171
172
% Se l l a m a a l a f u n c i ó n ’ C r e a t e D i f f ’ d e l s c r i p t Lua ’ c r e a t e ’
% e n c a r g a d a de c r e a r e l r o b o t de modelo d i f e r e n c i a l en l a
escena .
vrep . s i m x C a l l S c r i p t F u n c t i o n ( c l i e n t I D , ’ c r e a t e ’ , vrep .
sim scripttype childscript , ’ CreateDiff function ’ , 2 , . . .
[ b o d y s i z e , body mass , body pos , w h e e l s s i z e , wheels mass ,
left wheel pos , right wheel pos , prox sensor range , . . .
prox sensor pos , front force sensor pos ,
back force sensor pos , sliders size , sliders mass ,
front slider pos , . . .
back slider pos , far clipping plane ,
left vision sensor pos , right vision sensor pos ,
middle vision sensor pos ] , . . .
[ ] , [ ] , vrep . s i m x o p m o d e b l o c k i n g ) ;
Construcción de plataforma software para el diseño y la simulación...
83
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
173
% Se c i e r r a l a ventana y s e c o n t i n ú a con l a de g e n e r a c i ó n de
% o b s t á c u l o s s i l a e s c e n a en l a que s e t r a b a j a e s ’ O b s t a c l e s
’ o
% ’ C i r c u i t ’ o con l a s e l e c c i ó n d e l ı́tem a a ñ a d i r en
cualquier otro caso .
next =1;
close CreaDiffCir
i f ( s c e n e==1 | | s c e n e ==2)
CreaObs
else
SelectItem
end
174
175
176
177
178
179
180
181
182
183
184
end
185
186
187
188
189
190
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Back .
f u n c t i o n B a c k C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
close CreaDiffCir
SelectRobot
191
192
193
194
195
196
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Cancel .
f u n c t i o n C a n c e l C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
g l o b a l vrep
global clientID
197
198
199
200
201
vrep . s i m x C l o s e S c e n e ( c l i e n t I D , vrep . s i m x o p m o d e b l o c k i n g ) ;
vrep . simxGetPingTime ( c l i e n t I D ) ;
vrep . s i m x F i n i s h ( c l i e n t I D ) ;
close CreaDiffCir
202
203
204
205
206
%−−− Función que s e e j e c u t a cuando s e i n t e n t a c e r r a r l a ventana
.
f u n c t i o n f i g u r e 1 C l o s e R e q u e s t F c n ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
207
208
209
210
211
% S i l a p e t i c i ó n de c i e r r e de l a ventana no e s c o n s e c u e n c i a d e l
normal t r á n s i t o d e l programa de una ventana a o t r a
% s i n o que d e r i v a d e l c l i c por p a r t e d e l u s u a r i o en e l i c o n o de
c i e r r e , s e e x i g e una c o n f i r m a c i ó n .
i f next==0
opc=q u e s t d l g ( ’ ¿Desea s a l i r d e l programa ? ’ , ’ S a l i r ’ , ’ Sı́ ’ , ’No ’ ,
’No ’ ) ;
Construcción de plataforma software para el diseño y la simulación...
84
A CÓDIGO DE MATLAB
212
213
214
215
216
217
Carlos Sosa Marrero
i f strcmp ( opc , ’No ’ )
return
end
end
next =0;
d e l e t e ( hObject ) ;
Construcción de plataforma software para el diseño y la simulación...
85
A CÓDIGO DE MATLAB
A.6.
1
2
3
4
5
6
7
8
9
10
11
12
Carlos Sosa Marrero
CreaDiffQuad.m
f u n c t i o n v a r a r g o u t = CreaDiffQuad ( v a r a r g i n )
% Comienzo d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
gui Singleton = 1;
g u i S t a t e = s t r u c t ( ’ gui Name ’ ,
mfilename , . . .
’ gui Singleton ’ , gui Singleton , . . .
’ gui OpeningFcn ’ , @CreaDiffQuad OpeningFcn ,
...
’ gui OutputFcn ’ , @CreaDiffQuad OutputFcn ,
...
[] , ...
’ gui LayoutFcn ’ ,
’ gui Callback ’ ,
[]) ;
i f n a r g i n && i s c h a r ( v a r a r g i n { 1 } )
g u i S t a t e . gui Callback = s t r 2 f u n c ( varargin {1}) ;
end
13
14
15
16
17
18
19
i f nargout
[ varargout {1: nargout } ] = gui mainfcn ( gui State , varargin { : } )
;
else
gui mainfcn ( gui State , varargin {:}) ;
end
% Fin d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
20
21
22
23
24
25
26
27
28
29
30
31
%−−− Se e j e c u t a j u s t o a n t e s de que CreaDiffQuad s e haga v i s i b l e
.
f u n c t i o n CreaDiffQuad OpeningFcn ( hObject , e ve nt da ta , h a n d l e s ,
varargin )
% Se p o s i c i o n a l a ventana en l a e s q u i n a s u p e r i o r d e r e c h a de l a
pantalla .
s c r s z=g e t ( 0 , ’ S c r e e n S i z e ’ ) ;
p o s a c t=g e t ( g c f , ’ P o s i t i o n ’ ) ;
xr=s c r s z ( 3 )−p o s a c t ( 3 ) ;
xp=round ( xr / 2 ) ;
yr=s c r s z ( 4 )−p o s a c t ( 4 ) ;
yp=round ( yr / 2 ) ;
s e t ( g c f , ’ p o s i t i o n ’ , [ xp , yp , p o s a c t ( 3 ) , p o s a c t ( 4 ) ] ) ;
32
33
34
h a n d l e s . output = hObject ;
g u i d a t a ( hObject , h a n d l e s ) ;
35
36
37
%−−− Las s a l i d a s de e s t a f u n c i ó n s e d e v u e l v e n por l a lı́ n e a de
Construcción de plataforma software para el diseño y la simulación...
86
A CÓDIGO DE MATLAB
38
39
Carlos Sosa Marrero
comandos .
f u n c t i o n v a r a r g o u t = CreaDiffQuad OutputFcn ( hObject , e ve nt da ta ,
handles )
v a r a r g o u t {1} = h a n d l e s . output ;
40
41
42
43
44
45
46
47
48
49
50
51
52
%−−− Función que l e e e l v a l o r d e l l a r g o d e l c u e r p o d e l r o b o t y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n BodyLength Callback ( hObject , ev e ntd at a , h a n d l e s )
b o d y l e n g t h=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( b o d y l e n g t h ) | | s i g n ( b o d y l e n g t h ) ˜=1)
e r r o r d l g ( ’ El l a r g o d e l c u e r p o d e l r o b o t ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
b o d y l e n g t h =0;
s e t ( hObject , ’ s t r i n g ’ , b o d y l e n g t h ) ;
end
h a n d l e s . BodyLength=b o d y l e n g t h ;
g u i d a t a ( hObject , h a n d l e s ) ;
53
54
55
56
57
58
59
60
61
62
63
64
65
%−−− Función que l e e e l v a l o r d e l ancho d e l c u e r p o d e l r o b o t y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n BodyWidth Callback ( hObject , ev en td at a , h a n d l e s )
body width=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( body width ) | | s i g n ( body width ) ˜=1)
e r r o r d l g ( ’ El ancho d e l c u e r p o d e l r o b o t ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
body width =0;
s e t ( hObject , ’ s t r i n g ’ , body width ) ;
end
h a n d l e s . BodyWidth=body width ;
g u i d a t a ( hObject , h a n d l e s ) ;
66
67
68
69
70
71
72
73
74
%−−− Función que l e e e l v a l o r de l a a l t u r a d e l c u e r p o d e l r o b o t
y verifica
% que s e a un número p o s i t i v o .
f u n c t i o n B o d y H e i g h t C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
b o d y h e i g h t=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( b o d y h e i g h t ) | | s i g n ( b o d y h e i g h t ) ˜=1)
e r r o r d l g ( ’ La a l t u r a d e l c u e r p o d e l r o b o t ha de s e r un
número p o s i t i v o . ’ , ’ERROR ’ )
b o d y h e i g h t =0;
Construcción de plataforma software para el diseño y la simulación...
87
A CÓDIGO DE MATLAB
75
76
77
78
Carlos Sosa Marrero
s e t ( hObject , ’ s t r i n g ’ , b o d y h e i g h t ) ;
end
h a n d l e s . BodyHeight=b o d y h e i g h t ;
g u i d a t a ( hObject , h a n d l e s ) ;
79
80
81
82
83
84
85
86
87
88
89
90
91
92
%−−− Función que l e e e l v a l o r d e l r a d i o de l a s r u e d a s y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n WheelsRad Callback ( hObject , ev en td at a , h a n d l e s )
global wheels rad
w h e e l s r a d=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( w h e e l s r a d ) | | s i g n ( w h e e l s r a d ) ˜=1)
e r r o r d l g ( ’ El r a d i o de l a s r u e d a s ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
w h e e l s r a d =0;
s e t ( hObject , ’ s t r i n g ’ , w h e e l s r a d ) ;
end
h a n d l e s . WheelsRad=w h e e l s r a d ;
g u i d a t a ( hObject , h a n d l e s ) ;
93
94
95
96
97
98
99
100
101
102
103
104
105
%−−− Función que l e e e l v a l o r d e l ancho de l a s r u e d a s y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n WheelsWidth Callback ( hObject , ev en tda ta , h a n d l e s )
w h e e l s w i d t h=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( w h e e l s w i d t h ) | | s i g n ( w h e e l s w i d t h ) ˜=1)
e r r o r d l g ( ’ El ancho de l a s r u e d a s ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
w h e e l s w i d t h =0;
s e t ( hObject , ’ s t r i n g ’ , w h e e l s w i d t h ) ;
end
h a n d l e s . WheelsWidth=w h e e l s w i d t h ;
g u i d a t a ( hObject , h a n d l e s ) ;
106
107
108
109
110
111
112
113
%−−− Función que l e e e l v a l o r de l a d i s t a n c i a e n t r e e j e s y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n A x i s D i s t C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
a x i s d i s t=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( a x i s d i s t ) | | s i g n ( a x i s d i s t ) ˜=1)
e r r o r d l g ( ’ La d i s t a n c i a e n t r e e j e s ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
Construcción de plataforma software para el diseño y la simulación...
88
A CÓDIGO DE MATLAB
a x i s d i s t =0;
s e t ( hObject , ’ s t r i n g ’ , a x i s d i s t ) ;
114
115
116
117
118
Carlos Sosa Marrero
end
h a n d l e s . A x i s D i s t=a x i s d i s t ;
g u i d a t a ( hObject , h a n d l e s ) ;
119
120
121
122
123
124
125
126
%−−− Función que s e e j e c u t a a l p u l s a r e l botón C r e a t e .
f u n c t i o n C r e a t e C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l vrep
global clientID
global scene
g l o b a l next
127
128
129
% Se r e a l i z a n l a s c o m p r o b a c i o n e s n e c e s a r i a s en l o que r e s p e c t a a
las
% r e s t r i c c i o n e s g e o mé t r i c a s d e l r o b o t .
130
131
132
i f ( h a n d l e s . A x i s D i s t+h a n d l e s . WheelsWidth>h a n d l e s . BodyWidth )
e r r o r d l g ( ’ Las r u e d a s s o b r e s a l e n por l o s l a t e r a l e s d e l c u e r p o
d e l r o b o t . Pruebe a r e d u c i r l a d i s t a n c i a e n t r e e j e s o e l
ancho de l a s r u e d a s o aumente e l ancho d e l c u e r p o d e l
r o b o t . ’ , ’ERROR ’ )
133
134
135
e l s e i f ( h a n d l e s . WheelsRad>h a n d l e s . BodyLength / 2 )
e r r o r d l g ( ’ Las r u e d a s s o b r e s a l e n por l a s p a r t e s d e l a n t e r a y
t r a s e r a d e l c u e r p o d e l r o b o t . Pruebe a r e d u c i r e l r a d i o
de l a s r u e d a s o aumente e l l a r g o d e l c u e r p o d e l r o b o t . ’ , ’
ERROR ’ )
136
137
138
e l s e i f ( h a n d l e s . A x i s D i s t<=h a n d l e s . WheelsWidth )
e r r o r d l g ( ’ Las r u e d a s s e superponen . Pruebe a aumentar l a
d i s t a n c i a e n t r e e j e s o r e d u z c a e l ancho de l a s r u e d a s . ’ , ’
ERROR ’ )
139
140
141
142
143
144
145
146
else
% Se c a l c u l a n l a s d i m e n s i o n e s , l a masa y l a p o s i c i ó n d e l
cuerpo del
% r o b o t a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
b o d y s i z e =[ h a n d l e s . BodyWidth , h a n d l e s . BodyLength , h a n d l e s .
BodyHeight ] ;
b o d y d e n s i t y =100;
body mass=b o d y d e n s i t y ∗ h a n d l e s . BodyLength ∗ h a n d l e s . BodyWidth∗
h a n d l e s . BodyHeight ;
body pos = [ 0 , 0 , 2 ∗ h a n d l e s . WheelsRad+h a n d l e s . BodyHeight / 2 ] ;
Construcción de plataforma software para el diseño y la simulación...
89
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
147
148
149
150
151
152
153
154
% Se c a l c u l a n l a s d i m e n s i o n e s , l a masa y l a p o s i c i ó n de l a s
ruedas
% a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
w h e e l s s i z e =[2∗ h a n d l e s . WheelsRad , 2 ∗ h a n d l e s . WheelsRad , h a n d l e s
. WheelsWidth ] ;
w h e e l s d e n s i t y =50;
w h e e l s m a s s=w h e e l s d e n s i t y ∗ p i ∗ h a n d l e s . WheelsRad ˆ2∗ h a n d l e s .
WheelsWidth ;
l e f t w h e e l p o s =[− h a n d l e s . A x i s D i s t / 2 , 0 , h a n d l e s . WheelsRad ] ;
r i g h t w h e e l p o s =[ h a n d l e s . A x i s D i s t / 2 , 0 , h a n d l e s . WheelsRad ] ;
155
156
157
158
159
% Se c a l c u l a n e l rango y l a p o s i c i ó n d e l s e n s o r de
proximidad
% a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
p r o x s e n s o r r a n g e =( h a n d l e s . BodyWidth / 2 ) / c o s ( p i / 4 ) ;
p r o x s e n s o r p o s =[0 , h a n d l e s . BodyLength / 2 , 2 ∗ h a n d l e s . WheelsRad+
h a n d l e s . BodyHeight / 2 ] ;
160
161
162
163
164
% Se c a l c u l a l a p o s i c i ó n de l o s s e n s o r e s de f u e r z a
% a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
f r o n t f o r c e s e n s o r p o s =[0 , h a n d l e s . BodyLength/2− h a n d l e s .
WheelsRad , 2 ∗ h a n d l e s . WheelsRad ] ;
b a c k f o r c e s e n s o r p o s =[0,− h a n d l e s . BodyLength/2+ h a n d l e s .
WheelsRad , 2 ∗ h a n d l e s . WheelsRad ] ;
165
166
167
168
169
170
171
% Se c a l c u l a n l a s d i m e n s i o n e s , l a masa y l a p o s i c i ó n de l a s
esferas
% d e s l i z a n t e s a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l
usuario .
s l i d e r s s i z e =[2∗ h a n d l e s . WheelsRad , 2 ∗ h a n d l e s . WheelsRad , 2 ∗
h a n d l e s . WheelsRad ] ;
s l i d e r s m a s s =0.01;
f r o n t s l i d e r p o s =[0 , h a n d l e s . BodyLength/2− h a n d l e s . WheelsRad ,
h a n d l e s . WheelsRad ] ;
b a c k s l i d e r p o s =[0,− h a n d l e s . BodyLength/2+ h a n d l e s . WheelsRad ,
h a n d l e s . WheelsRad ] ;
172
173
174
175
176
177
% Se c a l c u l a n e l ’ f a r c l i p p i n g plane ’ y l a p o s i c i ó n de l o s
s e n s o r e s de
% v i s i ó n a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
f a r c l i p p i n g p l a n e =3∗ h a n d l e s . WheelsRad ;
l e f t v i s i o n s e n s o r p o s =[ −0.1 , h a n d l e s . BodyLength /2+0.005 ,2∗
h a n d l e s . WheelsRad ] ;
r i g h t v i s i o n s e n s o r p o s = [ 0 . 1 , h a n d l e s . BodyLength /2+0.005 ,2∗
Construcción de plataforma software para el diseño y la simulación...
90
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
h a n d l e s . WheelsRad ] ;
m i d d l e v i s i o n s e n s o r p o s =[0 , h a n d l e s . BodyLength / 2 , 2 ∗ h a n d l e s .
WheelsRad+h a n d l e s . BodyHeight / 2 ] ;
178
179
vrep . s i m x S t a r t S i m u l a t i o n ( c l i e n t I D , vrep . simx opmode oneshot ) ;
180
181
% Se l l a m a a l a f u n c i ó n ’ C r e a t e D i f f ’ d e l s c r i p t Lua ’ c r e a t e ’
% e n c a r g a d a de c r e a r e l r o b o t de modelo d i f e r e n c i a l en l a
escena .
vrep . s i m x C a l l S c r i p t F u n c t i o n ( c l i e n t I D , ’ c r e a t e ’ , vrep .
sim scripttype childscript , ’ CreateDiff function ’ , 0 , . . .
[ b o d y s i z e , body mass , body pos , w h e e l s s i z e , wheels mass ,
left wheel pos , right wheel pos , prox sensor range , . . .
prox sensor pos , front force sensor pos ,
back force sensor pos , sliders size , sliders mass ,
front slider pos , . . .
back slider pos , far clipping plane ,
left vision sensor pos , right vision sensor pos ,
middle vision sensor pos ] , . . .
[ ] , [ ] , vrep . s i m x o p m o d e b l o c k i n g ) ;
182
183
184
185
186
187
188
189
% Se c i e r r a l a ventana y s e c o n t i n ú a con l a de g e n e r a c i ó n de
% o b s t á c u l o s s i l a e s c e n a en l a que s e t r a b a j a e s ’ O b s t a c l e s
’ o
% ’ C i r c u i t ’ o con l a s e l e c c i ó n d e l ı́tem a a ñ a d i r en
cualquier otro caso .
next =1;
c l o s e CreaDiffQuad
i f ( s c e n e==1 | | s c e n e ==2)
CreaObs
else
SelectItem
end
190
191
192
193
194
195
196
197
198
199
200
end
201
202
203
204
205
206
207
208
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Back .
f u n c t i o n B a c k C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
next =1;
c l o s e CreaDiffQuad
SelectRobot
209
210
211
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Cancel .
Construcción de plataforma software para el diseño y la simulación...
91
A CÓDIGO DE MATLAB
212
213
214
Carlos Sosa Marrero
f u n c t i o n C a n c e l C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
g l o b a l vrep
global clientID
215
216
217
218
219
vrep . s i m x C l o s e S c e n e ( c l i e n t I D , vrep . s i m x o p m o d e b l o c k i n g ) ;
vrep . simxGetPingTime ( c l i e n t I D ) ;
vrep . s i m x F i n i s h ( c l i e n t I D ) ;
c l o s e CreaDiffQuad
220
221
222
223
224
%−−− Función que s e e j e c u t a cuando s e i n t e n t a c e r r a r l a ventana
.
f u n c t i o n f i g u r e 1 C l o s e R e q u e s t F c n ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
225
226
227
228
229
230
231
232
233
234
235
% S i l a p e t i c i ó n de c i e r r e de l a ventana no e s c o n s e c u e n c i a d e l
normal t r á n s i t o d e l programa de una ventana a o t r a
% s i n o que d e r i v a d e l c l i c por p a r t e d e l u s u a r i o en e l i c o n o de
c i e r r e , s e e x i g e una c o n f i r m a c i ó n .
i f next==0
opc=q u e s t d l g ( ’ ¿Desea s a l i r d e l programa ? ’ , ’ S a l i r ’ , ’ Sı́ ’ , ’No ’ ,
’No ’ ) ;
i f strcmp ( opc , ’No ’ )
return
end
end
next =0;
d e l e t e ( hObject ) ;
Construcción de plataforma software para el diseño y la simulación...
92
A CÓDIGO DE MATLAB
A.7.
1
2
3
4
5
6
7
8
9
10
11
12
Carlos Sosa Marrero
CreaTricycleCir.m
function varargout = CreaTricycleCir ( varargin )
% Comienzo d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
gui Singleton = 1;
g u i S t a t e = s t r u c t ( ’ gui Name ’ ,
mfilename , . . .
’ gui Singleton ’ , gui Singleton , . . .
’ gui OpeningFcn ’ , @CreaTricycleCir OpeningFcn , . . .
’ gui OutputFcn ’ , @CreaTricycleCir OutputFcn , . . .
[] , ...
’ gui LayoutFcn ’ ,
’ gui Callback ’ ,
[]) ;
i f n a r g i n && i s c h a r ( v a r a r g i n { 1 } )
g u i S t a t e . gui Callback = s t r 2 f u n c ( varargin {1}) ;
end
13
14
15
16
17
18
19
i f nargout
[ varargout {1: nargout } ] = gui mainfcn ( gui State , varargin { : } )
;
else
gui mainfcn ( gui State , varargin {:}) ;
end
% Fin d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
20
21
22
23
24
25
26
27
28
29
30
31
%−−− Se e j e c u t a j u s t o a n t e s de que C r e a T r i c y c l e C i r s e haga
visible .
f u n c t i o n C r e a T r i c y c l e C i r O p e n i n g F c n ( hObject , e ve nt da ta , h a n d l e s ,
varargin )
% Se p o s i c i o n a l a ventana en l a e s q u i n a s u p e r i o r d e r e c h a de l a
pantalla .
s c r s z=g e t ( 0 , ’ S c r e e n S i z e ’ ) ;
p o s a c t=g e t ( g c f , ’ P o s i t i o n ’ ) ;
xr=s c r s z ( 3 )−p o s a c t ( 3 ) ;
xp=round ( xr / 2 ) ;
yr=s c r s z ( 4 )−p o s a c t ( 4 ) ;
yp=round ( yr / 2 ) ;
s e t ( g c f , ’ p o s i t i o n ’ , [ xp , yp , p o s a c t ( 3 ) , p o s a c t ( 4 ) ] ) ;
32
33
34
h a n d l e s . output = hObject ;
g u i d a t a ( hObject , h a n d l e s ) ;
35
36
37
38
%−−− Las s a l i d a s de e s t a f u n c i ó n s e d e v u e l v e n por l a lı́ n e a de
comandos .
f u n c t i o n v a r a r g o u t = C r e a T r i c y c l e C i r O u t p u t F c n ( hObject ,
Construcción de plataforma software para el diseño y la simulación...
93
A CÓDIGO DE MATLAB
39
Carlos Sosa Marrero
e ve nt dat a , h a n d l e s )
v a r a r g o u t {1} = h a n d l e s . output ;
40
41
42
43
44
45
46
47
48
49
50
51
%−−− Función que l e e e l v a l o r d e l r a d i o d e l c u e r p o d e l r o b o t y
v e r i f i c a que s e a un número p o s i t i v o .
f u n c t i o n BodyRad Callback ( hObject , e ve nt da ta , h a n d l e s )
body rad=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( body rad ) | | s i g n ( body rad ) ˜=1)
e r r o r d l g ( ’ El r a d i o d e l c u e r p o d e l r o b o t ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
body rad =0;
s e t ( hObject , ’ s t r i n g ’ , body rad ) ;
end
h a n d l e s . BodyRad=body rad ;
g u i d a t a ( hObject , h a n d l e s ) ;
52
53
54
55
56
57
58
59
60
61
62
63
%−−− Función que l e e e l v a l o r de l a a l t u r a d e l c u e r p o d e l r o b o t
y v e r i f i c a que s e a un número p o s i t i v o .
f u n c t i o n B o d y H e i g h t C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
b o d y h e i g h t=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( b o d y h e i g h t ) | | s i g n ( b o d y h e i g h t ) ˜=1)
e r r o r d l g ( ’ El a l t o d e l c u e r p o d e l r o b o t ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
b o d y h e i g h t =0;
s e t ( hObject , ’ s t r i n g ’ , b o d y h e i g h t ) ;
end
h a n d l e s . BodyHeight=b o d y h e i g h t ;
g u i d a t a ( hObject , h a n d l e s ) ;
64
65
66
67
68
69
70
71
72
73
74
75
76
%−−− Función que l e e e l v a l o r d e l r a d i o de l a s r u e d a s y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n WheelsRad Callback ( hObject , ev en td at a , h a n d l e s )
global wheels rad
w h e e l s r a d=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( w h e e l s r a d ) | | s i g n ( w h e e l s r a d ) ˜=1)
e r r o r d l g ( ’ El r a d i o de l a s r u e d a s ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
w h e e l s r a d =0;
s e t ( hObject , ’ s t r i n g ’ , w h e e l s r a d ) ;
end
h a n d l e s . WheelsRad=w h e e l s r a d ;
Construcción de plataforma software para el diseño y la simulación...
94
A CÓDIGO DE MATLAB
77
Carlos Sosa Marrero
g u i d a t a ( hObject , h a n d l e s ) ;
78
79
80
81
82
83
84
85
86
87
88
89
90
%−−− Función que l e e e l v a l o r d e l ancho de l a s r u e d a s y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n WheelsWidth Callback ( hObject , ev en tda ta , h a n d l e s )
w h e e l s w i d t h=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( w h e e l s w i d t h ) | | s i g n ( w h e e l s w i d t h ) ˜=1)
e r r o r d l g ( ’ El ancho de l a s r u e d a s ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
w h e e l s w i d t h =0;
s e t ( hObject , ’ s t r i n g ’ , w h e e l s w i d t h ) ;
end
h a n d l e s . WheelsWidth=w h e e l s w i d t h ;
g u i d a t a ( hObject , h a n d l e s ) ;
91
92
93
94
95
96
97
98
99
100
101
102
103
%−−− Función que l e e e l v a l o r de l a d i s t a n c i a e n t r e l o s e j e s
i z q u i e r d o −d e r e c h o y v e r i f i c a
% que s e a un número p o s i t i v o .
f u n c t i o n A x i s D i s t L R C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
a x i s d i s t l r =s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( a x i s d i s t l r ) | | s i g n ( a x i s d i s t l r ) ˜=1)
e r r o r d l g ( ’ La d i s t a n c i a e n t r e l o s e j e s i z q u i e r d o y d e r e c h o ha
de s e r un número p o s i t i v o . ’ , ’ERROR ’ )
a x i s d i s t l r =0;
s e t ( hObject , ’ s t r i n g ’ , a x i s d i s t l r ) ;
end
h a n d l e s . AxisDistLR= a x i s d i s t l r ;
g u i d a t a ( hObject , h a n d l e s ) ;
104
105
106
107
108
109
110
111
112
113
114
115
%−−− Función que l e e e l v a l o r de l a d i s t a n c i a e n t r e l o s e j e s
d e l a n t e r o −t r a s e r o y v e r i f i c a
% que s e a un número p o s i t i v o .
f u n c t i o n A x i s D i s t F R C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
a x i s d i s t f r =s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( a x i s d i s t f r ) | | s i g n ( a x i s d i s t f r ) ˜=1)
e r r o r d l g ( ’ La d i s t a n c i a e n t r e l o s e j e s d e l a n t e r o y t r a s e r o ha
de s e r un número p o s i t i v o . ’ , ’ERROR ’ )
a x i s d i s t f r =0;
s e t ( hObject , ’ s t r i n g ’ , a x i s d i s t f r ) ;
end
h a n d l e s . AxisDistFR= a x i s d i s t f r ;
Construcción de plataforma software para el diseño y la simulación...
95
A CÓDIGO DE MATLAB
116
Carlos Sosa Marrero
g u i d a t a ( hObject , h a n d l e s ) ;
117
118
119
120
121
122
123
124
%−−− Función que s e e j e c u t a a l p u l s a r e l botón C r e a t e .
f u n c t i o n C r e a t e C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l vrep
global clientID
global scene
g l o b a l next
125
126
127
128
129
% Se r e a l i z a n l a s c o m p r o b a c i o n e s n e c e s a r i a s en l o que r e s p e c t a a
las
% r e s t r i c c i o n e s g e o mé t r i c a s d e l r o b o t .
i f ( h a n d l e s . AxisDistLR >2∗ h a n d l e s . BodyRad )
e r r o r d l g ( ’ La d i s t a n c i a e n t r e l o s e j e s i z q u i e r d o y d e r e c h o no
puede s e r en ning ún c a s o mayor que e l d i á m e t r o d e l
c u e r p o d e l r o b o t . ’ , ’ERROR ’ )
130
131
132
e l s e i f ( h a n d l e s . AxisDistFR >2∗ h a n d l e s . BodyRad )
e r r o r d l g ( ’ La d i s t a n c i a e n t r e l o s e j e s d e l a n t e r o y t r a s e r o no
puede s e r en ning ún c a s o mayor que e l d i á m e t r o d e l
c u e r p o d e l r o b o t . ’ , ’ERROR ’ )
133
134
135
e l s e i f ( s q r t ( ( h a n d l e s . AxisDistLR/2+ h a n d l e s . WheelsWidth / 2 ) ˆ2+(
h a n d l e s . AxisDistFR/2+ h a n d l e s . WheelsRad ) ˆ 2 )>h a n d l e s . BodyRad )
e r r o r d l g ( ’ Las r u e d a s t r a s e r a s s o b r e s a l e n d e l c u e r p o d e l
r o b o t . Pruebe a r e d u c i r l a s d i s t a n c i a s e n t r e e j e s , e l
r a d i o o e l ancho de l a s r u e d a s o aumente e l r a d i o d e l
c u e r p o d e l r o b o t . ’ , ’ERROR ’ )
136
137
138
139
140
141
e l s e i f ( s q r t ( c o s ( atan ( h a n d l e s . WheelsRad / ( h a n d l e s . WheelsWidth / 2 ) )+
min ( p i / 6 , atan ( ( h a n d l e s . WheelsWidth / 2 ) / h a n d l e s . WheelsRad ) ) ) . . .
∗ s q r t ( ( h a n d l e s . WheelsWidth / 2 ) ˆ2+ h a n d l e s . WheelsRad ˆ 2 ) )
ˆ2+...
( c o s ( atan ( ( h a n d l e s . WheelsWidth / 2 ) / h a n d l e s . WheelsRad )−min
( p i / 6 , atan ( ( h a n d l e s . WheelsWidth / 2 ) / h a n d l e s . WheelsRad )
)) ...
∗ s q r t ( ( h a n d l e s . WheelsWidth / 2 ) ˆ2+ h a n d l e s . WheelsRad ˆ 2 )+
h a n d l e s . AxisDistFR / 2 ) ˆ2> h a n d l e s . BodyRad )
e r r o r d l g ( ’ La rueda d e l a n t e r a s o b r e s a l e a l g i r a r d e l c u e r p o
d e l r o b o t . Pruebe a r e d u c i r l a d i s t a n c i a e n t r e l o s e j e s
d e l a n t e r o y t r a s e r o , e l r a d i o o e l ancho de l a s r u e d a s o
aumente e l r a d i o d e l c u e r p o d e l r o b o t . ’ , ’ERROR ’ )
142
143
e l s e i f ( h a n d l e s . AxisDistLR<=h a n d l e s . WheelsWidth )
Construcción de plataforma software para el diseño y la simulación...
96
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
e r r o r d l g ( ’ Las r u e d a s t r a s e r a s s e superponen . Pruebe a
aumentar l a d i s t a n c i a e n t r e l o s e j e s i z q u i e r d o y d e r e c h o
o r e d u z c a e l ancho de l a s r u e d a s . ’ , ’ERROR ’ )
144
145
146
147
148
149
150
151
152
e l s e i f ( h a n d l e s . AxisDistFR−h a n d l e s . WheelsRad < = . . .
s q r t ( ( h a n d l e s . WheelsWidth / 2 ) ˆ2+ h a n d l e s . WheelsRad ˆ 2 )
&&...
h a n d l e s . AxisDistLR/2− h a n d l e s . WheelsWidth /2 <=...
s q r t ( ( h a n d l e s . WheelsWidth / 2 ) ˆ2+ h a n d l e s . WheelsRad ˆ 2 )
&&...
h a n d l e s . AxisDistLR/2− h a n d l e s . WheelsWidth /2 <=...
c o s ( atan ( h a n d l e s . WheelsRad / ( h a n d l e s . WheelsWidth / 2 ) )−p i
/ 6 ) ∗ s q r t ( ( h a n d l e s . WheelsWidth / 2 ) ˆ2+ h a n d l e s . WheelsRad
ˆ2) )
e r r o r d l g ( ’ La rueda d e l a n t e r a s e s u p e r p o n e a l g i r a r con l a s
t r a s e r a s . Pruebe a aumentar l a s d i s t a n c i a s e n t r e e j e s o
r e d u z c a e l r a d i o o e l ancho de l a s r u e d a s . ’ , ’ERROR ’ )
153
154
155
156
157
158
159
160
else
% Se c a l c u l a n l a s d i m e n s i o n e s , l a masa y l a p o s i c i ó n d e l
cuerpo del
% r o b o t a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
b o d y s i z e =[2∗ h a n d l e s . BodyRad , 2 ∗ h a n d l e s . BodyRad , h a n d l e s .
BodyHeight ] ;
b o d y d e n s i t y =100;
body mass=b o d y d e n s i t y ∗ p i ∗ h a n d l e s . BodyRadˆ2∗ h a n d l e s .
BodyHeight ;
body pos = [ 0 , 0 , 2 ∗ h a n d l e s . WheelsRad+h a n d l e s . BodyHeight / 2 ] ;
161
162
163
164
165
166
167
168
169
% Se c a l c u l a n l a s d i m e n s i o n e s , l a masa y l a p o s i c i ó n de l a s
ruedas
% a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
w h e e l s s i z e =[2∗ h a n d l e s . WheelsRad , 2 ∗ h a n d l e s . WheelsRad , h a n d l e s
. WheelsWidth ] ;
w h e e l s d e n s i t y =500;
w h e e l s m a s s=w h e e l s d e n s i t y ∗ p i ∗ h a n d l e s . WheelsRad ˆ2∗ h a n d l e s .
WheelsWidth ;
l e f t w h e e l p o s =[− h a n d l e s . AxisDistLR /2,− h a n d l e s . AxisDistFR / 2 ,
h a n d l e s . WheelsRad ] ;
r i g h t w h e e l p o s =[ h a n d l e s . AxisDistLR /2,− h a n d l e s . AxisDistFR / 2 ,
h a n d l e s . WheelsRad ] ;
o r i e n t w h e e l p o s =[0 , h a n d l e s . AxisDistFR / 2 , h a n d l e s . WheelsRad ] ;
170
171
% Se c a l c u l a n e l rango y l a p o s i c i ó n d e l s e n s o r de
proximidad
Construcción de plataforma software para el diseño y la simulación...
97
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
% a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
p r o x s e n s o r r a n g e=h a n d l e s . BodyRad/ c o s ( p i / 4 ) ;
p r o x s e n s o r p o s =[0 , h a n d l e s . BodyRad , 2 ∗ h a n d l e s . WheelsRad+
h a n d l e s . BodyHeight / 2 ] ;
172
173
174
175
% Se c a l c u l a n e l ’ f a r c l i p p i n g plane ’ y l a p o s i c i ó n de l o s
s e n s o r e s de
% v i s i ó n a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
f a r c l i p p i n g p l a n e =3∗ h a n d l e s . WheelsRad ;
l e f t v i s i o n s e n s o r p o s =[ −0.1 , s q r t ( h a n d l e s . BodyRad ˆ2 −0.1ˆ2)
+0.005 ,2∗ h a n d l e s . WheelsRad ] ;
r i g h t v i s i o n s e n s o r p o s = [ 0 . 1 , s q r t ( h a n d l e s . BodyRad ˆ2 −0.1ˆ2)
+0.005 ,2∗ h a n d l e s . WheelsRad ] ;
m i d d l e v i s i o n s e n s o r p o s =[0 , h a n d l e s . BodyRad , 2 ∗ h a n d l e s .
WheelsRad+h a n d l e s . BodyHeight / 2 ] ;
176
177
178
179
180
181
182
vrep . s i m x S t a r t S i m u l a t i o n ( c l i e n t I D , vrep . simx opmode oneshot ) ;
183
184
% Se l l a m a a l a f u n c i ó n ’ C r e a t e T r i c y c l e ’ d e l s c r i p t Lua ’
create ’
% e n c a r g a d a de c r e a r e l r o b o t de modelo t r i c i c l o en l a
escena .
vrep . s i m x C a l l S c r i p t F u n c t i o n ( c l i e n t I D , ’ c r e a t e ’ , vrep .
sim scripttype childscript , ’ CreateTricycle function ’
,2 ,...
[ b o d y s i z e , body mass , body pos , w h e e l s s i z e , wheels mass ,
left wheel pos , right wheel pos , orient wheel pos , . . .
prox sensor range , prox sensor pos , far clipping plane ,
left vision sensor pos , right vision sensor pos , . . .
m i d d l e v i s i o n s e n s o r p o s ] , [ ] , [ ] , vrep .
simx opmode blocking ) ;
185
186
187
188
189
190
191
% Se c i e r r a l a ventana y s e c o n t i n ú a con l a de g e n e r a c i ó n de
% o b s t á c u l o s s i l a e s c e n a en l a que s e t r a b a j a e s ’ O b s t a c l e s
’ o
% ’ C i r c u i t ’ o con l a s e l e c c i ó n d e l ı́tem a a ñ a d i r en
cualquier otro caso .
next =1;
close CreaTricycleCir
i f ( s c e n e==1 | | s c e n e ==2)
CreaObs
else
SelectItem
end
192
193
194
195
196
197
198
199
200
201
202
end
Construcción de plataforma software para el diseño y la simulación...
98
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
203
204
205
206
%−−− Función que s e e j e c u t a a l p u l s a r e l botón ’ Back ’ .
f u n c t i o n B a c k C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
207
208
209
210
next =1;
close CreaTricycleCir
SelectRobot
211
212
213
214
215
216
%−−− Función que s e e j e c u t a a l p u l s a r e l botón ’ Cancel ’ .
f u n c t i o n C a n c e l C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
g l o b a l vrep
global clientID
217
218
219
220
221
vrep . s i m x C l o s e S c e n e ( c l i e n t I D , vrep . s i m x o p m o d e b l o c k i n g ) ;
vrep . simxGetPingTime ( c l i e n t I D ) ;
vrep . s i m x F i n i s h ( c l i e n t I D ) ;
close CreaTricycleCir
222
223
224
225
%−−− Función que s e e j e c u t a cuando s e i n t e n t a c e r r a r l a ventana
.
f u n c t i o n f i g u r e 1 C l o s e R e q u e s t F c n ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
226
227
228
229
230
231
232
233
234
235
236
% S i l a p e t i c i ó n de c i e r r e de l a ventana no e s c o n s e c u e n c i a d e l
normal t r á n s i t o d e l programa de una ventana a o t r a
% s i n o que d e r i v a d e l c l i c por p a r t e d e l u s u a r i o en e l i c o n o de
c i e r r e , s e e x i g e una c o n f i r m a c i ó n .
i f next==0
opc=q u e s t d l g ( ’ ¿Desea s a l i r d e l programa ? ’ , ’ S a l i r ’ , ’ Sı́ ’ , ’No ’ ,
’No ’ ) ;
i f strcmp ( opc , ’No ’ )
return
end
end
next =0;
d e l e t e ( hObject ) ;
Construcción de plataforma software para el diseño y la simulación...
99
A CÓDIGO DE MATLAB
A.8.
1
2
3
4
5
6
7
8
9
10
11
12
Carlos Sosa Marrero
CreaTricycleQuad.m
f u n c t i o n v a r a r g o u t = CreaTricycleQuad ( v a r a r g i n )
% Comienzo d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
gui Singleton = 1;
g u i S t a t e = s t r u c t ( ’ gui Name ’ ,
mfilename , . . .
’ gui Singleton ’ , gui Singleton , . . .
’ gui OpeningFcn ’ , @CreaTricycleQuad OpeningFcn , . . .
’ gui OutputFcn ’ , @CreaTricycleQuad OutputFcn , . . .
[] , ...
’ gui LayoutFcn ’ ,
’ gui Callback ’ ,
[]) ;
i f n a r g i n && i s c h a r ( v a r a r g i n { 1 } )
g u i S t a t e . gui Callback = s t r 2 f u n c ( varargin {1}) ;
end
13
14
15
16
17
18
19
i f nargout
[ varargout {1: nargout } ] = gui mainfcn ( gui State , varargin { : } )
;
else
gui mainfcn ( gui State , varargin {:}) ;
end
% Fin d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
20
21
22
23
24
25
26
27
28
29
30
31
%−−− Se e j e c u t a j u s t o a n t e s de que CreaTricycleQuad s e haga
visible .
f u n c t i o n CreaTricycleQuad OpeningFcn ( hObject , ev en td at a , h a n d l e s
, varargin )
% Se p o s i c i o n a l a ventana en l a e s q u i n a s u p e r i o r d e r e c h a de l a
pantalla .
s c r s z=g e t ( 0 , ’ S c r e e n S i z e ’ ) ;
p o s a c t=g e t ( g c f , ’ P o s i t i o n ’ ) ;
xr=s c r s z ( 3 )−p o s a c t ( 3 ) ;
xp=round ( xr / 2 ) ;
yr=s c r s z ( 4 )−p o s a c t ( 4 ) ;
yp=round ( yr / 2 ) ;
s e t ( g c f , ’ p o s i t i o n ’ , [ xp , yp , p o s a c t ( 3 ) , p o s a c t ( 4 ) ] ) ;
32
33
34
h a n d l e s . output = hObject ;
g u i d a t a ( hObject , h a n d l e s ) ;
35
36
37
38
%−−− Las s a l i d a s de e s t a f u n c i ó n s e d e v u e l v e n por l a lı́ n e a de
comandos .
f u n c t i o n v a r a r g o u t = CreaTricycleQuad OutputFcn ( hObject ,
Construcción de plataforma software para el diseño y la simulación...
100
A CÓDIGO DE MATLAB
39
Carlos Sosa Marrero
e ve nt dat a , h a n d l e s )
v a r a r g o u t {1} = h a n d l e s . output ;
40
41
42
43
44
45
46
47
48
49
50
51
52
%−−− Función que l e e e l v a l o r d e l l a r g o d e l c u e r p o d e l r o b o t y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n BodyLength Callback ( hObject , ev e ntd at a , h a n d l e s )
b o d y l e n g t h=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( b o d y l e n g t h ) | | s i g n ( b o d y l e n g t h ) ˜=1)
e r r o r d l g ( ’ El l a r g o d e l c u e r p o d e l r o b o t ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
b o d y l e n g t h =0;
s e t ( hObject , ’ s t r i n g ’ , b o d y l e n g t h ) ;
end
h a n d l e s . BodyLength=b o d y l e n g t h ;
g u i d a t a ( hObject , h a n d l e s ) ;
53
54
55
56
57
58
59
60
61
62
63
64
65
%−−− Función que l e e e l v a l o r d e l ancho d e l c u e r p o d e l r o b o t y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n BodyWidth Callback ( hObject , ev en td at a , h a n d l e s )
body width=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( body width ) | | s i g n ( body width ) ˜=1)
e r r o r d l g ( ’ El ancho d e l c u e r p o d e l r o b o t ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
body width =0;
s e t ( hObject , ’ s t r i n g ’ , body width ) ;
end
h a n d l e s . BodyWidth=body width ;
g u i d a t a ( hObject , h a n d l e s ) ;
66
67
68
69
70
71
72
73
74
75
76
%−−− Función que l e e e l v a l o r de l a a l t u r a d e l c u e r p o d e l r o b o t
y verifica
% que s e a un número p o s i t i v o .
f u n c t i o n B o d y H e i g h t C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
b o d y h e i g h t=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( b o d y h e i g h t ) | | s i g n ( b o d y h e i g h t ) ˜=1)
e r r o r d l g ( ’ El a l t o d e l c u e r p o d e l r o b o t ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
b o d y h e i g h t =0;
s e t ( hObject , ’ s t r i n g ’ , b o d y h e i g h t ) ;
end
Construcción de plataforma software para el diseño y la simulación...
101
A CÓDIGO DE MATLAB
77
78
Carlos Sosa Marrero
h a n d l e s . BodyHeight=b o d y h e i g h t ;
g u i d a t a ( hObject , h a n d l e s ) ;
79
80
81
82
83
84
%−−− Función que l e e e l v a l o r d e l r a d i o de l a s r u e d a s y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n WheelsRad Callback ( hObject , ev en td at a , h a n d l e s )
global wheels rad
85
86
87
88
89
90
91
92
93
w h e e l s r a d=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( w h e e l s r a d ) | | s i g n ( w h e e l s r a d ) ˜=1)
e r r o r d l g ( ’ El r a d i o de l a s r u e d a s ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
w h e e l s r a d =0;
s e t ( hObject , ’ s t r i n g ’ , w h e e l s r a d ) ;
end
h a n d l e s . WheelsRad=w h e e l s r a d ;
g u i d a t a ( hObject , h a n d l e s ) ;
94
95
96
97
98
99
100
101
102
103
104
105
106
%−−− Función que l e e e l v a l o r d e l ancho de l a s r u e d a s y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n WheelsWidth Callback ( hObject , ev en tda ta , h a n d l e s )
w h e e l s w i d t h=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( w h e e l s w i d t h ) | | s i g n ( w h e e l s w i d t h ) ˜=1)
e r r o r d l g ( ’ El ancho de l a s r u e d a s ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
w h e e l s w i d t h =0;
s e t ( hObject , ’ s t r i n g ’ , w h e e l s w i d t h ) ;
end
h a n d l e s . WheelsWidth=w h e e l s w i d t h ;
g u i d a t a ( hObject , h a n d l e s ) ;
107
108
109
110
111
112
113
114
115
%−−− Función que l e e e l v a l o r de l a d i s t a n c i a e n t r e l o s e j e s
i z q u i e r d o −d e r e c h o y v e r i f i c a
% que s e a un número p o s i t i v o .
f u n c t i o n A x i s D i s t L R C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
a x i s d i s t l r =s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( a x i s d i s t l r ) | | s i g n ( a x i s d i s t l r ) ˜=1)
e r r o r d l g ( ’ La d i s t a n c i a e n t r e l o s e j e s i z q u i e r d o y d e r e c h o ha
de s e r un número p o s i t i v o . ’ , ’ERROR ’ )
a x i s d i s t l r =0;
Construcción de plataforma software para el diseño y la simulación...
102
A CÓDIGO DE MATLAB
116
117
118
119
Carlos Sosa Marrero
s e t ( hObject , ’ s t r i n g ’ , a x i s d i s t l r ) ;
end
h a n d l e s . AxisDistLR= a x i s d i s t l r ;
g u i d a t a ( hObject , h a n d l e s ) ;
120
121
122
123
124
125
126
127
128
129
130
131
132
%−−− Función que l e e e l v a l o r de l a d i s t a n c i a e n t r e l o s e j e s
d e l a n t e r o −t r a s e r o y v e r i f i c a
% que s e a un número p o s i t i v o .
f u n c t i o n A x i s D i s t F R C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
a x i s d i s t f r =s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( a x i s d i s t f r ) | | s i g n ( a x i s d i s t f r ) ˜=1)
e r r o r d l g ( ’ La d i s t a n c i a e n t r e l o s e j e s d e l a n t e r o y t r a s e r o ha
de s e r un número p o s i t i v o . ’ , ’ERROR ’ )
a x i s d i s t f r =0;
s e t ( hObject , ’ s t r i n g ’ , a x i s d i s t f r ) ;
end
h a n d l e s . AxisDistFR= a x i s d i s t f r ;
g u i d a t a ( hObject , h a n d l e s ) ;
133
134
135
136
137
138
139
140
%−−− Función que s e e j e c u t a a l p u l s a r e l botón C r e a t e .
f u n c t i o n C r e a t e C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l vrep
global clientID
global scene
g l o b a l next
141
142
143
% Se r e a l i z a n l a s c o m p r o b a c i o n e s n e c e s a r i a s en l o que r e s p e c t a a
las
% r e s t r i c c i o n e s de d i s e ñ o d e l r o b o t .
144
145
146
i f ( ( h a n d l e s . AxisDistLR+h a n d l e s . WheelsWidth )>h a n d l e s . BodyWidth )
e r r o r d l g ( ’ Las r u e d a s t r a s e r a s s o b r e s a l e n por l o s l a t e r a l e s
d e l c u e r p o d e l r o b o t . Pruebe a r e d u c i r l a d i s t a n c i a s
e n t r e l o s e j e s i z q u i e r d o y d e r e c h o o e l ancho de l a s
r u e d a s o aumente e l ancho d e l c u e r p o d e l r o b o t . ’ , ’ERROR ’ )
147
148
149
e l s e i f ( ( h a n d l e s . AxisDistFR/2+ h a n d l e s . WheelsRad )>h a n d l e s .
BodyLength / 2 )
e r r o r d l g ( ’ Las r u e d a s t r a s e r a s s o b r e s a l e n por l a p a r t e
p o s t e r i o r d e l c u e r p o d e l r o b o t . Pruebe a r e d u c i r l a
distancia entre los ejes delantero y trasero o el radio
de l a s r u e d a s o aumente e l l a r g o d e l c u e r p o d e l r o b o t . ’ , ’
ERROR ’ )
Construcción de plataforma software para el diseño y la simulación...
103
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
150
151
152
153
e l s e i f ( c o s ( atan ( ( h a n d l e s . WheelsRad ) / ( h a n d l e s . WheelsWidth / 2 ) )−min
( p i / 6 , atan ( h a n d l e s . WheelsRad / ( h a n d l e s . WheelsWidth / 2 ) ) ) ) . . .
∗ s q r t ( ( h a n d l e s . WheelsWidth / 2 ) ˆ2+ h a n d l e s . WheelsRad ˆ 2 )>
h a n d l e s . BodyWidth / 2 )
e r r o r d l g ( ’ La rueda d e l a n t e r a s o b r e s a l e a l g i r a r por l o s
l a t e r a l e s d e l c u e r p o d e l r o b o t . Pruebe a r e d u c i r e l r a d i o
o e l ancho de l a s r u e d a s o aumente e l ancho d e l c u e r p o
d e l r o b o t . ’ , ’ERROR ’ )
154
155
156
157
e l s e i f ( c o s ( atan ( ( h a n d l e s . WheelsWidth / 2 ) / h a n d l e s . WheelsRad )−min (
p i / 6 , atan ( ( h a n d l e s . WheelsWidth / 2 ) / h a n d l e s . WheelsRad ) ) ) . . .
∗ s q r t ( ( h a n d l e s . WheelsWidth / 2 ) ˆ2+ h a n d l e s . WheelsRad ˆ 2 )+
h a n d l e s . AxisDistFR/2> h a n d l e s . BodyLength / 2 )
e r r o r d l g ( ’ La rueda d e l a n t e r a s o b r e s a l e a l g i r a r por l a p a r t e
f r o n t a l d e l c u e r p o d e l r o b o t . Pruebe a r e d u c i r l a
distancia entre los ejes delantero y trasero , el radio o
e l ancho de l a s r u e d a s o aumente e l l a r g o d e l c u e r p o d e l
r o b o t . ’ , ’ERROR ’ )
158
159
160
e l s e i f ( h a n d l e s . AxisDistLR<=h a n d l e s . WheelsWidth )
e r r o r d l g ( ’ Las r u e d a s t r a s e r a s s e superponen . Pruebe a
aumentar l a d i s t a n c i a e n t r e l o s e j e s i z q u i e r d o y d e r e c h o
o r e d u z c a e l ancho de l a s r u e d a s . ’ , ’ERROR ’ )
161
162
163
164
165
166
167
168
e l s e i f ( h a n d l e s . AxisDistFR−h a n d l e s . WheelsRad < = . . .
s q r t ( ( h a n d l e s . WheelsWidth / 2 ) ˆ2+ h a n d l e s . WheelsRad ˆ 2 )
&&...
h a n d l e s . AxisDistLR/2− h a n d l e s . WheelsWidth /2 <=...
s q r t ( ( h a n d l e s . WheelsWidth / 2 ) ˆ2+ h a n d l e s . WheelsRad ˆ 2 )
&&...
h a n d l e s . AxisDistLR/2− h a n d l e s . WheelsWidth /2 <=...
c o s ( atan ( h a n d l e s . WheelsRad / ( h a n d l e s . WheelsWidth / 2 ) )−p i
/ 6 ) ∗ s q r t ( ( h a n d l e s . WheelsWidth / 2 ) ˆ2+ h a n d l e s . WheelsRad
ˆ2) )
e r r o r d l g ( ’ La rueda d e l a n t e r a s e s u p e r p o n e a l g i r a r con l a s
t r a s e r a s . Pruebe a aumentar l a s d i s t a n c i a s e n t r e e j e s o
r e d u z c a e l r a d i o o e l ancho de l a s r u e d a s . ’ , ’ERROR ’ )
169
170
171
172
173
174
else
% Se c a l c u l a n l a s d i m e n s i o n e s , l a masa y l a p o s i c i ó n d e l
cuerpo del
% r o b o t a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
b o d y s i z e =[ h a n d l e s . BodyWidth , h a n d l e s . BodyLength , h a n d l e s .
Construcción de plataforma software para el diseño y la simulación...
104
A CÓDIGO DE MATLAB
175
176
177
Carlos Sosa Marrero
BodyHeight ] ;
b o d y d e n s i t y =100;
body mass=b o d y d e n s i t y ∗ h a n d l e s . BodyLength ∗ h a n d l e s . BodyWidth∗
h a n d l e s . BodyHeight ;
body pos = [ 0 , 0 , 2 ∗ h a n d l e s . WheelsRad+h a n d l e s . BodyHeight / 2 ] ;
178
179
180
181
182
183
184
185
186
% Se c a l c u l a n l a s d i m e n s i o n e s , l a masa y l a p o s i c i ó n de l a s
ruedas
% a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
w h e e l s s i z e =[2∗ h a n d l e s . WheelsRad , 2 ∗ h a n d l e s . WheelsRad , h a n d l e s
. WheelsWidth ] ;
w h e e l s d e n s i t y =500;
w h e e l s m a s s=w h e e l s d e n s i t y ∗ p i ∗ h a n d l e s . WheelsRad ˆ2∗ h a n d l e s .
WheelsWidth ;
l e f t w h e e l p o s =[− h a n d l e s . AxisDistLR /2,− h a n d l e s . AxisDistFR / 2 ,
h a n d l e s . WheelsRad ] ;
r i g h t w h e e l p o s =[ h a n d l e s . AxisDistLR /2,− h a n d l e s . AxisDistFR / 2 ,
h a n d l e s . WheelsRad ] ;
o r i e n t w h e e l p o s =[0 , h a n d l e s . AxisDistFR / 2 , h a n d l e s . WheelsRad ] ;
187
188
189
190
191
% Se c a l c u l a n e l rango y l a p o s i c i ó n d e l s e n s o r de
proximidad
% a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
p r o x s e n s o r r a n g e =( h a n d l e s . BodyWidth / 2 ) / c o s ( p i / 4 ) ;
p r o x s e n s o r p o s =[0 , h a n d l e s . BodyLength / 2 , 2 ∗ h a n d l e s . WheelsRad+
h a n d l e s . BodyHeight / 2 ] ;
192
193
194
195
196
197
198
% Se c a l c u l a n e l ’ f a r c l i p p i n g plane ’ y l a p o s i c i ó n de l o s
s e n s o r e s de
% v i s i ó n a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
f a r c l i p p i n g p l a n e =3∗ h a n d l e s . WheelsRad ;
l e f t v i s i o n s e n s o r p o s =[ −0.1 , h a n d l e s . BodyLength /2+0.005 ,2∗
h a n d l e s . WheelsRad ] ;
r i g h t v i s i o n s e n s o r p o s = [ 0 . 1 , h a n d l e s . BodyLength /2+0.005 ,2∗
h a n d l e s . WheelsRad ] ;
m i d d l e v i s i o n s e n s o r p o s =[0 , h a n d l e s . BodyLength / 2 , 2 ∗ h a n d l e s .
WheelsRad+h a n d l e s . BodyHeight / 2 ] ;
199
200
vrep . s i m x S t a r t S i m u l a t i o n ( c l i e n t I D , vrep . simx opmode oneshot ) ;
201
202
203
204
% Se l l a m a a l a f u n c i ó n ’ C r e a t e T r i c y c l e ’ d e l s c r i p t Lua ’
create ’
% e n c a r g a d a de c r e a r e l r o b o t de modelo t r i c i c l o en l a
escena .
vrep . s i m x C a l l S c r i p t F u n c t i o n ( c l i e n t I D , ’ c r e a t e ’ , vrep .
Construcción de plataforma software para el diseño y la simulación...
105
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
sim scripttype childscript , ’ CreateTricycle function ’
,0 ,...
[ b o d y s i z e , body mass , body pos , w h e e l s s i z e , wheels mass ,
left wheel pos , right wheel pos , orient wheel pos , . . .
prox sensor range , prox sensor pos , far clipping plane ,
left vision sensor pos , right vision sensor pos , . . .
m i d d l e v i s i o n s e n s o r p o s ] , [ ] , [ ] , vrep .
simx opmode blocking ) ;
205
206
207
208
% Se c i e r r a l a ventana y s e c o n t i n ú a con l a de g e n e r a c i ó n de
% o b s t á c u l o s s i l a e s c e n a en l a que s e t r a b a j a e s ’ O b s t a c l e s
’ o
% ’ C i r c u i t ’ o con l a s e l e c c i ó n d e l ı́tem a a ñ a d i r en
cualquier otro caso .
next =1;
c l o s e CreaTricycleQuad
i f ( s c e n e==1 | | s c e n e ==2)
CreaObs
else
SelectItem
end
209
210
211
212
213
214
215
216
217
218
219
end
220
221
222
223
224
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Back .
f u n c t i o n B a c k C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
225
226
227
228
next =1;
c l o s e CreaTricycleQuad
SelectRobot
229
230
231
232
233
234
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Cancel .
f u n c t i o n C a n c e l C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
g l o b a l vrep
global clientID
235
236
237
238
239
vrep . s i m x C l o s e S c e n e ( c l i e n t I D , vrep . s i m x o p m o d e b l o c k i n g ) ;
vrep . simxGetPingTime ( c l i e n t I D ) ;
vrep . s i m x F i n i s h ( c l i e n t I D ) ;
c l o s e CreaTricycleQuad
240
241
%−−− Función que s e e j e c u t a cuando s e i n t e n t a c e r r a r l a ventana
.
Construcción de plataforma software para el diseño y la simulación...
106
A CÓDIGO DE MATLAB
242
243
Carlos Sosa Marrero
f u n c t i o n f i g u r e 1 C l o s e R e q u e s t F c n ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
244
245
246
247
248
249
250
251
252
253
254
%S i l a p e t i c i ó n de c i e r r e de l a ventana no e s c o n s e c u e n c i a d e l
normal t r á n s i t o d e l programa de una ventana a o t r a
%s i n o que d e r i v a d e l c l i c por p a r t e d e l u s u a r i o en e l i c o n o de
c i e r r e , s e e x i g e una c o n f i r m a c i ó n .
i f next==0
opc=q u e s t d l g ( ’ ¿Desea s a l i r d e l programa ? ’ , ’ S a l i r ’ , ’ Sı́ ’ , ’No ’ ,
’No ’ ) ;
i f strcmp ( opc , ’No ’ )
return
end
end
next =0;
d e l e t e ( hObject ) ;
Construcción de plataforma software para el diseño y la simulación...
107
A CÓDIGO DE MATLAB
A.9.
1
2
3
4
5
6
7
8
9
10
11
12
Carlos Sosa Marrero
CreaQuadricycleCir.m
function varargout = CreaQuadricycleCir ( varargin )
% Comienzo d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
gui Singleton = 1;
g u i S t a t e = s t r u c t ( ’ gui Name ’ ,
mfilename , . . .
’ gui Singleton ’ , gui Singleton , . . .
’ gui OpeningFcn ’ ,
@CreaQuadricycleCir OpeningFcn , . . .
’ gui OutputFcn ’ ,
@CreaQuadricycleCir OutputFcn , . . .
[] , ...
’ gui LayoutFcn ’ ,
’ gui Callback ’ ,
[]) ;
i f n a r g i n && i s c h a r ( v a r a r g i n { 1 } )
g u i S t a t e . gui Callback = s t r 2 f u n c ( varargin {1}) ;
end
13
14
15
16
17
18
19
i f nargout
[ varargout {1: nargout } ] = gui mainfcn ( gui State , varargin { : } )
;
else
gui mainfcn ( gui State , varargin {:}) ;
end
% Fin d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
20
21
22
23
24
25
26
27
28
29
30
31
%−−− Se e j e c u t a j u s t o a n t e s de que C r e a Q u a d r i c y c l e C i r s e haga
visible .
f u n c t i o n C r e a Q u a d r i c y c l e C i r O p e n i n g F c n ( hObject , e ve nt dat a ,
handles , varargin )
% Se p o s i c i o n a l a ventana en l a e s q u i n a s u p e r i o r d e r e c h a de l a
pantalla .
s c r s z=g e t ( 0 , ’ S c r e e n S i z e ’ ) ;
p o s a c t=g e t ( g c f , ’ P o s i t i o n ’ ) ;
xr=s c r s z ( 3 )−p o s a c t ( 3 ) ;
xp=round ( xr / 2 ) ;
yr=s c r s z ( 4 )−p o s a c t ( 4 ) ;
yp=round ( yr / 2 ) ;
s e t ( g c f , ’ p o s i t i o n ’ , [ xp , yp , p o s a c t ( 3 ) , p o s a c t ( 4 ) ] ) ;
32
33
34
h a n d l e s . output = hObject ;
g u i d a t a ( hObject , h a n d l e s ) ;
35
36
37
%−−− Las s a l i d a s de e s t a f u n c i ó n s e d e v u e l v e n por l a lı́ n e a de
Construcción de plataforma software para el diseño y la simulación...
108
A CÓDIGO DE MATLAB
38
39
Carlos Sosa Marrero
comandos .
f u n c t i o n v a r a r g o u t = C r e a Q u a d r i c y c l e C i r O u t p u t F c n ( hObject ,
e ve nt dat a , h a n d l e s )
v a r a r g o u t {1} = h a n d l e s . output ;
40
41
42
43
44
45
46
47
48
49
50
51
52
%−−− Función que l e e e l v a l o r d e l r a d i o d e l c u e r p o d e l r o b o t y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n BodyRad Callback ( hObject , e ve nt da ta , h a n d l e s )
body rad=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( body rad ) | | s i g n ( body rad ) ˜=1)
e r r o r d l g ( ’ El r a d i o d e l c u e r p o d e l r o b o t ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
body rad =0;
s e t ( hObject , ’ s t r i n g ’ , body rad ) ;
end
h a n d l e s . BodyRad=body rad ;
g u i d a t a ( hObject , h a n d l e s ) ;
53
54
55
56
57
58
59
60
61
62
63
64
65
%−−− Función que l e e e l v a l o r de l a a l t u r a d e l c u e r p o d e l r o b o t
y verifica
% que s e a un número p o s i t i v o .
f u n c t i o n B o d y H e i g h t C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
b o d y h e i g h t=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( b o d y h e i g h t ) | | s i g n ( b o d y h e i g h t ) ˜=1)
e r r o r d l g ( ’ El a l t o d e l c u e r p o d e l r o b o t ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
b o d y h e i g h t =0;
s e t ( hObject , ’ s t r i n g ’ , b o d y h e i g h t ) ;
end
h a n d l e s . BodyHeight=b o d y h e i g h t ;
g u i d a t a ( hObject , h a n d l e s ) ;
66
67
68
69
70
71
72
73
74
%−−− Función que l e e e l v a l o r d e l r a d i o de l a s r u e d a s y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n WheelsRad Callback ( hObject , ev en td at a , h a n d l e s )
global wheels rad
w h e e l s r a d=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( w h e e l s r a d ) | | s i g n ( w h e e l s r a d ) ˜=1)
e r r o r d l g ( ’ El r a d i o de l a s r u e d a s ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
Construcción de plataforma software para el diseño y la simulación...
109
A CÓDIGO DE MATLAB
75
76
77
78
79
Carlos Sosa Marrero
w h e e l s r a d =0;
s e t ( hObject , ’ s t r i n g ’ , w h e e l s r a d ) ;
end
h a n d l e s . WheelsRad=w h e e l s r a d ;
g u i d a t a ( hObject , h a n d l e s ) ;
80
81
82
83
84
85
86
87
88
89
90
91
92
%−−− Función que l e e e l v a l o r d e l ancho de l a s r u e d a s y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n WheelsWidth Callback ( hObject , ev en tda ta , h a n d l e s )
w h e e l s w i d t h=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( w h e e l s w i d t h ) | | s i g n ( w h e e l s w i d t h ) ˜=1)
e r r o r d l g ( ’ El ancho de l a s r u e d a s ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
w h e e l s w i d t h =0;
s e t ( hObject , ’ s t r i n g ’ , w h e e l s w i d t h ) ;
end
h a n d l e s . WheelsWidth=w h e e l s w i d t h ;
g u i d a t a ( hObject , h a n d l e s ) ;
93
94
95
96
97
98
99
100
101
102
103
104
105
106
%−−− Función que l e e e l v a l o r de l a d i s t a n c i a e n t r e l o s e j e s
i z q u i e r d o −d e r e c h o y v e r i f i c a
% que s e a un número p o s i t i v o .
f u n c t i o n A x i s D i s t L R C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
global a x i s d i s t l r
a x i s d i s t l r =s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( a x i s d i s t l r ) | | s i g n ( a x i s d i s t l r ) ˜=1)
e r r o r d l g ( ’ La d i s t a n c i a e n t r e l o s e j e s i z q u i e r d o y d e r e c h o
ha de s e r un número p o s i t i v o . ’ , ’ERROR ’ )
a x i s d i s t l r =0;
s e t ( hObject , ’ s t r i n g ’ , a x i s d i s t l r ) ;
end
h a n d l e s . AxisDistLR= a x i s d i s t l r ;
g u i d a t a ( hObject , h a n d l e s ) ;
107
108
109
110
111
112
113
114
%−−− Función que l e e e l v a l o r de l a d i s t a n c i a e n t r e l o s e j e s
d e l a n t e r o −t r a s e r o y v e r i f i c a
% que s e a un número p o s i t i v o .
f u n c t i o n A x i s D i s t F R C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
global a x i s d i s t f r
a x i s d i s t f r =s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( a x i s d i s t f r ) | | s i g n ( a x i s d i s t f r ) ˜=1)
Construcción de plataforma software para el diseño y la simulación...
110
A CÓDIGO DE MATLAB
115
116
117
118
119
120
Carlos Sosa Marrero
e r r o r d l g ( ’ La d i s t a n c i a e n t r e l o s e j e s d e l a n t e r o y t r a s e r o
ha de s e r un número p o s i t i v o . ’ , ’ERROR ’ )
a x i s d i s t f r =0;
s e t ( hObject , ’ s t r i n g ’ , a x i s d i s t f r ) ;
end
h a n d l e s . AxisDistFR= a x i s d i s t f r ;
g u i d a t a ( hObject , h a n d l e s ) ;
121
122
123
124
125
126
127
128
%−−− Función que s e e j e c u t a a l p u l s a r e l botón C r e a t e .
f u n c t i o n C r e a t e C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l vrep
global clientID
global scene
g l o b a l next
129
130
131
132
133
% Se c a l c u l a e l á n g u l o máximo que s e a s i g n a r á a l a rueda
d i r e c t r i z d e r e c h a para
% c o n s e g u i r un g i r o de p i /6 d e l r o b o t .
t h e t a=−p i / 6 ;
t h e t a r i g h t m a x=a c o t ( c o t ( t h e t a ) +( h a n d l e s . AxisDistLR / 2 ) / h a n d l e s .
AxisDistFR ) ;
134
135
136
137
138
% Se r e a l i z a n l a s c o m p r o b a c i o n e s n e c e s a r i a s en l o que r e s p e c t a a
las
% r e s t r i c c i o n e s g e o mé t r i c a s d e l r o b o t .
i f ( h a n d l e s . AxisDistLR >2∗ h a n d l e s . BodyRad )
e r r o r d l g ( ’ La d i s t a n c i a e n t r e l o s e j e s i z q u i e r d o y d e r e c h o no
puede s e r en ning ún c a s o mayor que e l d i á m e t r o d e l
c u e r p o d e l r o b o t . ’ , ’ERROR ’ )
139
140
141
e l s e i f ( h a n d l e s . AxisDistFR >2∗ h a n d l e s . BodyRad )
e r r o r d l g ( ’ La d i s t a n c i a e n t r e l o s e j e s d e l a n t e r o y t r a s e r o no
puede s e r en ning ún c a s o mayor que e l d i á m e t r o d e l
c u e r p o d e l r o b o t . ’ , ’ERROR ’ )
142
143
144
e l s e i f ( ( h a n d l e s . AxisDistLR/2+ h a n d l e s . WheelsWidth / 2 ) ˆ2+( h a n d l e s .
AxisDistFR/2+ h a n d l e s . WheelsRad ) ˆ2> h a n d l e s . BodyRad ˆ 2 )
e r r o r d l g ( ’ Las r u e d a s t r a s e r a s s o b r e s a l e n d e l c u e r p o d e l
r o b o t . Pruebe a r e d u c i r l a s d i s t a n c i a s e n t r e e j e s , e l
r a d i o o e l ancho de l a s r u e d a s o aumente e l r a d i o d e l
c u e r p o d e l r o b o t . ’ , ’ERROR ’ )
145
146
e l s e i f ( s q r t ( c o s ( atan ( h a n d l e s . WheelsRad / ( h a n d l e s . WheelsWidth / 2 ) )−
min(− t h e t a r i g h t m a x , atan ( h a n d l e s . AxisDistLR / h a n d l e s .
Construcción de plataforma software para el diseño y la simulación...
111
A CÓDIGO DE MATLAB
147
148
149
150
Carlos Sosa Marrero
AxisDistFR )−atan ( ( h a n d l e s . WheelsWidth / 2 ) / h a n d l e s . WheelsRad ) ) )
...
∗ s q r t ( ( h a n d l e s . WheelsWidth / 2 ) ˆ2+ h a n d l e s . WheelsRad ˆ 2 )+
h a n d l e s . AxisDistLR / 2 ) ˆ 2 + . . .
( c o s ( atan ( ( h a n d l e s . WheelsWidth / 2 ) / h a n d l e s . WheelsRad )+min
(− t h e t a r i g h t m a x , atan ( h a n d l e s . AxisDistLR / h a n d l e s .
AxisDistFR )−atan ( ( h a n d l e s . WheelsWidth / 2 ) / h a n d l e s .
WheelsRad ) ) ) . . .
∗ s q r t ( ( h a n d l e s . WheelsWidth / 2 ) ˆ2+ h a n d l e s . WheelsRad ˆ 2 )+
h a n d l e s . AxisDistFR / 2 ) ˆ2> h a n d l e s . BodyRad )
e r r o r d l g ( ’ Las r u e d a s d e l a n t e r a s s o b r e s a l e n a l g i r a r d e l
c u e r p o d e l r o b o t . Pruebe a r e d u c i r l a s d i s t a n c i a s e n t r e
e j e s , e l r a d i o o e l ancho de l a s r u e d a s o aumente e l
r a d i o d e l c u e r p o d e l r o b o t . ’ , ’ERROR ’ )
151
152
153
e l s e i f ( h a n d l e s . AxisDistLR<=h a n d l e s . WheelsWidth )
e r r o r d l g ( ’ Las r u e d a s t r a s e r a s s e superponen . Pruebe a
aumentar l a d i s t a n c i a e n t r e l o s e j e s i z q u i e r d o y d e r e c h o
o r e d u z c a e l ancho de l a s r u e d a s . ’ , ’ERROR ’ )
154
155
156
157
e l s e i f (− c o s ( atan ( ( h a n d l e s . WheelsWidth / 2 ) / h a n d l e s . WheelsRad )−min
(− t h e t a r i g h t m a x , atan ( ( h a n d l e s . WheelsWidth / 2 ) / h a n d l e s .
WheelsRad ) ) ) . . .
∗ s q r t ( ( h a n d l e s . WheelsWidth / 2 ) ˆ2+ h a n d l e s . WheelsRad ˆ 2 )+
h a n d l e s . AxisDistFR/2<=−h a n d l e s . AxisDistFR/2+ h a n d l e s .
WheelsRad )
e r r o r d l g ( ’ Las r u e d a s d e l a n t e r a s s e superponen a l g i r a r con
l a s t r a s e r a s . Pruebe a aumentar l a d i s t a n c i a s e n t r e e j e s
o r e d u z c a e l r a d i o o e l ancho de l a s r u e d a s . ’ , ’ERROR ’ )
158
159
160
161
162
163
164
165
else
% Se c a l c u l a n l a s d i m e n s i o n e s , l a masa y l a p o s i c i ó n d e l
cuerpo del
% r o b o t a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
b o d y s i z e =[2∗ h a n d l e s . BodyRad , 2 ∗ h a n d l e s . BodyRad , h a n d l e s .
BodyHeight ] ;
b o d y d e n s i t y =100;
body mass=b o d y d e n s i t y ∗ p i ∗ h a n d l e s . BodyRadˆ2∗ h a n d l e s .
BodyHeight ;
body pos = [ 0 , 0 , 2 ∗ h a n d l e s . WheelsRad+h a n d l e s . BodyHeight / 2 ] ;
166
167
168
169
% Se c a l c u l a n l a s d i m e n s i o n e s , l a masa y l a p o s i c i ó n de l a s
ruedas
% a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
w h e e l s s i z e =[2∗ h a n d l e s . WheelsRad , 2 ∗ h a n d l e s . WheelsRad , h a n d l e s
Construcción de plataforma software para el diseño y la simulación...
112
A CÓDIGO DE MATLAB
170
171
172
173
174
175
Carlos Sosa Marrero
. WheelsWidth ] ;
w h e e l s d e n s i t y =500;
w h e e l s m a s s=w h e e l s d e n s i t y ∗ p i ∗ h a n d l e s . WheelsRad ˆ2∗ h a n d l e s .
WheelsWidth ;
r e a r l e f t w h e e l p o s =[− h a n d l e s . AxisDistLR /2,− h a n d l e s .
AxisDistFR / 2 , h a n d l e s . WheelsRad ] ;
r e a r r i g h t w h e e l p o s =[ h a n d l e s . AxisDistLR /2,− h a n d l e s .
AxisDistFR / 2 , h a n d l e s . WheelsRad ] ;
f r o n t l e f t w h e e l p o s =[− h a n d l e s . AxisDistLR / 2 , h a n d l e s .
AxisDistFR / 2 , h a n d l e s . WheelsRad ] ;
f r o n t r i g h t w h e e l p o s =[ h a n d l e s . AxisDistLR / 2 , h a n d l e s .
AxisDistFR / 2 , h a n d l e s . WheelsRad ] ;
176
177
178
179
180
% Se c a l c u l a n e l rango y l a p o s i c i ó n d e l s e n s o r de
proximidad
% a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
p r o x s e n s o r r a n g e=h a n d l e s . BodyRad/ c o s ( p i / 4 ) ;
p r o x s e n s o r p o s =[0 , h a n d l e s . BodyRad , 2 ∗ h a n d l e s . WheelsRad+
h a n d l e s . BodyHeight / 2 ] ;
181
182
183
184
185
186
187
% Se c a l c u l a n e l ’ f a r c l i p p i n g plane ’ y l a p o s i c i ó n de l o s
s e n s o r e s de
% v i s i ó n a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
f a r c l i p p i n g p l a n e =3∗ h a n d l e s . WheelsRad ;
l e f t v i s i o n s e n s o r p o s =[ −0.1 , s q r t ( h a n d l e s . BodyRad ˆ2 −0.1ˆ2)
+0.005 ,2∗ h a n d l e s . WheelsRad ] ;
r i g h t v i s i o n s e n s o r p o s = [ 0 . 1 , s q r t ( h a n d l e s . BodyRad ˆ2 −0.1ˆ2)
+0.005 ,2∗ h a n d l e s . WheelsRad ] ;
m i d d l e v i s i o n s e n s o r p o s =[0 , h a n d l e s . BodyRad , 2 ∗ h a n d l e s .
WheelsRad+h a n d l e s . BodyHeight / 2 ] ;
188
189
vrep . s i m x S t a r t S i m u l a t i o n ( c l i e n t I D , vrep . simx opmode oneshot ) ;
190
191
192
193
194
195
% Se l l a m a a l a f u n c i ó n ’ C r e a t e Q u a d r i c y c l e ’ d e l s c r i p t Lua ’
create ’
% e n c a r g a d a de c r e a r e l r o b o t de modelo c u a t r i c i c l o en l a
escena .
vrep . s i m x C a l l S c r i p t F u n c t i o n ( c l i e n t I D , ’ c r e a t e ’ , vrep .
sim scripttype childscript , ’ CreateQuadricycle function ’
,2 ,...
[ b o d y s i z e , body mass , body pos , w h e e l s s i z e , wheels mass ,
rear left wheel pos , rear right wheel pos , . . .
front left wheel pos , front right wheel pos ,
prox sensor range , prox sensor pos , far clipping plane
,...
Construcción de plataforma software para el diseño y la simulación...
113
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
left vision sensor pos , right vision sensor pos ,
m i d d l e v i s i o n s e n s o r p o s ] , [ ] , [ ] , vrep .
simx opmode blocking ) ;
196
197
% Se c i e r r a l a ventana y s e c o n t i n ú a con l a de g e n e r a c i ó n de
% o b s t á c u l o s s i l a e s c e n a en l a que s e t r a b a j a e s ’ O b s t a c l e s
’ o
% ’ C i r c u i t ’ o con l a s e l e c c i ó n d e l ı́tem a a ñ a d i r en
cualquier otro caso .
next =1;
c l o s e CreaQuadricycleCir
i f ( s c e n e==1 | | s c e n e ==2)
CreaObs
else
SelectItem
end
198
199
200
201
202
203
204
205
206
207
208
end
209
210
211
212
213
214
215
216
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Back .
f u n c t i o n B a c k C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
next =1;
c l o s e CreaQuadricycleCir
SelectRobot
217
218
219
220
221
222
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Cancel .
f u n c t i o n C a n c e l C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
g l o b a l vrep
global clientID
223
224
225
226
227
vrep . s i m x C l o s e S c e n e ( c l i e n t I D , vrep . s i m x o p m o d e b l o c k i n g ) ;
vrep . simxGetPingTime ( c l i e n t I D ) ;
vrep . s i m x F i n i s h ( c l i e n t I D ) ;
c l o s e CreaQuadricycleCir
228
229
230
231
232
%−−− Función que s e e j e c u t a cuando s e i n t e n t a c e r r a r l a ventana
.
f u n c t i o n f i g u r e 1 C l o s e R e q u e s t F c n ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
233
234
% S i l a p e t i c i ó n de c i e r r e de l a ventana no e s c o n s e c u e n c i a d e l
normal t r á n s i t o d e l programa de una ventana a o t r a
Construcción de plataforma software para el diseño y la simulación...
114
A CÓDIGO DE MATLAB
235
236
237
238
239
240
241
242
243
Carlos Sosa Marrero
% s i n o que d e r i v a d e l c l i c por p a r t e d e l u s u a r i o en e l i c o n o de
c i e r r e , s e e x i g e una c o n f i r m a c i ó n .
i f next==0
opc=q u e s t d l g ( ’ ¿Desea s a l i r d e l programa ? ’ , ’ S a l i r ’ , ’ Sı́ ’ , ’No ’ ,
’No ’ ) ;
i f strcmp ( opc , ’No ’ )
return
end
end
next =0;
d e l e t e ( hObject ) ;
Construcción de plataforma software para el diseño y la simulación...
115
A CÓDIGO DE MATLAB
A.10.
1
2
3
4
5
6
7
8
9
10
11
12
Carlos Sosa Marrero
CreaQuadricycleQuad.m
f u n c t i o n v a r a r g o u t = CreaQuadricycleQuad ( v a r a r g i n )
% Comienzo d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
gui Singleton = 1;
g u i S t a t e = s t r u c t ( ’ gui Name ’ ,
mfilename , . . .
’ gui Singleton ’ , gui Singleton , . . .
’ gui OpeningFcn ’ ,
@CreaQuadricycleQuad OpeningFcn , . . .
’ gui OutputFcn ’ ,
@CreaQuadricycleQuad OutputFcn , . . .
[] , ...
’ gui LayoutFcn ’ ,
’ gui Callback ’ ,
[]) ;
i f n a r g i n && i s c h a r ( v a r a r g i n { 1 } )
g u i S t a t e . gui Callback = s t r 2 f u n c ( varargin {1}) ;
end
13
14
15
16
17
18
19
i f nargout
[ varargout {1: nargout } ] = gui mainfcn ( gui State , varargin { : } )
;
else
gui mainfcn ( gui State , varargin {:}) ;
end
% Fin d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
20
21
22
23
24
25
26
27
28
29
30
31
%−−− Se e j e c u t a j u s t o a n t e s de que CreaQuadricycleQuad s e haga
visible .
f u n c t i o n CreaQuadricycleQuad OpeningFcn ( hObject , ev en td at a ,
handles , varargin )
% Se p o s i c i o n a l a ventana en l a e s q u i n a s u p e r i o r d e r e c h a de l a
pantalla .
s c r s z=g e t ( 0 , ’ S c r e e n S i z e ’ ) ;
p o s a c t=g e t ( g c f , ’ P o s i t i o n ’ ) ;
xr=s c r s z ( 3 )−p o s a c t ( 3 ) ;
xp=round ( xr / 2 ) ;
yr=s c r s z ( 4 )−p o s a c t ( 4 ) ;
yp=round ( yr / 2 ) ;
s e t ( g c f , ’ p o s i t i o n ’ , [ xp , yp , p o s a c t ( 3 ) , p o s a c t ( 4 ) ] ) ;
32
33
34
h a n d l e s . output = hObject ;
g u i d a t a ( hObject , h a n d l e s ) ;
35
36
37
%−−− Las s a l i d a s de e s t a f u n c i ó n s e d e v u e l v e n por l a lı́ n e a de
Construcción de plataforma software para el diseño y la simulación...
116
A CÓDIGO DE MATLAB
38
39
Carlos Sosa Marrero
comandos .
f u n c t i o n v a r a r g o u t = CreaQuadricycleQuad OutputFcn ( hObject ,
e ve nt dat a , h a n d l e s )
v a r a r g o u t {1} = h a n d l e s . output ;
40
41
42
43
44
45
46
47
48
49
50
51
52
%−−− Función que l e e e l v a l o r d e l l a r g o d e l c u e r p o d e l r o b o t y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n BodyLength Callback ( hObject , ev e ntd at a , h a n d l e s )
b o d y l e n g t h=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( b o d y l e n g t h ) | | s i g n ( b o d y l e n g t h ) ˜=1)
e r r o r d l g ( ’ El l a r g o d e l c u e r p o d e l r o b o t ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
b o d y l e n g t h =0;
s e t ( hObject , ’ s t r i n g ’ , b o d y l e n g t h ) ;
end
h a n d l e s . BodyLength=b o d y l e n g t h ;
g u i d a t a ( hObject , h a n d l e s ) ;
53
54
55
56
57
58
59
60
61
62
63
64
65
%−−− Función que l e e e l v a l o r d e l ancho d e l c u e r p o d e l r o b o t y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n BodyWidth Callback ( hObject , ev en td at a , h a n d l e s )
body width=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( body width ) | | s i g n ( body width ) ˜=1)
e r r o r d l g ( ’ El ancho d e l c u e r p o d e l r o b o t ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
body width =0;
s e t ( hObject , ’ s t r i n g ’ , body width ) ;
end
h a n d l e s . BodyWidth=body width ;
g u i d a t a ( hObject , h a n d l e s ) ;
66
67
68
69
70
71
72
73
74
%−−− Función que l e e e l v a l o r de l a a l t u r a d e l c u e r p o d e l r o b o t
y verifica
% que s e a un número p o s i t i v o .
f u n c t i o n B o d y H e i g h t C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
b o d y h e i g h t=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( b o d y h e i g h t ) | | s i g n ( b o d y h e i g h t ) ˜=1)
e r r o r d l g ( ’ El a l t o d e l c u e r p o d e l r o b o t ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
b o d y h e i g h t =0;
Construcción de plataforma software para el diseño y la simulación...
117
A CÓDIGO DE MATLAB
75
76
77
78
Carlos Sosa Marrero
s e t ( hObject , ’ s t r i n g ’ , b o d y h e i g h t ) ;
end
h a n d l e s . BodyHeight=b o d y h e i g h t ;
g u i d a t a ( hObject , h a n d l e s ) ;
79
80
81
82
83
84
85
86
87
88
89
90
91
92
%−−− Función que l e e e l v a l o r d e l r a d i o de l a s r u e d a s y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n WheelsRad Callback ( hObject , ev en td at a , h a n d l e s )
global wheels rad
w h e e l s r a d=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( w h e e l s r a d ) | | s i g n ( w h e e l s r a d ) ˜=1)
e r r o r d l g ( ’ El r a d i o de l a s r u e d a s ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
w h e e l s r a d =0;
s e t ( hObject , ’ s t r i n g ’ , w h e e l s r a d ) ;
end
h a n d l e s . WheelsRad=w h e e l s r a d ;
g u i d a t a ( hObject , h a n d l e s ) ;
93
94
95
96
97
98
99
100
101
102
103
104
105
%−−− Función que l e e e l v a l o r d e l ancho de l a s r u e d a s y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n WheelsWidth Callback ( hObject , ev en tda ta , h a n d l e s )
w h e e l s w i d t h=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( w h e e l s w i d t h ) | | s i g n ( w h e e l s w i d t h ) ˜=1)
e r r o r d l g ( ’ El ancho de l a s r u e d a s ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
w h e e l s w i d t h =0;
s e t ( hObject , ’ s t r i n g ’ , w h e e l s w i d t h ) ;
end
h a n d l e s . WheelsWidth=w h e e l s w i d t h ;
g u i d a t a ( hObject , h a n d l e s ) ;
106
107
108
109
110
111
112
113
114
%−−− Función que l e e e l v a l o r de l a d i s t a n c i a e n t r e l o s e j e s
i z q u i e r d o −d e r e c h o y v e r i f i c a
% que s e a un número p o s i t i v o .
f u n c t i o n A x i s D i s t L R C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
global a x i s d i s t l r
a x i s d i s t l r =s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( a x i s d i s t l r ) | | s i g n ( a x i s d i s t l r ) ˜=1)
e r r o r d l g ( ’ La d i s t a n c i a e n t r e l o s e j e s i z q u i e r d o y d e r e c h o
Construcción de plataforma software para el diseño y la simulación...
118
A CÓDIGO DE MATLAB
115
116
117
118
119
Carlos Sosa Marrero
ha de s e r un número p o s i t i v o . ’ , ’ERROR ’ )
a x i s d i s t l r =0;
s e t ( hObject , ’ s t r i n g ’ , a x i s d i s t l r ) ;
end
h a n d l e s . AxisDistLR= a x i s d i s t l r ;
g u i d a t a ( hObject , h a n d l e s ) ;
120
121
122
123
124
125
126
127
128
129
130
131
132
133
%−−− Función que l e e e l v a l o r de l a d i s t a n c i a e n t r e l o s e j e s
d e l a n t e r o −t r a s e r o y v e r i f i c a
% que s e a un número p o s i t i v o .
f u n c t i o n A x i s D i s t F R C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
global a x i s d i s t f r
a x i s d i s t f r =s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( a x i s d i s t f r ) | | s i g n ( a x i s d i s t f r ) ˜=1)
e r r o r d l g ( ’ La d i s t a n c i a e n t r e l o s e j e s d e l a n t e r o y t r a s e r o
ha de s e r un número p o s i t i v o . ’ , ’ERROR ’ )
a x i s d i s t f r =0;
s e t ( hObject , ’ s t r i n g ’ , a x i s d i s t f r ) ;
end
h a n d l e s . AxisDistFR= a x i s d i s t f r ;
g u i d a t a ( hObject , h a n d l e s ) ;
134
135
136
137
138
139
140
141
%−−− Función que s e e j e c u t a a l p u l s a r e l botón C r e a t e .
f u n c t i o n C r e a t e C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l vrep
global clientID
global scene
g l o b a l next
142
143
144
145
146
% Se c a l c u l a e l á n g u l o s que s e a s i g n a r á a l a rueda d i r e c t r i z
d e r e c h a para
% c o n s e g u i r un g i r o de p i /6 d e l r o b o t .
t h e t a=−p i / 6 ;
t h e t a r i g h t m a x=a c o t ( c o t ( t h e t a ) +( h a n d l e s . AxisDistLR / 2 ) / h a n d l e s .
AxisDistFR ) ;
147
148
149
150
151
% Se r e a l i z a n l a s c o m p r o b a c i o n e s n e c e s a r i a s en l o que r e s p e c t a a
las
% r e s t r i c c i o n e s g e o mé t r i c a s d e l r o b o t .
i f ( h a n d l e s . AxisDistLR>h a n d l e s . BodyWidth )
e r r o r d l g ( ’ La d i s t a n c i a e n t r e l o s e j e s i z q u i e r d o y d e r e c h o no
puede s e r en ning ún c a s o mayor que e l ancho d e l c u e r p o
d e l r o b o t . ’ , ’ERROR ’ )
Construcción de plataforma software para el diseño y la simulación...
119
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
152
153
154
e l s e i f ( h a n d l e s . AxisDistFR >2∗ h a n d l e s . BodyLength )
e r r o r d l g ( ’ La d i s t a n c i a e n t r e l o s e j e s d e l a n t e r o y t r a s e r o no
puede s e r en ning ún c a s o mayor que e l l a r g o d e l c u e r p o
d e l r o b o t . ’ , ’ERROR ’ )
155
156
157
e l s e i f ( ( h a n d l e s . AxisDistLR/2+ h a n d l e s . WheelsWidth / 2 )>h a n d l e s .
BodyWidth / 2 )
e r r o r d l g ( ’ Las r u e d a s t r a s e r a s s o b r e s a l e n por l o s l a t e r a l e s
d e l c u e r p o d e l r o b o t . Pruebe a r e d u c i r l a d i s t a n c i a s
e n t r e l o s e j e s i z q u i e r d o y d e r e c h o o e l ancho de l a s
r u e d a s o aumente e l ancho d e l c u e r p o d e l r o b o t . ’ , ’ERROR ’ )
158
159
160
e l s e i f ( ( h a n d l e s . AxisDistFR/2+ h a n d l e s . WheelsRad )>h a n d l e s .
BodyLength / 2 )
e r r o r d l g ( ’ Las r u e d a s t r a s e r a s s o b r e s a l e n por l a p a r t e
p o s t e r i o r d e l c u e r p o d e l r o b o t . Pruebe a r e d u c i r l a
distancia entre los ejes delantero y trasero o el radio
de l a s r u e d a s o aumente e l l a r g o d e l c u e r p o d e l r o b o t . ’ , ’
ERROR ’ )
161
162
163
164
e l s e i f ( c o s ( atan ( h a n d l e s . WheelsRad / ( h a n d l e s . WheelsWidth / 2 ) )−min(−
t h e t a r i g h t m a x , atan ( h a n d l e s . WheelsRad / ( h a n d l e s . WheelsWidth
/2) ) ) ) . . .
∗ s q r t ( ( h a n d l e s . WheelsWidth / 2 ) ˆ2+ h a n d l e s . WheelsRad ˆ 2 )+
h a n d l e s . AxisDistLR/2> h a n d l e s . BodyWidth / 2 )
e r r o r d l g ( ’ Las r u e d a s d e l a n t e r a s s o b r e s a l e n a l g i r a r por l o s
l a t e r a l e s d e l c u e r p o d e l r o b o t . Pruebe a r e d u c i r l a
d i s t a n c i a e n t r e l o s e j e s i z q u i e r d o y derecho , e l r a d i o o
e l ancho de l a s r u e d a s o aumente e l ancho d e l c u e r p o d e l
r o b o t . ’ , ’ERROR ’ )
165
166
167
168
e l s e i f ( c o s ( atan ( ( h a n d l e s . WheelsWidth / 2 ) / h a n d l e s . WheelsRad )−min(−
t h e t a r i g h t m a x , atan ( ( h a n d l e s . WheelsWidth / 2 ) / h a n d l e s .
WheelsRad ) ) ) . . .
∗ s q r t ( ( h a n d l e s . WheelsWidth / 2 ) ˆ2+ h a n d l e s . WheelsRad ˆ 2 )+
h a n d l e s . AxisDistFR/2> h a n d l e s . BodyLength / 2 )
e r r o r d l g ( ’ Las rueda d e l a n t e r a s s o b r e s a l e n a l g i r a r por l a
p a r t e f r o n t a l d e l c u e r p o d e l r o b o t . Pruebe a r e d u c i r l a
distancia entre los ejes delantero y trasero , el radio o
e l ancho de l a s r u e d a s o aumente e l l a r g o d e l c u e r p o d e l
r o b o t . ’ , ’ERROR ’ )
169
170
171
e l s e i f ( h a n d l e s . AxisDistLR<=h a n d l e s . WheelsWidth )
e r r o r d l g ( ’ Las r u e d a s t r a s e r a s s e superponen . Pruebe a
Construcción de plataforma software para el diseño y la simulación...
120
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
aumentar l a d i s t a n c i a e n t r e l o s e j e s i z q u i e r d o y d e r e c h o
o r e d u z c a e l ancho de l a s r u e d a s . ’ , ’ERROR ’ )
172
173
174
175
e l s e i f (− c o s ( atan ( ( h a n d l e s . WheelsWidth / 2 ) / h a n d l e s . WheelsRad )−min
(− t h e t a r i g h t m a x , atan ( ( h a n d l e s . WheelsWidth / 2 ) / h a n d l e s .
WheelsRad ) ) ) . . .
∗ s q r t ( ( h a n d l e s . WheelsWidth / 2 ) ˆ2+ h a n d l e s . WheelsRad ˆ 2 )+
h a n d l e s . AxisDistFR/2<=−h a n d l e s . AxisDistFR/2+ h a n d l e s .
WheelsRad )
e r r o r d l g ( ’ Las r u e d a s d e l a n t e r a s s e superponen a l g i r a r con
l a s t r a s e r a s . Pruebe a aumentar l a d i s t a n c i a s e n t r e e j e s
o r e d u z c a e l r a d i o o e l ancho de l a s r u e d a s . ’ , ’ERROR ’ )
176
177
178
179
180
181
182
183
184
185
else
% Se c a l c u l a n l a s d i m e n s i o n e s , l a masa y l a p o s i c i ó n d e l
cuerpo del
% r o b o t a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
b o d y s i z e =[ h a n d l e s . BodyWidth , h a n d l e s . BodyLength , h a n d l e s .
BodyHeight ] ;
b o d y d e n s i t y =100;
body mass=b o d y d e n s i t y ∗ h a n d l e s . BodyLength ∗ h a n d l e s . BodyWidth∗
h a n d l e s . BodyHeight ;
body pos = [ 0 , 0 , 2 ∗ h a n d l e s . WheelsRad+h a n d l e s . BodyHeight / 2 ] ;
186
187
188
189
190
191
192
193
194
195
% Se c a l c u l a n l a s d i m e n s i o n e s , l a masa y l a p o s i c i ó n de l a s
ruedas
% a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
w h e e l s s i z e =[2∗ h a n d l e s . WheelsRad , 2 ∗ h a n d l e s . WheelsRad , h a n d l e s
. WheelsWidth ] ;
w h e e l s d e n s i t y =500;
w h e e l s m a s s=w h e e l s d e n s i t y ∗ p i ∗ h a n d l e s . WheelsRad ˆ2∗ h a n d l e s .
WheelsWidth ;
r e a r l e f t w h e e l p o s =[− h a n d l e s . AxisDistLR /2,− h a n d l e s .
AxisDistFR / 2 , h a n d l e s . WheelsRad ] ;
r e a r r i g h t w h e e l p o s =[ h a n d l e s . AxisDistLR /2,− h a n d l e s .
AxisDistFR / 2 , h a n d l e s . WheelsRad ] ;
f r o n t l e f t w h e e l p o s =[− h a n d l e s . AxisDistLR / 2 , h a n d l e s .
AxisDistFR / 2 , h a n d l e s . WheelsRad ] ;
f r o n t r i g h t w h e e l p o s =[ h a n d l e s . AxisDistLR / 2 , h a n d l e s .
AxisDistFR / 2 , h a n d l e s . WheelsRad ] ;
196
197
% Se c a l c u l a n e l rango y l a p o s i c i ó n d e l s e n s o r de
proximidad
Construcción de plataforma software para el diseño y la simulación...
121
A CÓDIGO DE MATLAB
198
199
200
Carlos Sosa Marrero
% a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
p r o x s e n s o r r a n g e =( h a n d l e s . BodyWidth / 2 ) / c o s ( p i / 4 ) ;
p r o x s e n s o r p o s =[0 , h a n d l e s . BodyLength / 2 , 2 ∗ h a n d l e s . WheelsRad+
h a n d l e s . BodyHeight / 2 ] ;
201
202
203
204
205
206
207
% Se c a l c u l a n e l ’ f a r c l i p p i n g plane ’ y l a p o s i c i ó n de l o s
s e n s o r e s de
% v i s i ó n a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o .
f a r c l i p p i n g p l a n e =3∗ h a n d l e s . WheelsRad ;
l e f t v i s i o n s e n s o r p o s =[ −0.1 , h a n d l e s . BodyLength /2+0.005 ,2∗
h a n d l e s . WheelsRad ] ;
r i g h t v i s i o n s e n s o r p o s = [ 0 . 1 , h a n d l e s . BodyLength /2+0.005 ,2∗
h a n d l e s . WheelsRad ] ;
m i d d l e v i s i o n s e n s o r p o s =[0 , h a n d l e s . BodyLength / 2 , 2 ∗ h a n d l e s .
WheelsRad+h a n d l e s . BodyHeight / 2 ] ;
208
209
vrep . s i m x S t a r t S i m u l a t i o n ( c l i e n t I D , vrep . simx opmode oneshot ) ;
210
211
212
213
214
215
216
% Se l l a m a a l a f u n c i ó n ’ C r e a t e Q u a d r i c y c l e ’ d e l s c r i p t Lua ’
create ’
% e n c a r g a d a de c r e a r e l r o b o t de modelo c u a t r i c i c l o en l a
escena .
vrep . s i m x C a l l S c r i p t F u n c t i o n ( c l i e n t I D , ’ c r e a t e ’ , vrep .
sim scripttype childscript , ’ CreateQuadricycle function ’
,0 ,...
[ b o d y s i z e , body mass , body pos , w h e e l s s i z e , wheels mass ,
rear left wheel pos , rear right wheel pos , . . .
front left wheel pos , front right wheel pos ,
prox sensor range , prox sensor pos , far clipping plane
,...
left vision sensor pos , right vision sensor pos ,
m i d d l e v i s i o n s e n s o r p o s ] , [ ] , [ ] , vrep .
simx opmode blocking ) ;
217
218
219
220
221
222
223
224
225
226
% Se c i e r r a l a ventana y s e c o n t i n ú a con l a de g e n e r a c i ó n de
% o b s t á c u l o s s i l a e s c e n a en l a que s e t r a b a j a e s ’ O b s t a c l e s
’ o
% ’ C i r c u i t ’ o con l a s e l e c c i ó n d e l ı́tem a a ñ a d i r en
cualquier otro caso .
next =1;
c l o s e CreaQuadricycleQuad
i f ( s c e n e==1 | | s c e n e ==2)
CreaObs
else
SelectItem
Construcción de plataforma software para el diseño y la simulación...
122
A CÓDIGO DE MATLAB
end
227
228
Carlos Sosa Marrero
end
229
230
231
232
233
234
235
236
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Back .
f u n c t i o n B a c k C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
next =1;
c l o s e CreaQuadricycleQuad
SelectRobot
237
238
239
240
241
242
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Cancel .
f u n c t i o n C a n c e l C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
g l o b a l vrep
global clientID
243
244
245
246
247
vrep . s i m x C l o s e S c e n e ( c l i e n t I D , vrep . s i m x o p m o d e b l o c k i n g ) ;
vrep . simxGetPingTime ( c l i e n t I D ) ;
vrep . s i m x F i n i s h ( c l i e n t I D ) ;
c l o s e CreaQuadricycleQuad
248
249
250
251
252
%−−− Función que s e e j e c u t a cuando s e i n t e n t a c e r r a r l a ventana
.
f u n c t i o n f i g u r e 1 C l o s e R e q u e s t F c n ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
253
254
255
256
257
258
259
260
261
262
263
% S i l a p e t i c i ó n de c i e r r e de l a ventana no e s c o n s e c u e n c i a d e l
normal t r á n s i t o d e l programa de una ventana a o t r a
% s i n o que d e r i v a d e l c l i c por p a r t e d e l u s u a r i o en e l i c o n o de
c i e r r e , s e e x i g e una c o n f i r m a c i ó n .
i f next==0
opc=q u e s t d l g ( ’ ¿Desea s a l i r d e l programa ? ’ , ’ S a l i r ’ , ’ Sı́ ’ , ’No ’ ,
’No ’ ) ;
i f strcmp ( opc , ’No ’ )
return
end
end
next =0;
d e l e t e ( hObject ) ;
Construcción de plataforma software para el diseño y la simulación...
123
A CÓDIGO DE MATLAB
A.11.
1
2
3
4
5
6
7
8
9
10
11
12
Carlos Sosa Marrero
SelectItem.m
function varargout = SelectItem ( varargin )
% Comienzo d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
gui Singleton = 1;
g u i S t a t e = s t r u c t ( ’ gui Name ’ ,
mfilename , . . .
’ gui Singleton ’ , gui Singleton , . . .
’ gui OpeningFcn ’ , @SelectItem OpeningFcn , . . .
’ gui OutputFcn ’ , @SelectItem OutputFcn , . . .
[] , ...
’ gui LayoutFcn ’ ,
’ gui Callback ’ ,
[]) ;
i f n a r g i n && i s c h a r ( v a r a r g i n { 1 } )
g u i S t a t e . gui Callback = s t r 2 f u n c ( varargin {1}) ;
end
13
14
15
16
17
18
19
i f nargout
[ varargout {1: nargout } ] = gui mainfcn ( gui State , varargin { : } )
;
else
gui mainfcn ( gui State , varargin {:}) ;
end
% Fin d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
20
21
22
23
24
25
26
27
28
29
30
31
%−−− Se e j e c u t a j u s t o a n t e s de que S e l e c t I t e m s e haga v i s i b l e .
f u n c t i o n S e l e c t I t e m O p e n i n g F c n ( hObject , ev en td at a , h a n d l e s ,
varargin )
% Se p o s i c i o n a l a ventana en l a e s q u i n a s u p e r i o r d e r e c h a de l a
pantalla .
s c r s z=g e t ( 0 , ’ S c r e e n S i z e ’ ) ;
p o s a c t=g e t ( g c f , ’ P o s i t i o n ’ ) ;
xr=s c r s z ( 3 )−p o s a c t ( 3 ) ;
xp=round ( xr / 2 ) ;
yr=s c r s z ( 4 )−p o s a c t ( 4 ) ;
yp=round ( yr / 2 ) ;
s e t ( g c f , ’ p o s i t i o n ’ , [ xp , yp , p o s a c t ( 3 ) , p o s a c t ( 4 ) ] ) ;
32
33
34
h a n d l e s . output = hObject ;
g u i d a t a ( hObject , h a n d l e s ) ;
35
36
37
38
%−−− Las s a l i d a s de e s t a f u n c i ó n s e d e v u e l v e n por l a lı́ n e a de
comandos .
f u n c t i o n v a r a r g o u t = S e l e c t I t e m O u t p u t F c n ( hObject , e v en tda ta ,
handles )
Construcción de plataforma software para el diseño y la simulación...
124
A CÓDIGO DE MATLAB
39
Carlos Sosa Marrero
v a r a r g o u t {1} = h a n d l e s . output ;
40
41
42
43
44
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Marker .
f u n c t i o n M a r k e r C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
g l o b a l next
45
46
47
48
next =1;
close SelectItem
PosMarker
49
50
51
52
53
%−−− Función que s e e j e c u t a a l p u l s a r e l botón O b s t a c l e .
f u n c t i o n O b s t a c l e C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
54
55
56
57
next =1;
close SelectItem
CreaObs
58
59
60
61
62
%−−− Función que s e e j e c u t a a l p u l s a r e l botón S i m u l a t e .
f u n c t i o n S i m u l a t e C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
g l o b a l next
63
64
65
66
next =1;
close SelectItem
Simulation
67
68
69
70
71
%−−− Función que s e e j e c u t a cuando s e i n t e n t a c e r r a r l a ventana
.
f u n c t i o n f i g u r e 1 C l o s e R e q u e s t F c n ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
72
73
74
75
76
77
78
79
% S i l a p e t i c i ó n de c i e r r e de l a ventana no e s c o n s e c u e n c i a d e l
normal t r á n s i t o d e l programa de una ventana a o t r a
% s i n o que d e r i v a d e l c l i c por p a r t e d e l u s u a r i o en e l i c o n o de
c i e r r e , s e e x i g e una c o n f i r m a c i ó n .
i f next==0
opc=q u e s t d l g ( ’ ¿Desea s a l i r d e l programa ? ’ , ’ S a l i r ’ , ’ Sı́ ’ , ’No ’ ,
’No ’ ) ;
i f strcmp ( opc , ’No ’ )
return
end
Construcción de plataforma software para el diseño y la simulación...
125
A CÓDIGO DE MATLAB
80
81
82
Carlos Sosa Marrero
end
next =0;
d e l e t e ( hObject ) ;
Construcción de plataforma software para el diseño y la simulación...
126
A CÓDIGO DE MATLAB
A.12.
1
2
3
4
5
6
7
8
9
10
11
12
Carlos Sosa Marrero
PosMarker.m
f u n c t i o n v a r a r g o u t = PosMarker ( v a r a r g i n )
% Comienzo d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
gui Singleton = 1;
g u i S t a t e = s t r u c t ( ’ gui Name ’ ,
mfilename , . . .
’ gui Singleton ’ , gui Singleton , . . .
’ gui OpeningFcn ’ , @PosMarker OpeningFcn , . . .
’ gui OutputFcn ’ , @PosMarker OutputFcn , . . .
[] , ...
’ gui LayoutFcn ’ ,
’ gui Callback ’ ,
[]) ;
i f n a r g i n && i s c h a r ( v a r a r g i n { 1 } )
g u i S t a t e . gui Callback = s t r 2 f u n c ( varargin {1}) ;
end
13
14
15
16
17
18
19
i f nargout
[ varargout {1: nargout } ] = gui mainfcn ( gui State , varargin { : } )
;
else
gui mainfcn ( gui State , varargin {:}) ;
end
% Fin d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
20
21
22
23
24
25
26
27
%−−− Se e j e c u t a j u s t o a n t e s de que PosMarker s e haga v i s i b l e .
f u n c t i o n PosMarker OpeningFcn ( hObject , ev en td at a , h a n d l e s ,
varargin )
g l o b a l vrep
global clientID
g l o b a l num marker
g l o b a l marker
28
29
30
31
32
33
34
35
36
% Se p o s i c i o n a l a ventana en l a e s q u i n a s u p e r i o r d e r e c h a de l a
pantalla .
s c r s z=g e t ( 0 , ’ S c r e e n S i z e ’ ) ;
p o s a c t=g e t ( g c f , ’ P o s i t i o n ’ ) ;
xr=s c r s z ( 3 )−p o s a c t ( 3 ) ;
xp=round ( xr / 2 ) ;
yr=s c r s z ( 4 )−p o s a c t ( 4 ) ;
yp=round ( yr / 2 ) ;
s e t ( g c f , ’ p o s i t i o n ’ , [ xp , yp , p o s a c t ( 3 ) , p o s a c t ( 4 ) ] ) ;
37
38
39
% Se muestra l a imagen d e l i c o n o a s o c i a d o a l d e s p l a z a m i e n t o de
un o b j e t o en
% V−REP.
Construcción de plataforma software para el diseño y la simulación...
127
A CÓDIGO DE MATLAB
40
41
42
43
44
45
Carlos Sosa Marrero
axes ( handles . axes1 )
path=pwd ;
image=imread ( [ path , ’ \ o b j e c t S h i f t B u t t o n . j p g ’ ] ) ;
image=i m r e s i z e ( image , [ 2 5 , NaN ] ) ;
axis off
imshow ( image )
46
47
num marker=num marker +1;
48
49
50
51
% Se d e t e r m i n a e l nombre de l a nueva b a l i z a y su p o s i c i ó n por
defecto .
marker name=s t r c a t ( ’ marker ’ , num2str ( num marker ) ) ;
marker pos = [ 0 , 1 , 0 . 2 5 ] ;
52
53
54
55
56
57
58
% Se l l a m a a l a f u n c i ó n ’ CreateMarker ’ d e l s c r i p t Lua ’ c r e a t e ’
% e n c a r g a d a de c r e a r una b a l i z a en l a e s c e n a . Esta d e v u e l v e e l
identificador
% de l a b a l i z a que s e a ñade a l v e c t o r marker .
[ ˜ , r e t I n t s , ˜ , ˜ ,]= vrep . s i m x C a l l S c r i p t F u n c t i o n ( c l i e n t I D , ’
c r e a t e ’ , vrep . s i m s c r i p t t y p e c h i l d s c r i p t , . . .
’ C r e a t e M a r k e r f u n c t i o n ’ , [ ] , marker pos , marker name , [ ] , vrep .
simx opmode blocking ) ;
marker ( num marker )=r e t I n t s ( 1 ) ;
59
60
61
62
63
64
% Se f u e r z a n l a s c o o r d e n a d a s c a r t e s i a n a s de l a b a l i z a l o s
v a l o r e s por
% defecto .
s e t ( h a n d l e s . XPos , ’ s t r i n g ’ , 0 ) ;
s e t ( h a n d l e s . YPos , ’ s t r i n g ’ , 1 ) ;
s e t ( h a n d l e s . ZPos , ’ s t r i n g ’ , 0 . 2 5 ) ;
65
66
67
h a n d l e s . output = hObject ;
g u i d a t a ( hObject , h a n d l e s ) ;
68
69
70
71
72
%−−− Las s a l i d a s de e s t a f u n c i ó n s e d e v u e l v e n por l a lı́ n e a de
comandos .
f u n c t i o n v a r a r g o u t = PosMarker OutputFcn ( hObject , ev en tda ta ,
handles )
v a r a r g o u t {1} = h a n d l e s . output ;
73
74
75
76
%−−− Función que a c t u a l i z a e l v a l o r de l a coordenada x d e l
o b s t á c u l o .
f u n c t i o n XPos Callback ( hObject , ev en td at a , h a n d l e s )
Construcción de plataforma software para el diseño y la simulación...
128
A CÓDIGO DE MATLAB
77
78
79
80
global
global
global
global
Carlos Sosa Marrero
vrep
clientID
marker
num marker
81
82
83
84
85
86
x p o s=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
y p o s=s t r 2 d o u b l e ( g e t ( h a n d l e s . YPos , ’ s t r i n g ’ ) ) ;
z p o s=s t r 2 d o u b l e ( g e t ( h a n d l e s . ZPos , ’ s t r i n g ’ ) ) ;
g u i d a t a ( hObject , h a n d l e s ) ;
m a r k e r p o s =[ x pos , y pos , z p o s ] ;
87
88
89
90
91
% Se l l a m a a l a f u n c i ó n ’ P o s i t i o n ’ d e l s c r i p t Lua ’ c r e a t e ’
% e n c a r g a d a de p o s i c i o n a r un o b j e t o en l a e s c e n a .
vrep . s i m x C a l l S c r i p t F u n c t i o n ( c l i e n t I D , ’ c r e a t e ’ , vrep .
sim scripttype childscript , . . .
’ P o s i t i o n f u n c t i o n ’ , marker ( num marker ) , marker pos , [ ] , [ ] , vrep .
simx opmode blocking ) ;
92
93
94
95
96
97
98
99
100
%−−− Función que a c t u a l i z a e l v a l o r de l a coordenada y d e l
o b s t á c u l o .
f u n c t i o n YPos Callback ( hObject , ev en td at a , h a n d l e s )
g l o b a l vrep
global clientID
g l o b a l marker
g l o b a l num marker
101
102
103
104
105
106
x p o s=s t r 2 d o u b l e ( g e t ( h a n d l e s . XPos , ’ s t r i n g ’ ) ) ;
y p o s=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
z p o s=s t r 2 d o u b l e ( g e t ( h a n d l e s . ZPos , ’ s t r i n g ’ ) ) ;
g u i d a t a ( hObject , h a n d l e s ) ;
m a r k e r p o s =[ x pos , y pos , z p o s ] ;
107
108
109
110
111
% Se l l a m a a l a f u n c i ó n ’ P o s i t i o n ’ d e l s c r i p t Lua ’ c r e a t e ’
% e n c a r g a d a de p o s i c i o n a r un o b j e t o en l a e s c e n a .
vrep . s i m x C a l l S c r i p t F u n c t i o n ( c l i e n t I D , ’ c r e a t e ’ , vrep .
sim scripttype childscript , . . .
’ P o s i t i o n f u n c t i o n ’ , marker ( num marker ) , marker pos , [ ] , [ ] , vrep .
simx opmode blocking ) ;
112
113
114
115
%−−− Función que a c t u a l i z a e l v a l o r de l a coordenada z d e l
o b s t á c u l o .
Construcción de plataforma software para el diseño y la simulación...
129
A CÓDIGO DE MATLAB
116
117
118
119
120
Carlos Sosa Marrero
f u n c t i o n Z P o s C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l vrep
global clientID
g l o b a l marker
g l o b a l num marker
121
122
123
124
125
126
x p o s=s t r 2 d o u b l e ( g e t ( h a n d l e s . XPos , ’ s t r i n g ’ ) ) ;
y p o s=s t r 2 d o u b l e ( g e t ( h a n d l e s . YPos , ’ s t r i n g ’ ) ) ;
z p o s=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
g u i d a t a ( hObject , h a n d l e s ) ;
m a r k e r p o s =[ x pos , y pos , z p o s ] ;
127
128
129
130
131
% Se l l a m a a l a f u n c i ó n ’ P o s i t i o n ’ d e l s c r i p t Lua ’ c r e a t e ’
% e n c a r g a d a de p o s i c i o n a r un o b j e t o en l a e s c e n a .
vrep . s i m x C a l l S c r i p t F u n c t i o n ( c l i e n t I D , ’ c r e a t e ’ , vrep .
sim scripttype childscript , . . .
’ P o s i t i o n f u n c t i o n ’ , marker ( num marker ) , marker pos , [ ] , [ ] , vrep .
simx opmode blocking ) ;
132
133
134
135
136
137
138
139
140
%−−− Función que s e e j e c u t a a l p u l s a r e l botón NewMarker .
f u n c t i o n NewMarker Callback ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l vrep
global clientID
g l o b a l marker
g l o b a l num marker
141
142
num marker=num marker +1;
143
144
145
146
% Se d e t e r m i n a e l nombre de l a nueva b a l i z a y su p o s i c i ó n por
defecto .
marker name=s t r c a t ( ’ marker ’ , num2str ( num marker ) ) ;
marker pos = [ 0 , 1 , 0 . 2 5 ] ;
147
148
149
150
151
152
153
% Se l l a m a a l a f u n c i ó n ’ CreateMarker ’ d e l s c r i p t Lua ’ c r e a t e ’
% e n c a r g a d a de c r e a r una b a l i z a en l a e s c e n a . Esta d e v u e l v e e l
identificador
% de l a b a l i z a que s e a ñade a l v e c t o r marker .
[ ˜ , r e t I n t s , ˜ , ˜ ,]= vrep . s i m x C a l l S c r i p t F u n c t i o n ( c l i e n t I D , ’
c r e a t e ’ , vrep . s i m s c r i p t t y p e c h i l d s c r i p t , . . .
’ C r e a t e M a r k e r f u n c t i o n ’ , [ ] , marker pos , marker name , [ ] , vrep .
simx opmode blocking ) ;
marker ( num marker )=r e t I n t s ( 1 ) ;
154
Construcción de plataforma software para el diseño y la simulación...
130
A CÓDIGO DE MATLAB
155
156
157
158
159
Carlos Sosa Marrero
% Se f u e r z a n l a s c o o r d e n a d a s c a r t e s i a n a s de l a b a l i z a l o s
v a l o r e s por
% defecto .
s e t ( h a n d l e s . XPos , ’ s t r i n g ’ , m a r k e r p o s ( 1 ) ) ;
s e t ( h a n d l e s . YPos , ’ s t r i n g ’ , m a r k e r p o s ( 2 ) ) ;
s e t ( h a n d l e s . ZPos , ’ s t r i n g ’ , m a r k e r p o s ( 3 ) ) ;
160
161
162
163
%−−− Función que s e e j e c u t a a l p u l s a r e l botón S i m u l a t e .
f u n c t i o n S i m u l a t e C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
g l o b a l next
164
165
166
167
next =1;
c l o s e PosMarker
Simulation ;
168
169
170
171
172
%−−− Función que s e e j e c u t a a l p u l s a r e l botón O b s t a c l e .
f u n c t i o n O b s t a c l e C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
173
174
175
176
next =1;
c l o s e PosMarker
CreaObs
177
178
179
180
181
182
183
184
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Update e n c a r g a d a
de
% a c t u a l i z a r e l v a l o r de l a s c o o r d e n a d a s de l a b a l i z a t r a s
h a b e r l a a r r a s t r a d o por l a ventana de V−REP.
f u n c t i o n U p d a t e C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
g l o b a l vrep
global clientID
g l o b a l marker
g l o b a l num marker
185
186
187
188
189
% Se r e a l i z a una p r i m e r a l e c t u r a de l a p o s i c i ó n de l a b a l i z a
% cuyo r e s u l t a d o , por motivos d e l f u n c i o n a m i e n t o de l a
% API , e s d e s e c h a d o .
[ ˜ , m a r k e r p o s ]= vrep . s i m x G e t O b j e c t P o s i t i o n ( c l i e n t I D , marker (
num marker ) , −1 , vrep . simx opmode streaming ) ;
190
191
192
vrep . simxGetPingTime ( c l i e n t I D ) ;
vrep . s i m x S y n c h r o n o u s T r i g g e r ( c l i e n t I D ) ;
193
194
[ ˜ , m a r k e r p o s ]= vrep . s i m x G e t O b j e c t P o s i t i o n ( c l i e n t I D , marker (
num marker ) , −1 , vrep . s i m x o p m o d e b u f f e r ) ;
Construcción de plataforma software para el diseño y la simulación...
131
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
195
196
197
198
s e t ( h a n d l e s . XPos , ’ s t r i n g ’ , m a r k e r p o s ( 1 ) ) ;
s e t ( h a n d l e s . YPos , ’ s t r i n g ’ , m a r k e r p o s ( 2 ) ) ;
s e t ( h a n d l e s . ZPos , ’ s t r i n g ’ , m a r k e r p o s ( 3 ) ) ;
199
200
g u i d a t a ( hObject , h a n d l e s ) ;
201
202
203
204
205
206
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Cancel .
f u n c t i o n C a n c e l C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
g l o b a l vrep
global clientID
207
208
209
210
211
vrep . s i m x S t o p S i m u l a t i o n ( c l i e n t I D , vrep . simx opmode oneshot ) ;
vrep . simxGetPingTime ( c l i e n t I D ) ;
vrep . s i m x F i n i s h ( c l i e n t I D ) ;
c l o s e PosMarker
212
213
214
215
%−−− Función que s e e j e c u t a cuando s e i n t e n t a c e r r a r l a ventana
.
f u n c t i o n f i g u r e 1 C l o s e R e q u e s t F c n ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
216
217
218
219
220
221
222
223
224
225
226
% S i l a p e t i c i ó n de c i e r r e de l a ventana no e s c o n s e c u e n c i a d e l
normal t r á n s i t o d e l programa de una ventana a o t r a
% s i n o que d e r i v a d e l c l i c por p a r t e d e l u s u a r i o en e l i c o n o de
c i e r r e , s e e x i g e una c o n f i r m a c i ó n .
i f next==0
opc=q u e s t d l g ( ’ ¿Desea s a l i r d e l programa ? ’ , ’ S a l i r ’ , ’ Sı́ ’ , ’No ’ ,
’No ’ ) ;
i f strcmp ( opc , ’No ’ )
return
end
end
next =0;
d e l e t e ( hObject ) ;
Construcción de plataforma software para el diseño y la simulación...
132
A CÓDIGO DE MATLAB
A.13.
1
2
3
4
5
6
7
8
9
10
11
12
Carlos Sosa Marrero
CreaObs.m
f u n c t i o n v a r a r g o u t = CreaObs ( v a r a r g i n )
% Comienzo d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
gui Singleton = 1;
g u i S t a t e = s t r u c t ( ’ gui Name ’ ,
mfilename , . . .
’ gui Singleton ’ , gui Singleton , . . .
’ gui OpeningFcn ’ , @CreaObs OpeningFcn , . . .
’ gui OutputFcn ’ , @CreaObs OutputFcn , . . .
[] , ...
’ gui LayoutFcn ’ ,
’ gui Callback ’ ,
[]) ;
i f n a r g i n && i s c h a r ( v a r a r g i n { 1 } )
g u i S t a t e . gui Callback = s t r 2 f u n c ( varargin {1}) ;
end
13
14
15
16
17
18
19
i f nargout
[ varargout {1: nargout } ] = gui mainfcn ( gui State , varargin { : } )
;
else
gui mainfcn ( gui State , varargin {:}) ;
end
% Fin d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
20
21
22
23
24
25
26
27
28
29
30
31
%−−− Se e j e c u t a j u s t o a n t e s de que CreaObs s e haga v i s i b l e .
f u n c t i o n CreaObs OpeningFcn ( hObject , ev en td at a , h a n d l e s ,
varargin )
% Se p o s i c i o n a l a ventana en l a e s q u i n a s u p e r i o r d e r e c h a de l a
pantalla .
s c r s z=g e t ( 0 , ’ S c r e e n S i z e ’ ) ;
p o s a c t=g e t ( g c f , ’ P o s i t i o n ’ ) ;
xr=s c r s z ( 3 )−p o s a c t ( 3 ) ;
xp=round ( xr / 2 ) ;
yr=s c r s z ( 4 )−p o s a c t ( 4 ) ;
yp=round ( yr / 2 ) ;
s e t ( g c f , ’ p o s i t i o n ’ , [ xp , yp , p o s a c t ( 3 ) , p o s a c t ( 4 ) ] ) ;
32
33
34
h a n d l e s . output = hObject ;
g u i d a t a ( hObject , h a n d l e s ) ;
35
36
37
38
%−−− Las s a l i d a s de e s t a f u n c i ó n s e d e v u e l v e n por l a lı́ n e a de
comandos .
f u n c t i o n v a r a r g o u t = CreaObs OutputFcn ( hObject , ev en td at a ,
handles )
Construcción de plataforma software para el diseño y la simulación...
133
A CÓDIGO DE MATLAB
39
Carlos Sosa Marrero
v a r a r g o u t {1} = h a n d l e s . output ;
40
41
42
43
%−−− Función que s e e j e c u t a a l cambiar l a s e l e c c i ó n de Type .
f u n c t i o n Type Callback ( hObject , ev en td at a , h a n d l e s )
44
45
46
47
48
49
50
51
52
53
54
55
56
%−−− Función que l e e e l v a l o r d e l l a d o o r a d i o , seg ún
c o r r e s p o n d a , de l a b a s e d e l o b s t á c u l o y v e r i f i c a
% que s e a un número p o s i t i v o .
f u n c t i o n S i d e R a d C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
s i d e r a d=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( s i d e r a d ) | | s i g n ( s i d e r a d ) ˜=1)
e r r o r d l g ( ’ El l a d o / r a d i o d e l o b s t á c u l o ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
s i d e r a d =0;
s e t ( hObject , ’ s t r i n g ’ , s i d e r a d ) ;
end
h a n d l e s . SideRad=s i d e r a d ;
g u i d a t a ( hObject , h a n d l e s ) ;
57
58
59
60
61
62
63
64
65
66
67
68
69
%−−− Función que l e e e l v a l o r de l a a l t u r a d e l o b s t á c u l o y
verifica
% que s e a un número p o s i t i v o .
f u n c t i o n H e i g h t C a l l b a c k ( hObject , e v en tda ta , h a n d l e s )
h e i g h t=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
i f ( i s n a n ( h e i g h t ) | | s i g n ( h e i g h t ) ˜=1)
e r r o r d l g ( ’ La a l t u r a d e l o b s t á c u l o ha de s e r un número
p o s i t i v o . ’ , ’ERROR ’ )
h e i g h t =0;
s e t ( hObject , ’ s t r i n g ’ , h e i g h t ) ;
end
h a n d l e s . Height=h e i g h t ;
g u i d a t a ( hObject , h a n d l e s ) ;
70
71
72
73
74
75
76
77
78
79
%−−− Función que s e e j e c u t a a l p u l s a r e l botón C r e a t e .
f u n c t i o n C r e a t e C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l vrep
global clientID
g l o b a l next
global obstacle
global obs pos
g l o b a l num obs
Construcción de plataforma software para el diseño y la simulación...
134
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
80
81
num obs=num obs +1;
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
% Se d e t e r m i n a e l t i p o de o b s t á c u l o , s u s d i m e n s i o n e s y su
p o s i c i ó n por
% d e f e c t o a p a r t i r de l o s d a t o s i n t r o d u c i d o s por e l u s u a r i o . Se
define
% asimismo su masa y su nombre .
s e l=g e t ( h a n d l e s . Type , ’ v a l u e ’ ) ;
switch s e l
case 1
o b s t y p e =0;
o b s s i z e =[ h a n d l e s . SideRad , h a n d l e s . SideRad , h a n d l e s . Height
];
case 2
o b s t y p e =2;
o b s s i z e =[2∗ h a n d l e s . SideRad , 2 ∗ h a n d l e s . SideRad , h a n d l e s .
Height ] ;
case 3
o b s t y p e =3;
o b s s i z e =[2∗ h a n d l e s . SideRad , 2 ∗ h a n d l e s . SideRad , h a n d l e s .
Height ] ;
97
98
99
100
101
end
o b s p o s = [ 0 . 0 , 1 . 0 , h a n d l e s . Height / 2 ] ;
obs mass = 1 . 0 ;
obs name=s t r c a t ( ’ o b s t a c l e ’ , num2str ( num obs ) ) ;
102
103
104
105
106
107
% Se l l a m a a l a f u n c i ó n ’ C r e a t e O b s t a c l e ’ d e l s c r i p t Lua ’ c r e a t e ’
% e n c a r g a d a de c r e a r un o b s t á c u l o en l a e s c e n a . Esta d e v u e l v e e l
% i d e n t i f i c a d o r d e l o b s t á c u l o que s e a ñade a l v e c t o r o b s t a c l e .
[ ˜ , r e t I n t s , ˜ , ˜ ,]= vrep . s i m x C a l l S c r i p t F u n c t i o n ( c l i e n t I D , ’
c r e a t e ’ , vrep . s i m s c r i p t t y p e c h i l d s c r i p t , . . .
’ CreateObstacle function ’ , obs type , [ o b s s i z e , obs pos ,
obs mass ] , obs name , [ ] , vrep . s i m x o p m o d e b l o c k i n g ) ;
108
109
o b s t a c l e ( num obs )=r e t I n t s ( 1 ) ;
110
111
112
113
114
vrep . s i m x S y n c h r o n o u s T r i g g e r ( c l i e n t I D ) ;
next =1;
c l o s e CreaObs
PosObs ;
115
116
117
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Cancel .
Construcción de plataforma software para el diseño y la simulación...
135
A CÓDIGO DE MATLAB
118
119
120
Carlos Sosa Marrero
f u n c t i o n C a n c e l C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
g l o b a l vrep
global clientID
121
122
123
124
125
126
vrep . s i m x S t o p S i m u l a t i o n ( c l i e n t I D , vrep . simx opmode oneshot ) ;
vrep . s i m x C l o s e S c e n e ( c l i e n t I D , vrep . s i m x o p m o d e b l o c k i n g ) ;
vrep . simxGetPingTime ( c l i e n t I D ) ;
vrep . s i m x F i n i s h ( c l i e n t I D ) ;
c l o s e CreaObs
127
128
129
130
131
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Skip .
f u n c t i o n S k i p C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
132
133
134
135
next =1;
c l o s e CreaObs
Simulation
136
137
138
139
%−−− Función que s e e j e c u t a cuando s e i n t e n t a c e r r a r l a ventana
.
f u n c t i o n f i g u r e 1 C l o s e R e q u e s t F c n ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
140
141
142
143
144
145
146
147
148
149
150
% S i l a p e t i c i ó n de c i e r r e de l a ventana no e s c o n s e c u e n c i a d e l
normal t r á n s i t o d e l programa de una ventana a o t r a
% s i n o que d e r i v a d e l c l i c por p a r t e d e l u s u a r i o en e l i c o n o de
c i e r r e , s e e x i g e una c o n f i r m a c i ó n .
i f next==0
opc=q u e s t d l g ( ’ ¿Desea s a l i r d e l programa ? ’ , ’ S a l i r ’ , ’ Sı́ ’ , ’No ’ ,
’No ’ ) ;
i f strcmp ( opc , ’No ’ )
return
end
end
next =0;
d e l e t e ( hObject ) ;
Construcción de plataforma software para el diseño y la simulación...
136
A CÓDIGO DE MATLAB
A.14.
1
2
3
4
5
6
7
8
9
10
11
12
Carlos Sosa Marrero
PosObs.m
f u n c t i o n v a r a r g o u t = PosObs ( v a r a r g i n )
% Comienzo d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
gui Singleton = 1;
g u i S t a t e = s t r u c t ( ’ gui Name ’ ,
mfilename , . . .
’ gui Singleton ’ , gui Singleton , . . .
’ gui OpeningFcn ’ , @PosObs OpeningFcn , . . .
’ gui OutputFcn ’ , @PosObs OutputFcn , . . .
[] , ...
’ gui LayoutFcn ’ ,
’ gui Callback ’ ,
[]) ;
i f n a r g i n && i s c h a r ( v a r a r g i n { 1 } )
g u i S t a t e . gui Callback = s t r 2 f u n c ( varargin {1}) ;
end
13
14
15
16
17
18
19
i f nargout
[ varargout {1: nargout } ] = gui mainfcn ( gui State , varargin { : } )
;
else
gui mainfcn ( gui State , varargin {:}) ;
end
% Fin d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
20
21
22
23
24
25
26
27
28
29
30
31
32
%−−− Se e j e c u t a j u s t o a n t e s de que PosObs s e haga v i s i b l e .
f u n c t i o n PosObs OpeningFcn ( hObject , e ve nt da ta , h a n d l e s , v a r a r g i n
)
global obs pos
% Se p o s i c i o n a l a ventana en l a e s q u i n a s u p e r i o r d e r e c h a de l a
pantalla .
s c r s z=g e t ( 0 , ’ S c r e e n S i z e ’ ) ;
p o s a c t=g e t ( g c f , ’ P o s i t i o n ’ ) ;
xr=s c r s z ( 3 )−p o s a c t ( 3 ) ;
xp=round ( xr / 2 ) ;
yr=s c r s z ( 4 )−p o s a c t ( 4 ) ;
yp=round ( yr / 2 ) ;
s e t ( g c f , ’ p o s i t i o n ’ , [ xp , yp , p o s a c t ( 3 ) , p o s a c t ( 4 ) ] ) ;
33
34
35
36
37
38
% Se i n i c i a l i z a n l a s c o o r d e n a d a s c a r t e s i a n a s d e l o b s t á c u l o a l o s
v a l o r e s por
% d e f e c t o d e t e r m i n a d o s en l a ventana a n t e r i o r .
s e t ( h a n d l e s . XPos , ’ s t r i n g ’ , o b s p o s ( 1 ) ) ;
s e t ( h a n d l e s . YPos , ’ s t r i n g ’ , o b s p o s ( 2 ) ) ;
s e t ( h a n d l e s . ZPos , ’ s t r i n g ’ , o b s p o s ( 3 ) ) ;
39
Construcción de plataforma software para el diseño y la simulación...
137
A CÓDIGO DE MATLAB
40
41
42
43
44
45
46
47
Carlos Sosa Marrero
% Se muestra l a imagen d e l i c o n o a s o c i a d o a l d e s p l a z a m i e n t o de
un o b j e t o en
% V−REP.
axes ( handles . axes1 )
path=pwd ;
image=imread ( [ path , ’ \ o b j e c t S h i f t B u t t o n . j p g ’ ] ) ;
image=i m r e s i z e ( image , [ 2 5 , NaN ] ) ;
axis off
imshow ( image )
48
49
50
h a n d l e s . output = hObject ;
g u i d a t a ( hObject , h a n d l e s ) ;
51
52
53
54
55
%−−− Las s a l i d a s de e s t a f u n c i ó n s e d e v u e l v e n por l a lı́ n e a de
comandos .
f u n c t i o n v a r a r g o u t = PosObs OutputFcn ( hObject , ev en td at a ,
handles )
v a r a r g o u t {1} = h a n d l e s . output ;
56
57
58
59
60
61
62
63
%−−− Función que a c t u a l i z a e l v a l o r de l a coordenada x d e l
o b s t á c u l o .
f u n c t i o n XPos Callback ( hObject , ev en td at a , h a n d l e s )
g l o b a l vrep
global clientID
global obstacle
g l o b a l num obs
64
65
66
67
68
69
x p o s=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
y p o s=s t r 2 d o u b l e ( g e t ( h a n d l e s . YPos , ’ s t r i n g ’ ) ) ;
z p o s=s t r 2 d o u b l e ( g e t ( h a n d l e s . ZPos , ’ s t r i n g ’ ) ) ;
g u i d a t a ( hObject , h a n d l e s ) ;
o b s p o s =[ x pos , y pos , z p o s ] ;
70
71
72
73
74
% Se l l a m a a l a f u n c i ó n ’ P o s i t i o n ’ d e l s c r i p t Lua ’ c r e a t e ’
% e n c a r g a d a de p o s i c i o n a r un o b j e t o en l a e s c e n a .
vrep . s i m x C a l l S c r i p t F u n c t i o n ( c l i e n t I D , ’ c r e a t e ’ , vrep .
sim scripttype childscript , . . .
’ P o s i t i o n f u n c t i o n ’ , o b s t a c l e ( num obs ) , o b s p o s , [ ] , [ ] , vrep .
simx opmode blocking ) ;
75
76
77
78
%−−− Función que a c t u a l i z a e l v a l o r de l a coordenada y d e l
Construcción de plataforma software para el diseño y la simulación...
138
A CÓDIGO DE MATLAB
79
80
81
82
83
Carlos Sosa Marrero
o b s t á c u l o .
f u n c t i o n YPos Callback ( hObject , ev en td at a , h a n d l e s )
g l o b a l vrep
global clientID
global obstacle
g l o b a l num obs
84
85
86
87
88
89
x p o s=s t r 2 d o u b l e ( g e t ( h a n d l e s . XPos , ’ s t r i n g ’ ) ) ;
y p o s=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
z p o s=s t r 2 d o u b l e ( g e t ( h a n d l e s . ZPos , ’ s t r i n g ’ ) ) ;
g u i d a t a ( hObject , h a n d l e s ) ;
o b s p o s =[ x pos , y pos , z p o s ] ;
90
91
92
93
94
% Se l l a m a a l a f u n c i ó n ’ P o s i t i o n ’ d e l s c r i p t Lua ’ c r e a t e ’
% e n c a r g a d a de p o s i c i o n a r un o b j e t o en l a e s c e n a .
vrep . s i m x C a l l S c r i p t F u n c t i o n ( c l i e n t I D , ’ c r e a t e ’ , vrep .
sim scripttype childscript , . . .
’ P o s i t i o n f u n c t i o n ’ , o b s t a c l e ( num obs ) , o b s p o s , [ ] , [ ] , vrep .
simx opmode blocking ) ;
95
96
97
98
99
100
101
102
103
%−−− Función que a c t u a l i z a e l v a l o r de l a coordenada z d e l
o b s t á c u l o .
f u n c t i o n Z P o s C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l vrep
global clientID
global obstacle
g l o b a l num obs
104
105
106
107
108
109
x p o s=s t r 2 d o u b l e ( g e t ( h a n d l e s . XPos , ’ s t r i n g ’ ) ) ;
y p o s=s t r 2 d o u b l e ( g e t ( h a n d l e s . YPos , ’ s t r i n g ’ ) ) ;
z p o s=s t r 2 d o u b l e ( g e t ( hObject , ’ s t r i n g ’ ) ) ;
g u i d a t a ( hObject , h a n d l e s ) ;
o b s p o s =[ x pos , y pos , z p o s ] ;
110
111
112
113
114
% Se l l a m a a l a f u n c i ó n ’ P o s i t i o n ’ d e l s c r i p t Lua ’ c r e a t e ’
% e n c a r g a d a de p o s i c i o n a r un o b j e t o en l a e s c e n a .
vrep . s i m x C a l l S c r i p t F u n c t i o n ( c l i e n t I D , ’ c r e a t e ’ , vrep .
sim scripttype childscript , . . .
’ P o s i t i o n f u n c t i o n ’ , o b s t a c l e ( num obs ) , o b s p o s , [ ] , [ ] , vrep .
simx opmode blocking ) ;
115
116
117
%−−− Función que s e e j e c u t a a l p u l s a r e l botón NewObstacle .
Construcción de plataforma software para el diseño y la simulación...
139
A CÓDIGO DE MATLAB
118
119
Carlos Sosa Marrero
f u n c t i o n N e w O b s t a c l e C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
g l o b a l next
120
121
122
123
next =1;
c l o s e PosObs
CreaObs ;
124
125
126
127
128
%−−− Función que s e e j e c u t a a l p u l s a r e l botón S i m u l a t e .
f u n c t i o n S i m u l a t e C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
g l o b a l next
129
130
131
132
next =1;
c l o s e PosObs
Simulation ;
133
134
135
136
137
138
139
140
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Update e n c a r g a d a
de a c t u a l i z a r e l v a l o r de l a s c o o r d e n a d a s d e l o b s t á c u l o t r a s
h a b e r l o a r r a s t r a d o por l a ventana de V−REP.
f u n c t i o n U p d a t e C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
g l o b a l vrep
global clientID
global obstacle
g l o b a l num obs
141
142
143
144
145
% Se r e a l i z a una p r i m e r a l e c t u r a de l a p o s i c i ó n d e l o b s t á c u l o
% cuyo r e s u l t a d o , por motivos d e l f u n c i o n a m i e n t o de l a
% API , e s d e s e c h a d o .
[ ˜ , o b s p o s ]= vrep . s i m x G e t O b j e c t P o s i t i o n ( c l i e n t I D , o b s t a c l e ( num obs
) , −1 , vrep . simx opmode streaming ) ;
146
147
148
vrep . s i m x S y n c h r o n o u s T r i g g e r ( c l i e n t I D ) ;
vrep . simxGetPingTime ( c l i e n t I D ) ;
149
150
[ ˜ , o b s p o s ]= vrep . s i m x G e t O b j e c t P o s i t i o n ( c l i e n t I D , o b s t a c l e ( num obs
) , −1 , vrep . s i m x o p m o d e b u f f e r ) ;
151
152
153
154
s e t ( h a n d l e s . XPos , ’ s t r i n g ’ , o b s p o s ( 1 ) ) ;
s e t ( h a n d l e s . YPos , ’ s t r i n g ’ , o b s p o s ( 2 ) ) ;
s e t ( h a n d l e s . ZPos , ’ s t r i n g ’ , o b s p o s ( 3 ) ) ;
155
156
g u i d a t a ( hObject , h a n d l e s ) ;
157
158
Construcción de plataforma software para el diseño y la simulación...
140
A CÓDIGO DE MATLAB
159
160
161
162
Carlos Sosa Marrero
%−−− Función que s e e j e c u t a a l p u l s a r e l botón Cancel .
f u n c t i o n C a n c e l C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
g l o b a l vrep
global clientID
163
164
165
166
167
vrep . s i m x S t o p S i m u l a t i o n ( c l i e n t I D , vrep . simx opmode oneshot ) ;
vrep . simxGetPingTime ( c l i e n t I D ) ;
vrep . s i m x F i n i s h ( c l i e n t I D ) ;
c l o s e PosObs
168
169
170
171
172
%−−− Función que s e e j e c u t a cuando s e i n t e n t a c e r r a r l a ventana
.
f u n c t i o n f i g u r e 1 C l o s e R e q u e s t F c n ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
173
174
175
176
177
178
179
180
181
182
183
% S i l a p e t i c i ó n de c i e r r e de l a ventana no e s c o n s e c u e n c i a d e l
normal t r á n s i t o d e l programa de una ventana a o t r a
% s i n o que d e r i v a d e l c l i c por p a r t e d e l u s u a r i o en e l i c o n o de
c i e r r e , s e e x i g e una c o n f i r m a c i ó n .
i f next==0
opc=q u e s t d l g ( ’ ¿Desea s a l i r d e l programa ? ’ , ’ S a l i r ’ , ’ Sı́ ’ , ’No ’ ,
’No ’ ) ;
i f strcmp ( opc , ’No ’ )
return
end
end
next =0;
d e l e t e ( hObject ) ;
Construcción de plataforma software para el diseño y la simulación...
141
A CÓDIGO DE MATLAB
A.15.
1
2
3
4
5
6
7
8
9
10
11
12
Carlos Sosa Marrero
Simulation.m
function varargout = Simulation ( varargin )
% Comienzo d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
gui Singleton = 1;
g u i S t a t e = s t r u c t ( ’ gui Name ’ ,
mfilename , . . .
’ gui Singleton ’ , gui Singleton , . . .
’ gui OpeningFcn ’ , @Simulation OpeningFcn , . . .
’ gui OutputFcn ’ , @Simulation OutputFcn , . . .
[] , ...
’ gui LayoutFcn ’ ,
’ gui Callback ’ ,
[]) ;
i f n a r g i n && i s c h a r ( v a r a r g i n { 1 } )
g u i S t a t e . gui Callback = s t r 2 f u n c ( varargin {1}) ;
end
13
14
15
16
17
18
19
i f nargout
[ varargout {1: nargout } ] = gui mainfcn ( gui State , varargin { : } )
;
else
gui mainfcn ( gui State , varargin {:}) ;
end
% Fin d e l c ó d i g o de i n i c i a l i z a c i ó n − NO EDITAR
20
21
22
23
24
25
26
27
28
29
30
31
%−−− Se e j e c u t a j u s t o a n t e s de que S i m u l a t i o n s e haga v i s i b l e .
f u n c t i o n S i m u l a t i o n O p e n i ng F c n ( hObject , ev en td at a , h a n d l e s ,
varargin )
% Se p o s i c i o n a l a ventana en l a e s q u i n a s u p e r i o r d e r e c h a de l a
pantalla .
s c r s z=g e t ( 0 , ’ S c r e e n S i z e ’ ) ;
p o s a c t=g e t ( g c f , ’ P o s i t i o n ’ ) ;
xr=s c r s z ( 3 )−p o s a c t ( 3 ) ;
xp=round ( xr / 2 ) ;
yr=s c r s z ( 4 )−p o s a c t ( 4 ) ;
yp=round ( yr / 2 ) ;
s e t ( g c f , ’ p o s i t i o n ’ , [ xp , yp , p o s a c t ( 3 ) , p o s a c t ( 4 ) ] ) ;
32
33
34
h a n d l e s . output = hObject ;
g u i d a t a ( hObject , h a n d l e s ) ;
35
36
37
38
%−−− Las s a l i d a s de e s t a f u n c i ó n s e d e v u e l v e n por l a lı́ n e a de
comandos .
f u n c t i o n v a r a r g o u t = Simulation OutputFcn ( hObject , e ve ntd at a ,
handles )
Construcción de plataforma software para el diseño y la simulación...
142
A CÓDIGO DE MATLAB
39
Carlos Sosa Marrero
v a r a r g o u t {1} = h a n d l e s . output ;
40
41
42
43
44
45
46
47
48
49
50
51
52
%−−− Función que s e e j e c u t a a l p u l s a r e l botón ’ PlaySim ’
e n c a r g a d a de
% i n i c i a r o retomar l a s i m u l a c i ó n y d e l c o n t r o l d e l r o b o t
d u r a n t e l a misma .
f u n c t i o n P l a y S i m C a l l b a c k ( hObject , e ve nt dat a , h a n d l e s )
g l o b a l vrep
global clientID
g l o b a l model
global scene
global w
global a x i s d i s t f r
global a x i s d i s t l r
global i
53
54
55
% Se i n i c i a l i z a l a v a r i a b l e a s o c i a d a a l b u c l e de c o n t r o l .
i =1;
56
57
58
59
60
61
62
63
% Se i n i c i a l i z a n l a s v a r i a b l e s que ayudarán a d e f i n i r l a s
maniobras d e l
% robot .
b a c k u n t i l t i m e =0;
s t e e r u n t i l t i m e =0;
f o r w a r d =0; % s e pondrá a 1 cuando e l r o b o t s e d i r i j a en lı́ n e a
r e c t a h a c i a una b a l i z a
s t e e r =1; % v a r i a r á a 1 o −1 seg ún e l s e n t i d o de g i r o d e l r o b o t
deseado
i n =0; % s e pondrá a 1 cuando e l r o b o t haya a l c a n z a d o e l c i r c u i t o
64
65
66
67
68
69
70
71
72
73
% S i s e ha e s c o g i d o r e a l i z a r l a s i m u l a c i ó n en una e s c e n a v a cı́ a o
en una
% c a r g a d a por e l u s u a r i o , s e p r e g u n t a por e l comportamiento d e l
robot
% deseado .
i f ( s c e n e ==4|| s c e n e ==5)
opc=q u e s t d l g ( ’ S e l e c c i o n e e l comportamiento d e s e a d o d e l r o b o t
: ’ , ’ Comportamiento ’ , ’ E v i t a r o b s t á c u l o s ’ , ’ S e g u i r c i r c u i t o
y e v i t a r o b s t á c u l o s ’ , ’ D i r i g i r s e a l a s b a l i z a s y e v i t a r
o b s t á c u l o s ’ , ’ E v i t a r o b s t á c u l o s ’ ) ;
i f strcmp ( opc , ’ E v i t a r o b s t á c u l o s ’ )
s c e n e =1;
e l s e i f strcmp ( opc , ’ S e g u i r c i r c u i t o y e v i t a r o b s t á c u l o s ’ )
s c e n e =2;
Construcción de plataforma software para el diseño y la simulación...
143
A CÓDIGO DE MATLAB
e l s e i f strcmp ( opc , ’ D i r i g i r s e a l a s b a l i z a s y e v i t a r
o b s t á c u l o s ’ )
s c e n e =3;
end
74
75
76
77
Carlos Sosa Marrero
end
78
79
vrep . s i m x S t a r t S i m u l a t i o n ( c l i e n t I D , vrep . simx opmode oneshot ) ;
80
81
82
83
84
% Se l l a m a a l a f u n c i ó n ’ Hide ’ d e l s c r i p t Lua ’ c r e a t e ’
% e n c a r g a d a de o c u l t a r l o s s e n s o r e s que no i n t e r v e n g a n en e l
comportamiento d e l r o b o t en l a e s c e n a e l e g i d a a .
vrep . s i m x C a l l S c r i p t F u n c t i o n ( c l i e n t I D , ’ c r e a t e ’ , vrep .
s i m s c r i p t t y p e c h i l d s c r i p t , ’ Hide function ’ , scene , . . .
[ ] , [ ] , [ ] , vrep . s i m x o p m o d e b l o c k i n g ) ;
85
86
87
88
89
90
91
92
% Se d e f i n e e l comportamiento d e l r o b o t seg ún l a e s c e n a y e l
modelo
% elegidos .
switch scene
case 1
s w i t c h model
case 1
%−−− Robot de t i p o d i f e r e n c i a l en e s c e n a con
o b s t á c u l o s
93
94
95
96
97
98
% Se o b t i e n e n l o s i d e n t i f i c a d o r e s de l o s motores
y el sensor
% de proximidad .
[ ˜ , l e f t m o t o r ]= vrep . simxGetObjectHandle ( c l i e n t I D
, ’ d i f f l e f t m o t o r ’ , vrep . s i m x o p m o d e b l o c k i n g )
;
[ ˜ , r i g h t m o t o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ d i f f r i g h t m o t o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , p r o x s e n s o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ d i f f p r o x s e n s o r ’ , vrep .
simx opmode blocking ) ;
99
100
101
102
103
% Se r e a l i z a una p r i m e r a l e c t u r a d e l s e n s o r de
proximidad
% cuyo r e s u l t a d o , por motivos d e l f u n c i o n a m i e n t o
de l a
% API , e s d e s e c h a d o .
[ ˜ , p r o x d e t S t a t e , ˜ , ˜ , ˜ ] = vrep .
simxReadProximitySensor ( c l i e n t I D , p r o x s e n s o r ,
Construcción de plataforma software para el diseño y la simulación...
144
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
vrep . simx opmode streaming ) ;
104
105
106
vrep . s i m x S y n c h r o n o u s T r i g g e r ( c l i e n t I D ) ;
vrep . simxGetPingTime ( c l i e n t I D ) ;
107
108
109
110
% Se i n i c i a e l b u c l e de c o n t r o l d e l r o b o t .
w h i l e ( i ==1)
[ ˜ , p r o x d e t S t a t e , ˜ , ˜ , ˜ ] = vrep .
simxReadProximitySensor ( c l i e n t I D ,
p r o x s e n s o r , vrep . s i m x o p m o d e b u f f e r ) ;
111
c u r r e n t t i m e=vrep . simxGetLastCmdTime (
clientID ) ;
112
113
i f ( p r o x d e t S t a t e ==1)
b a c k u n t i l t i m e=c u r r e n t t i m e +50/ h a n d l e s .
LinVel ;
end
114
115
116
117
i f ( c u r r e n t t i m e <b a c k u n t i l t i m e )
wl=−4∗w/ 5 ;
wr=−6∗w/ 5 ;
s t e e r u n t i l t i m e=b a c k u n t i l t i m e +50/
h a n d l e s . LinVel ;
118
119
120
121
122
e l s e i f ( c u r r e n t t i m e <s t e e r u n t i l t i m e )
wl=6∗w/ 5 ;
wr=4∗w/ 5 ;
123
124
125
126
else
127
wl=w ;
wr=w ;
128
129
130
end
vrep . s i m x S e t J o i n t T a r g e t V e l o c i t y ( c l i e n t I D ,
l e f t m o t o r , wl , vrep . simx opmode oneshot ) ;
vrep . s i m x S e t J o i n t T a r g e t V e l o c i t y ( c l i e n t I D ,
r i g h t m o t o r , wr , vrep . simx opmode oneshot ) ;
131
132
133
134
vrep . s i m x S y n c h r o n o u s T r i g g e r ( c l i e n t I D ) ;
135
136
pause ( 0 . 0 1 )
137
138
end
139
140
case 2
Construcción de plataforma software para el diseño y la simulación...
145
A CÓDIGO DE MATLAB
141
Carlos Sosa Marrero
%−−− Robot de t i p o t r i c i c l o en e s c e n a con
o b s t á c u l o s
142
143
144
145
146
147
148
% Se o b t i e n e n l o s i d e n t i f i c a d o r e s de l o s motores
y el sensor
% de proximidad .
[ ˜ , l e f t m o t o r ]= vrep . simxGetObjectHandle ( c l i e n t I D
, ’ t r i c y c l e l e f t m o t o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , r i g h t m o t o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ t r i c y c l e r i g h t m o t o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , f r o n t m o t o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ t r i c y c l e f r o n t m o t o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , p r o x s e n s o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ t r i c y c l e p r o x s e n s o r ’ , vrep .
simx opmode blocking ) ;
149
150
151
152
153
% Se r e a l i z a una p r i m e r a l e c t u r a d e l s e n s o r de
proximidad
% cuyo r e s u l t a d o , por motivos d e l f u n c i o n a m i e n t o
de l a
% API , e s d e s e c h a d o .
[ ˜ , p r o x d e t S t a t e , ˜ , ˜ , ˜ ] = vrep .
simxReadProximitySensor ( c l i e n t I D , p r o x s e n s o r ,
vrep . simx opmode streaming ) ;
154
155
156
vrep . s i m x S y n c h r o n o u s T r i g g e r ( c l i e n t I D ) ;
vrep . simxGetPingTime ( c l i e n t I D ) ;
157
158
159
160
% Se i n i c i a e l b u c l e de c o n t r o l d e l r o b o t .
w h i l e ( i ==1)
[ ˜ , p r o x d e t S t a t e , ˜ , ˜ , ˜ ] = vrep .
simxReadProximitySensor ( c l i e n t I D ,
p r o x s e n s o r , vrep . s i m x o p m o d e b u f f e r ) ;
161
162
c u r r e n t t i m e=vrep . simxGetLastCmdTime (
clientID ) ;
163
164
165
166
i f ( p r o x d e t S t a t e ==1)
b a c k u n t i l t i m e=c u r r e n t t i m e +200/ h a n d l e s
. LinVel ;
end
167
Construcción de plataforma software para el diseño y la simulación...
146
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
i f ( c u r r e n t t i m e <b a c k u n t i l t i m e )
wl=−w ;
wr=−w ;
t h e t a=p i / 6 ;
s t e e r u n t i l t i m e=b a c k u n t i l t i m e +200/
h a n d l e s . LinVel ;
168
169
170
171
172
173
e l s e i f ( c u r r e n t t i m e <s t e e r u n t i l t i m e )
wl=w ;
wr=w ;
t h e t a=−p i / 6 ;
174
175
176
177
178
else
179
wl=w ;
wr=w ;
t h e t a =0;
180
181
182
end
183
184
vrep . s i m x S e t J o i n t T a r g e t V e l o c i t y ( c l i e n t I D ,
l e f t m o t o r , wl , vrep . simx opmode oneshot ) ;
vrep . s i m x S e t J o i n t T a r g e t V e l o c i t y ( c l i e n t I D ,
r i g h t m o t o r , wr , vrep . simx opmode oneshot ) ;
vrep . s i m x S e t J o i n t T a r g e t P o s i t i o n ( c l i e n t I D ,
f r o n t m o t o r , t h e t a , vrep .
simx opmode oneshot ) ;
185
186
187
188
vrep . s i m x S y n c h r o n o u s T r i g g e r ( c l i e n t I D ) ;
pause ( 0 . 0 1 )
189
190
191
end
192
193
194
case 3
%−−− Robot de t i p o c u a t r i c i c l o en e s c e n a con
o b s t á c u l o s
195
196
197
198
199
200
% Se o b t i e n e n l o s i d e n t i f i c a d o r e s de l o s motores
y el sensor
% de proximidad .
[ ˜ , r e a r l e f t m o t o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ q u a d r i c y c l e r e a r l e f t m o t o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , r e a r r i g h t m o t o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ q u a d r i c y c l e r e a r r i g h t m o t o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , f r o n t l e f t m o t o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ q u a d r i c y c l e f r o n t l e f t m o t o r ’ , vrep .
Construcción de plataforma software para el diseño y la simulación...
147
A CÓDIGO DE MATLAB
201
202
Carlos Sosa Marrero
simx opmode blocking ) ;
[ ˜ , f r o n t r i g h t m o t o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ q u a d r i c y c l e f r o n t r i g h t m o t o r ’ , vrep
. simx opmode blocking ) ;
[ ˜ , p r o x s e n s o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ q u a d r i c y c l e p r o x s e n s o r ’ , vrep .
simx opmode blocking ) ;
203
204
205
206
207
% Se r e a l i z a una p r i m e r a l e c t u r a d e l s e n s o r de
proximidad
% cuyo r e s u l t a d o , por motivos d e l f u n c i o n a m i e n t o
de l a
% API , e s d e s e c h a d o .
[ ˜ , p r o x d e t S t a t e , ˜ , ˜ , ˜ ] = vrep .
simxReadProximitySensor ( c l i e n t I D , p r o x s e n s o r ,
vrep . simx opmode streaming ) ;
208
209
210
vrep . s i m x S y n c h r o n o u s T r i g g e r ( c l i e n t I D ) ;
vrep . simxGetPingTime ( c l i e n t I D ) ;
211
212
213
214
% Se i n i c i a e l b u c l e de c o n t r o l d e l r o b o t .
w h i l e ( i ==1)
[ ˜ , p r o x d e t S t a t e , ˜ , ˜ , ˜ ] = vrep .
simxReadProximitySensor ( c l i e n t I D ,
p r o x s e n s o r , vrep . s i m x o p m o d e b u f f e r ) ;
215
216
c u r r e n t t i m e=vrep . simxGetLastCmdTime (
clientID ) ;
217
218
219
220
i f ( p r o x d e t S t a t e ==1)
b a c k u n t i l t i m e=c u r r e n t t i m e +200/ h a n d l e s
. LinVel ;
end
221
222
223
224
225
226
i f ( c u r r e n t t i m e <b a c k u n t i l t i m e )
wl=−w ;
wr=−w ;
t h e t a=p i / 6 ;
s t e e r u n t i l t i m e=b a c k u n t i l t i m e +200/
h a n d l e s . LinVel ;
227
228
229
230
231
e l s e i f ( c u r r e n t t i m e <s t e e r u n t i l t i m e )
wl=w ;
wr=w ;
t h e t a=−p i / 6 ;
Construcción de plataforma software para el diseño y la simulación...
148
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
232
else
233
wl=w ;
wr=w ;
t h e t a =0;
234
235
236
end
237
238
t h e t a l e f t=a c o t ( c o t ( t h e t a ) −( a x i s d i s t l r / 2 ) /
axis dist fr ) ;
t h e t a r i g h t=a c o t ( c o t ( t h e t a ) +( a x i s d i s t l r / 2 )
/axis dist fr ) ;
239
240
241
vrep . s i m x S e t J o i n t T a r g e t V e l o c i t y ( c l i e n t I D
r e a r l e f t m o t o r , wl , vrep .
simx opmode oneshot ) ;
vrep . s i m x S e t J o i n t T a r g e t V e l o c i t y ( c l i e n t I D
r e a r r i g h t m o t o r , wr , vrep .
simx opmode oneshot ) ;
vrep . s i m x S e t J o i n t T a r g e t P o s i t i o n ( c l i e n t I D
f r o n t l e f t m o t o r , t h e t a l e f t , vrep .
simx opmode oneshot ) ;
vrep . s i m x S e t J o i n t T a r g e t P o s i t i o n ( c l i e n t I D
f r o n t r i g h t m o t o r , t h e t a r i g h t , vrep .
simx opmode oneshot ) ;
242
243
244
245
,
,
,
,
246
vrep . s i m x S y n c h r o n o u s T r i g g e r ( c l i e n t I D ) ;
pause ( 0 . 0 1 )
247
248
end
249
250
end
251
252
253
254
255
case 2
s w i t c h model
case 1
%−−− Robot de t i p o d i f e r e n c i a l en e s c e n a con
circuito
256
257
258
259
% Se o b t i e n e n l o s i d e n t i f i c a d o r e s de l o s motores
y l o s s e n s o r e s de proximidad y v i s i ó n
i z q u i e r d o y derecho .
[ ˜ , l e f t m o t o r ]= vrep . simxGetObjectHandle ( c l i e n t I D
, ’ d i f f l e f t m o t o r ’ , vrep . s i m x o p m o d e b l o c k i n g )
;
[ ˜ , r i g h t m o t o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ d i f f r i g h t m o t o r ’ , vrep .
simx opmode blocking ) ;
Construcción de plataforma software para el diseño y la simulación...
149
A CÓDIGO DE MATLAB
260
261
262
Carlos Sosa Marrero
[ ˜ , p r o x s e n s o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ d i f f p r o x s e n s o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , l e f t v i s i o n s e n s o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ d i f f l e f t v i s i o n s e n s o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , r i g h t v i s i o n s e n s o r ]= vrep . simxGetObjectHandle
( c l i e n t I D , ’ d i f f r i g h t v i s i o n s e n s o r ’ , vrep .
simx opmode blocking ) ;
263
264
265
266
267
268
269
% Se r e a l i z a una p r i m e r a l e c t u r a de l o s s e n s o r e s
% cuyo r e s u l t a d o , por motivos d e l f u n c i o n a m i e n t o
de l a
% API , e s d e s e c h a d o .
[ ˜ , p r o x d e t S t a t e , ˜ , ˜ , ˜ ] = vrep .
simxReadProximitySensor ( c l i e n t I D , p r o x s e n s o r ,
vrep . simx opmode streaming ) ;
[ ˜ , ˜ , l e f t v i s i o n a u x D a t a , ˜ ] = vrep .
simxReadVisionSensor ( clientID ,
l e f t v i s i o n s e n s o r , vrep . simx opmode streaming
);
[ ˜ , ˜ , r i g h t v i s i o n a u x D a t a , ˜ ] = vrep .
simxReadVisionSensor ( clientID ,
r i g h t v i s i o n s e n s o r , vrep .
simx opmode streaming ) ;
270
271
272
vrep . s i m x S y n c h r o n o u s T r i g g e r ( c l i e n t I D ) ;
vrep . simxGetPingTime ( c l i e n t I D ) ;
273
274
275
276
277
278
% Se i n i c i a e l b u c l e de c o n t r o l d e l r o b o t .
w h i l e ( i ==1)
[ ˜ , p r o x d e t S t a t e , ˜ , ˜ , ˜ ] = vrep .
simxReadProximitySensor ( c l i e n t I D ,
p r o x s e n s o r , vrep . s i m x o p m o d e b u f f e r ) ;
[ ˜ , ˜ , l e f t v i s i o n a u x D a t a , ˜ ] = vrep .
simxReadVisionSensor ( clientID ,
l e f t v i s i o n s e n s o r , vrep .
simx opmode buffer ) ;
[ ˜ , ˜ , r i g h t v i s i o n a u x D a t a , ˜ ] = vrep .
simxReadVisionSensor ( clientID ,
r i g h t v i s i o n s e n s o r , vrep .
simx opmode buffer ) ;
279
280
l e f t v i s i o n r e a d =( l e f t v i s i o n a u x D a t a ( 1 1 )
<0.3) ;
Construcción de plataforma software para el diseño y la simulación...
150
A CÓDIGO DE MATLAB
281
Carlos Sosa Marrero
r i g h t v i s i o n r e a d =( r i g h t v i s i o n a u x D a t a ( 1 1 )
<0.3) ;
282
283
c u r r e n t t i m e=vrep . simxGetLastCmdTime (
clientID ) ;
284
285
286
287
288
i f ( p r o x d e t S t a t e ==1)
b a c k u n t i l t i m e=c u r r e n t t i m e +50/ h a n d l e s .
LinVel ;
end
289
290
291
292
293
294
295
296
297
298
i f ( c u r r e n t t i m e <b a c k u n t i l t i m e )
i f ( s t e e r ==1)
wl=−6∗w/ 5 ;
wr=−4∗w/ 5 ;
else
wl=−4∗w/ 5 ;
wr=−6∗w/ 5 ;
end
s t e e r u n t i l t i m e=b a c k u n t i l t i m e +50/
h a n d l e s . LinVel ;
299
300
301
302
303
304
305
306
307
308
309
310
311
e l s e i f ( c u r r e n t t i m e <s t e e r u n t i l t i m e )
i f ( s t e e r ==1)
wl=4∗w/ 5 ;
wr=6∗w/ 5 ;
else
wl=6∗w/ 5 ;
wr=4∗w/ 5 ;
end
e l s e i f ( l e f t v i s i o n r e a d ==1 &&
r i g h t v i s i o n r e a d ==0)
s t e e r =1;
wl=4∗w/ 5 ;
wr=6∗w/ 5 ;
312
313
314
315
316
e l s e i f ( l e f t v i s i o n r e a d ==0 &&
r i g h t v i s i o n r e a d ==1)
s t e e r =−1;
wl=6∗w/ 5 ;
wr=4∗w/ 5 ;
317
318
e l s e i f ( l e f t v i s i o n r e a d ==1 &&
r i g h t v i s i o n r e a d ==1)
Construcción de plataforma software para el diseño y la simulación...
151
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
i n =1;
i f ( s t e e r ==1)
wl=6∗w/ 5 ;
wr=4∗w/ 5 ;
else
wl=4∗w/ 5 ;
wr=6∗w/ 5 ;
end
319
320
321
322
323
324
325
326
327
e l s e i f ( i n ==0)
wl=w ;
wr=w ;
end
328
329
330
331
332
vrep . s i m x S e t J o i n t T a r g e t V e l o c i t y ( c l i e n t I D ,
l e f t m o t o r , wl , vrep . simx opmode oneshot ) ;
vrep . s i m x S e t J o i n t T a r g e t V e l o c i t y ( c l i e n t I D ,
r i g h t m o t o r , wr , vrep . simx opmode oneshot ) ;
vrep . s i m x S y n c h r o n o u s T r i g g e r ( c l i e n t I D ) ;
pause ( 0 . 0 1 )
333
334
335
336
337
end
338
339
340
case 2
%−−− Robot de t i p o t r i c i c l o en e s c e n a con
circuito
341
342
343
344
345
346
347
348
% Se o b t i e n e n l o s i d e n t i f i c a d o r e s de l o s motores
y los
% s e n s o r e s de proximidad y v i s i ó n i z q u i e r d o y
derecho .
[ ˜ , l e f t m o t o r ]= vrep . simxGetObjectHandle ( c l i e n t I D
, ’ t r i c y c l e l e f t m o t o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , r i g h t m o t o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ t r i c y c l e r i g h t m o t o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , f r o n t m o t o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ t r i c y c l e f r o n t m o t o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , p r o x s e n s o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ t r i c y c l e p r o x s e n s o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , l e f t v i s i o n s e n s o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ t r i c y c l e l e f t v i s i o n s e n s o r ’ , vrep .
simx opmode blocking ) ;
Construcción de plataforma software para el diseño y la simulación...
152
A CÓDIGO DE MATLAB
349
Carlos Sosa Marrero
[ ˜ , r i g h t v i s i o n s e n s o r ]= vrep . simxGetObjectHandle
( c l i e n t I D , ’ t r i c y c l e r i g h t v i s i o n s e n s o r ’ , vrep
. simx opmode blocking ) ;
350
351
352
353
354
355
356
% Se r e a l i z a una p r i m e r a l e c t u r a de l o s s e n s o r e s
% cuyo r e s u l t a d o , por motivos d e l f u n c i o n a m i e n t o
de l a
% API , e s d e s e c h a d o .
[ ˜ , p r o x d e t S t a t e , ˜ , ˜ , ˜ ] = vrep .
simxReadProximitySensor ( c l i e n t I D , p r o x s e n s o r ,
vrep . simx opmode streaming ) ;
[ ˜ , ˜ , l e f t v i s i o n a u x D a t a , ˜ ] = vrep .
simxReadVisionSensor ( clientID ,
l e f t v i s i o n s e n s o r , vrep . simx opmode streaming
);
[ ˜ , ˜ , r i g h t v i s i o n a u x D a t a , ˜ ] = vrep .
simxReadVisionSensor ( clientID ,
r i g h t v i s i o n s e n s o r , vrep .
simx opmode streaming ) ;
357
358
359
vrep . s i m x S y n c h r o n o u s T r i g g e r ( c l i e n t I D ) ;
vrep . simxGetPingTime ( c l i e n t I D ) ;
360
361
362
363
364
365
% Se i n i c i a e l b u c l e de c o n t r o l d e l r o b o t .
w h i l e ( i ==1)
[ ˜ , p r o x d e t S t a t e , ˜ , ˜ , ˜ ] = vrep .
simxReadProximitySensor ( c l i e n t I D ,
p r o x s e n s o r , vrep . s i m x o p m o d e b u f f e r ) ;
[ ˜ , ˜ , l e f t v i s i o n a u x D a t a , ˜ ] = vrep .
simxReadVisionSensor ( clientID ,
l e f t v i s i o n s e n s o r , vrep .
simx opmode buffer ) ;
[ ˜ , ˜ , r i g h t v i s i o n a u x D a t a , ˜ ] = vrep .
simxReadVisionSensor ( clientID ,
r i g h t v i s i o n s e n s o r , vrep .
simx opmode buffer ) ;
366
367
368
l e f t v i s i o n r e a d =( l e f t v i s i o n a u x D a t a ( 1 1 )
<0.3) ;
r i g h t v i s i o n r e a d =( r i g h t v i s i o n a u x D a t a ( 1 1 )
<0.3) ;
369
370
c u r r e n t t i m e=vrep . simxGetLastCmdTime (
clientID ) ;
371
Construcción de plataforma software para el diseño y la simulación...
153
A CÓDIGO DE MATLAB
372
373
374
Carlos Sosa Marrero
i f ( p r o x d e t S t a t e ==1)
b a c k u n t i l t i m e=c u r r e n t t i m e +200/ h a n d l e s
. LinVel ;
end
375
376
377
378
379
380
i f ( c u r r e n t t i m e <b a c k u n t i l t i m e )
wl=−w ;
wr=−w ;
t h e t a=−s t e e r ∗ p i / 6 ;
s t e e r u n t i l t i m e=b a c k u n t i l t i m e +200/
h a n d l e s . LinVel ;
381
382
383
384
385
e l s e i f ( c u r r e n t t i m e <s t e e r u n t i l t i m e )
wl=w ;
wr=w ;
t h e t a=s t e e r ∗ p i / 6 ;
386
387
388
389
390
391
e l s e i f ( l e f t v i s i o n r e a d ==1 &&
r i g h t v i s i o n r e a d ==0)
wl=w ;
wr=w ;
s t e e r =1;
t h e t a=s t e e r ∗ p i / 6 ;
392
393
394
395
396
397
e l s e i f ( l e f t v i s i o n r e a d ==0 &&
r i g h t v i s i o n r e a d ==1)
wl=w ;
wr=w ;
s t e e r =−1;
t h e t a=s t e e r ∗ p i / 6 ;
398
399
400
401
402
403
e l s e i f ( l e f t v i s i o n r e a d ==1 &&
r i g h t v i s i o n r e a d ==1)
i n =1;
wl=w ;
wr=w ;
t h e t a=−s t e e r ∗ p i / 6 ;
404
405
406
407
408
409
e l s e i f ( i n ==0)
wl=w ;
wr=w ;
t h e t a =0;
end
410
411
vrep . s i m x S e t J o i n t T a r g e t V e l o c i t y ( c l i e n t I D ,
Construcción de plataforma software para el diseño y la simulación...
154
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
l e f t m o t o r , wl , vrep . simx opmode oneshot ) ;
vrep . s i m x S e t J o i n t T a r g e t V e l o c i t y ( c l i e n t I D ,
r i g h t m o t o r , wr , vrep . simx opmode oneshot ) ;
vrep . s i m x S e t J o i n t T a r g e t P o s i t i o n ( c l i e n t I D ,
f r o n t m o t o r , t h e t a , vrep .
simx opmode oneshot ) ;
412
413
414
vrep . s i m x S y n c h r o n o u s T r i g g e r ( c l i e n t I D ) ;
pause ( 0 . 0 1 )
415
416
417
end
418
419
420
case 3
%−−− Robot de t i p o c u a t r i c i c l o en e s c e n a con
circuito
421
422
423
424
425
426
427
428
429
430
% Se o b t i e n e n l o s i d e n t i f i c a d o r e s de l o s motores
y los
% s e n s o r e s de proximidad y v i s i ó n i z q u i e r d o y
derecho .
[ ˜ , r e a r l e f t m o t o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ q u a d r i c y c l e r e a r l e f t m o t o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , r e a r r i g h t m o t o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ q u a d r i c y c l e r e a r r i g h t m o t o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , f r o n t l e f t m o t o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ q u a d r i c y c l e f r o n t l e f t m o t o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , f r o n t r i g h t m o t o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ q u a d r i c y c l e f r o n t r i g h t m o t o r ’ , vrep
. simx opmode blocking ) ;
[ ˜ , p r o x s e n s o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ q u a d r i c y c l e p r o x s e n s o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , l e f t v i s i o n s e n s o r ]= vrep . simxGetObjectHandle (
clientID , ’ q u a d r i c y c l e l e f t v i s i o n s e n s o r ’ ,
vrep . s i m x o p m o d e b l o c k i n g ) ;
[ ˜ , r i g h t v i s i o n s e n s o r ]= vrep . simxGetObjectHandle
( clientID , ’ quadricycle right vision sensor ’ ,
vrep . s i m x o p m o d e b l o c k i n g ) ;
431
432
433
434
% Se r e a l i z a una p r i m e r a l e c t u r a de l o s s e n s o r e s
% cuyo r e s u l t a d o , por motivos d e l f u n c i o n a m i e n t o
de l a
% API , e s d e s e c h a d o .
Construcción de plataforma software para el diseño y la simulación...
155
A CÓDIGO DE MATLAB
435
436
437
Carlos Sosa Marrero
[ ˜ , p r o x d e t S t a t e , ˜ , ˜ , ˜ ] = vrep .
simxReadProximitySensor ( c l i e n t I D , p r o x s e n s o r ,
vrep . simx opmode streaming ) ;
[ ˜ , ˜ , l e f t v i s i o n a u x D a t a , ˜ ] = vrep .
simxReadVisionSensor ( clientID ,
l e f t v i s i o n s e n s o r , vrep . simx opmode streaming
);
[ ˜ , ˜ , r i g h t v i s i o n a u x D a t a , ˜ ] = vrep .
simxReadVisionSensor ( clientID ,
r i g h t v i s i o n s e n s o r , vrep .
simx opmode streaming ) ;
438
439
440
vrep . s i m x S y n c h r o n o u s T r i g g e r ( c l i e n t I D ) ;
vrep . simxGetPingTime ( c l i e n t I D ) ;
441
442
443
444
445
446
% Se i n i c i a e l b u c l e de c o n t r o l d e l r o b o t .
w h i l e ( i ==1)
[ ˜ , p r o x d e t S t a t e , ˜ , ˜ , ˜ ] = vrep .
simxReadProximitySensor ( c l i e n t I D ,
p r o x s e n s o r , vrep . s i m x o p m o d e b u f f e r ) ;
[ ˜ , ˜ , l e f t v i s i o n a u x D a t a , ˜ ] = vrep .
simxReadVisionSensor ( clientID ,
l e f t v i s i o n s e n s o r , vrep .
simx opmode buffer ) ;
[ ˜ , ˜ , r i g h t v i s i o n a u x D a t a , ˜ ] = vrep .
simxReadVisionSensor ( clientID ,
r i g h t v i s i o n s e n s o r , vrep .
simx opmode buffer ) ;
447
448
449
l e f t v i s i o n r e a d =( l e f t v i s i o n a u x D a t a ( 1 1 )
<0.3) ;
r i g h t v i s i o n r e a d =( r i g h t v i s i o n a u x D a t a ( 1 1 )
<0.3) ;
450
451
c u r r e n t t i m e=vrep . simxGetLastCmdTime (
clientID ) ;
452
453
454
455
i f ( p r o x d e t S t a t e ==1)
b a c k u n t i l t i m e=c u r r e n t t i m e +200/ h a n d l e s
. LinVel ;
end
456
457
458
459
i f ( c u r r e n t t i m e <b a c k u n t i l t i m e )
wl=−w ;
wr=−w ;
Construcción de plataforma software para el diseño y la simulación...
156
A CÓDIGO DE MATLAB
460
461
Carlos Sosa Marrero
t h e t a=−s t e e r ∗ p i / 6 ;
s t e e r u n t i l t i m e=b a c k u n t i l t i m e +200/
h a n d l e s . LinVel ;
462
463
464
465
466
e l s e i f ( c u r r e n t t i m e <s t e e r u n t i l t i m e )
wl=w ;
wr=w ;
t h e t a=s t e e r ∗ p i / 6 ;
467
468
469
470
471
472
e l s e i f ( l e f t v i s i o n r e a d ==1 &&
r i g h t v i s i o n r e a d ==0)
wl=w ;
wr=w ;
s t e e r =1;
t h e t a=s t e e r ∗ p i / 6 ;
473
474
475
476
477
478
e l s e i f ( l e f t v i s i o n r e a d ==0 &&
r i g h t v i s i o n r e a d ==1)
wl=w ;
wr=w ;
s t e e r =−1;
t h e t a=s t e e r ∗ p i / 6 ;
479
480
481
482
483
484
e l s e i f ( l e f t v i s i o n r e a d ==1 &&
r i g h t v i s i o n r e a d ==1)
i n =1;
wl=w ;
wr=w ;
t h e t a=−s t e e r ∗ p i / 6 ;
485
486
487
488
489
490
e l s e i f ( i n ==0)
wl=w ;
wr=w ;
t h e t a =0;
end
491
492
493
t h e t a l e f t=a c o t ( c o t ( t h e t a ) −( a x i s d i s t l r / 2 ) /
axis dist fr ) ;
t h e t a r i g h t=a c o t ( c o t ( t h e t a ) +( a x i s d i s t l r / 2 )
/axis dist fr ) ;
494
495
496
vrep . s i m x S e t J o i n t T a r g e t V e l o c i t y ( c l i e n t I D ,
r e a r l e f t m o t o r , wl , vrep .
simx opmode oneshot ) ;
vrep . s i m x S e t J o i n t T a r g e t V e l o c i t y ( c l i e n t I D ,
Construcción de plataforma software para el diseño y la simulación...
157
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
r e a r r i g h t m o t o r , wr , vrep .
simx opmode oneshot ) ;
vrep . s i m x S e t J o i n t T a r g e t P o s i t i o n ( c l i e n t I D ,
f r o n t l e f t m o t o r , t h e t a l e f t , vrep .
simx opmode oneshot ) ;
vrep . s i m x S e t J o i n t T a r g e t P o s i t i o n ( c l i e n t I D ,
f r o n t r i g h t m o t o r , t h e t a r i g h t , vrep .
simx opmode oneshot ) ;
497
498
499
vrep . s i m x S y n c h r o n o u s T r i g g e r ( c l i e n t I D ) ;
pause ( 0 . 0 1 )
500
501
end
502
503
end
504
505
506
507
508
case 3
s w i t c h model
case 1
%−−− Robot de t i p o d i f e r e n c i a l en e s c e n a con
balizas
509
510
511
512
513
514
515
% Se o b t i e n e n l o s i d e n t i f i c a d o r e s de l o s motores
y el sensor
% de v i s i ó n c e n t r a l .
[ ˜ , l e f t m o t o r ]= vrep . simxGetObjectHandle ( c l i e n t I D
, ’ d i f f l e f t m o t o r ’ , vrep . s i m x o p m o d e b l o c k i n g )
;
[ ˜ , r i g h t m o t o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ d i f f r i g h t m o t o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , p r o x s e n s o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ d i f f p r o x s e n s o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , m i d d l e v i s i o n s e n s o r ]= vrep .
simxGetObjectHandle ( c l i e n t I D , ’
d i f f m i d d l e v i s i o n s e n s o r ’ , vrep .
simx opmode blocking ) ;
516
517
518
519
520
% Se r e a l i z a una p r i m e r a l e c t u r a d e l s e n s o r de
proximidad y
% e l de v i s i ó n c e n t r a l cuyo r e s u l t a d o , por
motivos d e l f u n c i o n a m i e n t o de l a
% API , e s d e s e c h a d o .
[ ˜ , p r o x d e t S t a t e , ˜ , ˜ , ˜ ] = vrep .
simxReadProximitySensor ( c l i e n t I D , p r o x s e n s o r ,
vrep . simx opmode streaming ) ;
Construcción de plataforma software para el diseño y la simulación...
158
A CÓDIGO DE MATLAB
521
Carlos Sosa Marrero
[ ˜ , ˜ , m i d d l e v i s i o n a u x D a t a , ˜ ] = vrep .
simxReadVisionSensor ( clientID ,
m i d d l e v i s i o n s e n s o r , vrep .
simx opmode streaming ) ;
522
523
524
vrep . s i m x S y n c h r o n o u s T r i g g e r ( c l i e n t I D ) ;
vrep . simxGetPingTime ( c l i e n t I D ) ;
525
526
527
528
529
% Se i n i c i a e l b u c l e de c o n t r o l d e l r o b o t .
w h i l e ( i ==1)
[ ˜ , p r o x d e t S t a t e , ˜ , ˜ , ˜ ] = vrep .
simxReadProximitySensor ( c l i e n t I D ,
p r o x s e n s o r , vrep . s i m x o p m o d e b u f f e r ) ;
[ ˜ , ˜ , m i d d l e v i s i o n a u x D a t a , ˜ ] = vrep .
simxReadVisionSensor ( clientID ,
m i d d l e v i s i o n s e n s o r , vrep .
simx opmode buffer ) ;
530
531
m i d d l e v i s i o n r e a d =( m i d d l e v i s i o n a u x D a t a ( 7 )
<0.2 && m i d d l e v i s i o n a u x D a t a ( 8 ) >0.8 &&
m i d d l e v i s i o n a u x D a t a ( 9 ) <0.2) ;
532
533
c u r r e n t t i m e=vrep . simxGetLastCmdTime (
clientID ) ;
534
535
536
i f ( p r o x d e t S t a t e ==1)
b a c k u n t i l t i m e=c u r r e n t t i m e +50/ h a n d l e s .
LinVel ;
537
538
539
540
e l s e i f ( m i d d l e v i s i o n r e a d ==1)
f o r w a r d =1;
end
541
542
543
544
545
i f ( c u r r e n t t i m e <b a c k u n t i l t i m e )
f o r w a r d =0;
wl=−4∗w/ 5 ;
wr=−6∗w/ 5 ;
546
547
548
549
e l s e i f ( f o r w a r d==1)
wl=w ;
wr=w ;
550
551
552
553
else
wl=w/ 3 0 ;
wr=−w/ 3 0 ;
Construcción de plataforma software para el diseño y la simulación...
159
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
end
554
555
vrep . s i m x S e t J o i n t T a r g e t V e l o c i t y ( c l i e n t I D ,
l e f t m o t o r , wl , vrep . simx opmode oneshot ) ;
vrep . s i m x S e t J o i n t T a r g e t V e l o c i t y ( c l i e n t I D ,
r i g h t m o t o r , wr , vrep . simx opmode oneshot ) ;
556
557
558
vrep . s i m x S y n c h r o n o u s T r i g g e r ( c l i e n t I D ) ;
pause ( 0 . 0 1 )
559
560
561
end
562
563
564
case 2
%−−− Robot de t i p o t r i c i c l o en e s c e n a con
balizas
565
566
567
568
569
570
571
572
% Se o b t i e n e n l o s i d e n t i f i c a d o r e s de l o s motores
y el sensor
% de v i s i ó n c e n t r a l .
[ ˜ , l e f t m o t o r ]= vrep . simxGetObjectHandle ( c l i e n t I D
, ’ t r i c y c l e l e f t m o t o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , r i g h t m o t o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ t r i c y c l e r i g h t m o t o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , f r o n t m o t o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ t r i c y c l e f r o n t m o t o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , p r o x s e n s o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ t r i c y c l e p r o x s e n s o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , m i d d l e v i s i o n s e n s o r ]= vrep .
simxGetObjectHandle ( c l i e n t I D , ’
t r i c y c l e m i d d l e v i s i o n s e n s o r ’ , vrep .
simx opmode blocking ) ;
573
574
575
576
577
578
% Se r e a l i z a una p r i m e r a l e c t u r a d e l s e n s o r de
proximidad y
% e l de v i s i ó n c e n t r a l cuyo r e s u l t a d o , por
motivos d e l f u n c i o n a m i e n t o de l a
% API , e s d e s e c h a d o .
[ ˜ , p r o x d e t S t a t e , ˜ , ˜ , ˜ ] = vrep .
simxReadProximitySensor ( c l i e n t I D , p r o x s e n s o r ,
vrep . simx opmode streaming ) ;
[ ˜ , ˜ , m i d d l e v i s i o n a u x D a t a , ˜ ] = vrep .
simxReadVisionSensor ( clientID ,
Construcción de plataforma software para el diseño y la simulación...
160
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
m i d d l e v i s i o n s e n s o r , vrep .
simx opmode streaming ) ;
579
580
581
vrep . s i m x S y n c h r o n o u s T r i g g e r ( c l i e n t I D ) ;
vrep . simxGetPingTime ( c l i e n t I D ) ;
582
583
584
585
586
% Se i n i c i a e l b u c l e de c o n t r o l d e l r o b o t .
w h i l e ( i ==1)
[ ˜ , p r o x d e t S t a t e , ˜ , ˜ , ˜ ] = vrep .
simxReadProximitySensor ( c l i e n t I D ,
p r o x s e n s o r , vrep . s i m x o p m o d e b u f f e r ) ;
[ ˜ , ˜ , m i d d l e v i s i o n a u x D a t a , ˜ ] = vrep .
simxReadVisionSensor ( clientID ,
m i d d l e v i s i o n s e n s o r , vrep .
simx opmode buffer ) ;
587
588
m i d d l e v i s i o n r e a d =( m i d d l e v i s i o n a u x D a t a ( 7 )
<0.2 && m i d d l e v i s i o n a u x D a t a ( 8 ) >0.8 &&
m i d d l e v i s i o n a u x D a t a ( 9 ) <0.2) ;
589
590
c u r r e n t t i m e=vrep . simxGetLastCmdTime (
clientID ) ;
591
592
593
i f ( p r o x d e t S t a t e ==1)
b a c k u n t i l t i m e=c u r r e n t t i m e +150/ h a n d l e s
. LinVel ;
594
595
596
597
e l s e i f ( m i d d l e v i s i o n r e a d ==1)
f o r w a r d =1;
end
598
599
600
601
602
603
i f ( c u r r e n t t i m e <b a c k u n t i l t i m e )
f o r w a r d =0;
wl=−w ;
wr=−w ;
t h e t a=−p i / 6 ;
604
605
606
607
608
e l s e i f ( f o r w a r d==1)
wl=w ;
wr=w ;
t h e t a =0;
609
610
611
612
else
wl=w ;
wr=w ;
Construcción de plataforma software para el diseño y la simulación...
161
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
t h e t a=p i / 6 ;
613
end
614
615
vrep . s i m x S e t J o i n t T a r g e t V e l o c i t y ( c l i e n t I D ,
l e f t m o t o r , wl , vrep . simx opmode oneshot ) ;
vrep . s i m x S e t J o i n t T a r g e t V e l o c i t y ( c l i e n t I D ,
r i g h t m o t o r , wr , vrep . simx opmode oneshot ) ;
vrep . s i m x S e t J o i n t T a r g e t P o s i t i o n ( c l i e n t I D ,
f r o n t m o t o r , t h e t a , vrep .
simx opmode oneshot ) ;
616
617
618
619
vrep . s i m x S y n c h r o n o u s T r i g g e r ( c l i e n t I D ) ;
pause ( 0 . 0 1 )
620
621
622
end
623
624
625
case 3
%−−− Robot de t i p o c u a t r i c i c l o en e s c e n a con
balizas
626
627
628
629
630
631
632
633
634
% Se o b t i e n e n l o s i d e n t i f i c a d o r e s de l o s motores
y el sensor
% de v i s i ó n c e n t r a l .
[ ˜ , r e a r l e f t m o t o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ q u a d r i c y c l e r e a r l e f t m o t o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , r e a r r i g h t m o t o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ q u a d r i c y c l e r e a r r i g h t m o t o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , f r o n t l e f t m o t o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ q u a d r i c y c l e f r o n t l e f t m o t o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , f r o n t r i g h t m o t o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ q u a d r i c y c l e f r o n t r i g h t m o t o r ’ , vrep
. simx opmode blocking ) ;
[ ˜ , p r o x s e n s o r ]= vrep . simxGetObjectHandle (
c l i e n t I D , ’ q u a d r i c y c l e p r o x s e n s o r ’ , vrep .
simx opmode blocking ) ;
[ ˜ , m i d d l e v i s i o n s e n s o r ]= vrep .
simxGetObjectHandle ( c l i e n t I D , ’
q u a d r i c y c l e m i d d l e v i s i o n s e n s o r ’ , vrep .
simx opmode blocking ) ;
635
636
637
% Se r e a l i z a una p r i m e r a l e c t u r a d e l s e n s o r de
proximidad y
% e l de v i s i ó n c e n t r a l cuyo r e s u l t a d o , por
Construcción de plataforma software para el diseño y la simulación...
162
A CÓDIGO DE MATLAB
638
639
640
Carlos Sosa Marrero
motivos d e l f u n c i o n a m i e n t o de l a
% API , e s d e s e c h a d o .
[ ˜ , p r o x d e t S t a t e , ˜ , ˜ , ˜ ] = vrep .
simxReadProximitySensor ( c l i e n t I D , p r o x s e n s o r ,
vrep . simx opmode streaming ) ;
[ ˜ , ˜ , m i d d l e v i s i o n a u x D a t a , ˜ ] = vrep .
simxReadVisionSensor ( clientID ,
m i d d l e v i s i o n s e n s o r , vrep .
simx opmode streaming ) ;
641
642
643
vrep . s i m x S y n c h r o n o u s T r i g g e r ( c l i e n t I D ) ;
vrep . simxGetPingTime ( c l i e n t I D ) ;
644
645
646
647
648
% Se i n i c i a e l b u c l e de c o n t r o l d e l r o b o t .
w h i l e ( i ==1)
[ ˜ , p r o x d e t S t a t e , ˜ , ˜ , ˜ ] = vrep .
simxReadProximitySensor ( c l i e n t I D ,
p r o x s e n s o r , vrep . s i m x o p m o d e b u f f e r ) ;
[ ˜ , ˜ , m i d d l e v i s i o n a u x D a t a , ˜ ] = vrep .
simxReadVisionSensor ( clientID ,
m i d d l e v i s i o n s e n s o r , vrep .
simx opmode buffer ) ;
649
650
m i d d l e v i s i o n r e a d =( m i d d l e v i s i o n a u x D a t a ( 7 )
<0.2 && m i d d l e v i s i o n a u x D a t a ( 8 ) >0.8 &&
m i d d l e v i s i o n a u x D a t a ( 9 ) <0.2) ;
651
652
c u r r e n t t i m e=vrep . simxGetLastCmdTime (
clientID ) ;
653
654
655
i f ( p r o x d e t S t a t e ==1)
b a c k u n t i l t i m e=c u r r e n t t i m e +150/ h a n d l e s
. LinVel ;
656
657
658
659
e l s e i f ( m i d d l e v i s i o n r e a d ==1)
f o r w a r d =1;
end
660
661
662
663
664
665
i f ( c u r r e n t t i m e <b a c k u n t i l t i m e )
f o r w a r d =0;
wl=−w ;
wr=−w ;
t h e t a=−p i / 6 ;
666
667
e l s e i f ( f o r w a r d==1)
Construcción de plataforma software para el diseño y la simulación...
163
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
wl=w ;
wr=w ;
t h e t a =0;
668
669
670
671
else
672
wl=w ;
wr=w ;
t h e t a=p i / 6 ;
673
674
675
end
676
677
t h e t a l e f t=a c o t ( c o t ( t h e t a ) −( a x i s d i s t l r / 2 ) /
axis dist fr ) ;
t h e t a r i g h t=a c o t ( c o t ( t h e t a ) +( a x i s d i s t l r / 2 )
/axis dist fr ) ;
678
679
680
vrep . s i m x S e t J o i n t T a r g e t V e l o c i t y ( c l i e n t I D
r e a r l e f t m o t o r , wl , vrep .
simx opmode oneshot ) ;
vrep . s i m x S e t J o i n t T a r g e t V e l o c i t y ( c l i e n t I D
r e a r r i g h t m o t o r , wr , vrep .
simx opmode oneshot ) ;
vrep . s i m x S e t J o i n t T a r g e t P o s i t i o n ( c l i e n t I D
f r o n t l e f t m o t o r , t h e t a l e f t , vrep .
simx opmode oneshot ) ;
vrep . s i m x S e t J o i n t T a r g e t P o s i t i o n ( c l i e n t I D
f r o n t r i g h t m o t o r , t h e t a r i g h t , vrep .
simx opmode oneshot ) ;
681
682
683
684
,
,
,
,
685
vrep . s i m x S y n c h r o n o u s T r i g g e r ( c l i e n t I D ) ;
pause ( 0 . 0 1 )
686
687
end
688
end
689
690
end
691
692
693
694
695
696
697
%−−− Función que s e e j e c u t a a l p u l s a r e l botón ’ PauseSim ’
e n c a r g a d a de
% p a u s a r l a s i m u l a c i ó n .
f u n c t i o n P au se S im C al lb a ck ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l vrep
global clientID
global i
698
699
700
i =0;
vrep . s i m x P a u s e S i m u l a t i o n ( c l i e n t I D , vrep . simx opmode oneshot ) ;
701
Construcción de plataforma software para el diseño y la simulación...
164
A CÓDIGO DE MATLAB
702
703
704
705
706
707
708
Carlos Sosa Marrero
%−−− Función que s e e j e c u t a a l p u l s a r e l botón ’ StopSim ’
e n c a r g a d a de
% p a r a r l a s i m u l a c i ó n y f i n a l i z a r l a c o n e x i ó n con V−REP s i no s e
d e s e a r e a l i z a r un nuevo t e s t e o .
f u n c t i o n S t o p S i m C a l l b a c k ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l vrep
global clientID
global i
g l o b a l next
709
710
711
i =0;
vrep . s i m x S t o p S i m u l a t i o n ( c l i e n t I D , vrep . simx opmode oneshot ) ;
712
713
714
715
716
717
718
719
720
721
722
723
opc=q u e s t d l g ( ’ ¿Desea r e a l i z a r una nueva s i m u l a c i ó n ? ’ , ’ S a l i r ’ , ’ Sı́
’ , ’No ’ , ’No ’ ) ;
i f strcmp ( opc , ’No ’ )
vrep . simxGetPingTime ( c l i e n t I D ) ;
vrep . s i m x F i n i s h ( c l i e n t I D ) ;
next =1;
c l o s e Simulation
else
next =1;
c l o s e Simulation
SelectScene
end
724
725
726
727
728
729
730
%−−− %−−− Función que l e e e l v a l o r de LinVel y o b t i e n e a
p a r t i r de é l un
% v a l o r de v e l o c i d a d a n g u l a r de r e f e r e n c i a para l a s r u e d a s .
f u n c t i o n L i n V e l C a l l b a c k ( hObject , ev en td at a , h a n d l e s )
global scene
global wheels rad
global w
731
732
733
734
735
736
737
738
739
740
l i n v e l=g e t ( hObject , ’ v a l u e ’ ) ;
h a n d l e s . LinVel=l i n v e l ;
i f ( s c e n e==2 && l i n v e l >0.5)
warndlg ( ’ V e l o c i d a d e s l i n e a l e s muy a l t a s pueden i m p e d i r que
e l r o b o t s i g a c o r r e c t a m e n t e e l c i r c u i t o . ’ , ’ADVERTENCIA ’ )
end
g u i d a t a ( hObject , h a n d l e s ) ;
d i s p l i n v e l=round ( 1 0 0 ∗ l i n v e l ) / 1 0 0 ;
s e t ( h a n d l e s . DispLinVel , ’ s t r i n g ’ , [ ’ v= ’ , num2str ( d i s p l i n v e l ) , ’ m/
s ’ ]) ;
g u i d a t a ( hObject , h a n d l e s ) ;
Construcción de plataforma software para el diseño y la simulación...
165
A CÓDIGO DE MATLAB
Carlos Sosa Marrero
741
742
w=h a n d l e s . LinVel / w h e e l s r a d ;
743
744
745
746
%−−− Función que s e e j e c u t a cuando s e i n t e n t a c e r r a r l a ventana
.
f u n c t i o n f i g u r e 1 C l o s e R e q u e s t F c n ( hObject , e ve nt da ta , h a n d l e s )
g l o b a l next
747
748
749
750
751
752
753
754
755
756
757
% S i l a p e t i c i ó n de c i e r r e de l a ventana no e s c o n s e c u e n c i a d e l
normal t r á n s i t o d e l programa de una ventana a o t r a
% s i n o que d e r i v a d e l c l i c por p a r t e d e l u s u a r i o en e l i c o n o de
c i e r r e , s e e x i g e una c o n f i r m a c i ó n .
i f next==0
opc=q u e s t d l g ( ’ ¿Desea s a l i r d e l programa ? ’ , ’ S a l i r ’ , ’ Sı́ ’ , ’No ’ ,
’No ’ ) ;
i f strcmp ( opc , ’No ’ )
return
end
end
next =0;
d e l e t e ( hObject ) ;
Construcción de plataforma software para el diseño y la simulación...
166
B CÓDIGO LUA
B.
Código Lua
B.1.
1
2
3
4
5
Carlos Sosa Marrero
create
−−Se d e f i n e n l o s m a t e r i a l e s de l a s r u e d a s y l a s e s f e r a s
deslizantes
i f ( s i m c a l l t y p e==s i m c h i l d s c r i p t c a l l i n i t i a l i z a t i o n ) then
w h e e l M a t e r i a l=s i m G e t M a t e r i a l I d ( ’ w h e e l M a t e r i a l ’ )
n o F r i c t i o n M a t e r i a l=s i m G e t M a t e r i a l I d ( ’ n o F r i c t i o n M a t e r i a l ’ )
end
6
7
8
−−Función que c r e a un r o b o t d i f e r e n c i a l en l a e s c e n a a p a r t i r de
l o s p a r á m e t r o s r e c i b i d o s d e s d e Matlab. Para una i n f o r m a c i ó n
−−d e t a l l a d a a c e r c a de l o s argumentos de cada f u n c i ó n API ,
c o n s u l t a r e l l i s t a d o ’ R egu lar API f u n c t i o n s ’ .
9
10
11
12
13
14
15
16
17
C r e a t e D i f f f u n c t i o n=f u n c t i o n ( i n I n t s , i n F l o a t s , i n S t r i n g s , i n B u f f e r )
d i f f =simCreatePureShape ( i n I n t s [ 1 ] , 0 1 1 1 0 , { i n F l o a t s [ 1 ] ,
i n F l o a t s [ 2 ] , i n F l o a t s [ 3 ] } , i n F l o a t s [ 4 ] , NULL)
s i m S e t O b j e c t P o s i t i o n ( d i f f , −1 ,{ i n F l o a t s [ 5 ] , i n F l o a t s [ 6 ] ,
inFloats [7]})
simSetObjectName ( d i f f , ’ d i f f ’ )
simSetObjectSpecialProperty ( dif f ,
s i m o b j e c t s p e c i a l p r o p e r t y m e a s u r a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y c o l l i d a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y r e n d e r a b l e+
sim objectspecialproperty detectable all )
simSetObjectInt32Parameter ( d i f f , sim shapeintparam static , 0 )
simSetObjectInt32Parameter ( d i f f ,
sim shapeintparam respondable , 1 )
s i m S e t S h a p e C o l o r ( d i f f , NULL,
sim colorcomponent ambient diffuse ,{1 .0 , 0 .71 , 0 .37 })
18
19
20
21
22
23
24
l e f t w h e e l=simCreatePureShape ( 2 , 0 1 1 1 0 , { i n F l o a t s [ 8 ] , i n F l o a t s
[ 9 ] , i n F l o a t s [ 1 0 ] } , i n F l o a t s [ 1 1 ] ,NULL)
s i m S e t O b j e c t P o s i t i o n ( l e f t w h e e l , −1 ,{ i n F l o a t s [ 1 2 ] , i n F l o a t s
[13] , inFloats [14]})
s i m S e t O b j e c t O r i e n t a t i o n ( l e f t w h e e l , −1 ,{0 , − m a t h . p i / 2 , 0 } )
simSetObjectName ( l e f t w h e e l , ’ d i f f l e f t w h e e l ’ )
simSetObjectSpecialProperty ( left wheel ,
s i m o b j e c t s p e c i a l p r o p e r t y m e a s u r a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y c o l l i d a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y r e n d e r a b l e+
sim objectspecialproperty detectable all )
simSetObjectInt32Parameter ( l e f t w h e e l ,
Construcción de plataforma software para el diseño y la simulación...
167
B CÓDIGO LUA
25
26
27
Carlos Sosa Marrero
sim shapeintparam static ,0)
simSetObjectInt32Parameter ( l e f t w h e e l ,
sim shapeintparam respondable , 1 )
s i m S e t S h a p e C o l o r ( l e f t w h e e l , NULL,
sim colorcomponent ambient diffuse ,{0 .15 , 0 .15 , 0 .15 })
simSetShapeMaterial ( l e f t w h e e l , wheelMaterial )
28
29
30
31
32
33
34
35
36
37
r i g h t w h e e l=simCreatePureShape ( 2 , 0 1 1 1 0 , { i n F l o a t s [ 8 ] , i n F l o a t s
[ 9 ] , i n F l o a t s [ 1 0 ] } , i n F l o a t s [ 1 1 ] ,NULL)
s i m S e t O b j e c t P o s i t i o n ( r i g h t w h e e l , −1 ,{ i n F l o a t s [ 1 5 ] , i n F l o a t s
[16] , inFloats [17]})
s i m S e t O b j e c t O r i e n t a t i o n ( r i g h t w h e e l , −1 ,{0 , − m a t h . p i / 2 , 0 } )
simSetObjectName ( r i g h t w h e e l , ’ d i f f r i g h t w h e e l ’ )
simSetObjectSpecialProperty ( right wheel ,
s i m o b j e c t s p e c i a l p r o p e r t y m e a s u r a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y c o l l i d a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y r e n d e r a b l e+
sim objectspecialproperty detectable all )
simSetObjectInt32Parameter ( right wheel ,
sim shapeintparam static ,0)
simSetObjectInt32Parameter ( right wheel ,
sim shapeintparam respondable , 1 )
s i m S e t S h a p e C o l o r ( r i g h t w h e e l , NULL,
sim colorcomponent ambient diffuse ,{0 .15 , 0 .15 , 0 .15 })
simSetShapeMaterial ( right wheel , wheelMaterial )
38
39
40
41
42
43
44
45
l e f t m o t o r=s i m C r e a t e J o i n t ( s i m j o i n t r e v o l u t e s u b t y p e ,
s i m j o i n t m o d e f o r c e , 0 ,NULL, NULL,NULL)
s i m S e t O b j e c t P o s i t i o n ( l e f t m o t o r , −1 ,{ i n F l o a t s [ 1 2 ] , i n F l o a t s
[13] , inFloats [14]})
s i m S e t O b j e c t O r i e n t a t i o n ( l e f t m o t o r , −1 ,{0 , − m a t h . p i / 2 , 0 } )
simSetObjectName ( l e f t m o t o r , ’ d i f f l e f t m o t o r ’ )
simSetObjectInt32Parameter ( left motor ,
sim jointintparam motor enabled ,1 )
simSetObjectInt32Parameter ( left motor ,
sim jointintparam velocity lock ,1)
simSetObjectInt32Parameter ( left motor ,
sim objintparam visibility layer ,1024)
46
47
48
49
50
r i g h t m o t o r=s i m C r e a t e J o i n t ( s i m j o i n t r e v o l u t e s u b t y p e ,
s i m j o i n t m o d e f o r c e , 0 ,NULL, NULL,NULL)
s i m S e t O b j e c t P o s i t i o n ( r i g h t m o t o r , −1 ,{ i n F l o a t s [ 1 5 ] , i n F l o a t s
[16] , inFloats [17]})
s i m S e t O b j e c t O r i e n t a t i o n ( r i g h t m o t o r , −1 ,{0 , − m a t h . p i / 2 , 0 } )
simSetObjectName ( r i g h t m o t o r , ’ d i f f r i g h t m o t o r ’ )
Construcción de plataforma software para el diseño y la simulación...
168
B CÓDIGO LUA
51
52
53
Carlos Sosa Marrero
simSetObjectInt32Parameter ( right motor ,
sim jointintparam motor enabled ,1 )
simSetObjectInt32Parameter ( right motor ,
sim jointintparam velocity lock ,1)
simSetObjectInt32Parameter ( right motor ,
sim objintparam visibility layer ,1024)
54
55
56
57
58
p r o x s e n s o r=s i m C r e a t e P r o x i m i t y S e n s o r (
sim proximitysensor disc subtype ,
sim objectspecialproperty detectable ultrasonic
,0000000000 ,{16 ,32 ,0 ,0 ,0 ,0 ,0 ,0} ,{0 .0 , i n F l o a t s [ 1 8 ] , 0 .0 , 0
.05 , 0 .0 , 0 .0 , 0 .0 , 0 .1 , 0 .0 , math.pi /2 ,0 .0 , 0 .0 , 0 .01 , 0 .0 , 0 . 0 } ,
NULL)
s i m S e t O b j e c t P o s i t i o n ( p r o x s e n s o r , −1 ,{ i n F l o a t s [ 1 9 ] , i n F l o a t s
[20] , inFloats [21]})
s i m S e t O b j e c t O r i e n t a t i o n ( p r o x s e n s o r ,−1,{− m a t h . p i / 2 , 0 , 0 } )
simSetObjectName ( p r o x s e n s o r , ’ d i f f p r o x s e n s o r ’ )
59
60
61
62
63
f r o n t f o r c e s e n s o r=s i m C r e a t e F o r c e S e n s o r ( 0 0 , { 0 , 1 , 1 0 , 0 , 0 } , { 0
. 0 1 , 1 0 0 . 0 , 1 0 . 0 , 0 . 0 , 0 . 0 } ,NULL)
s i m S e t O b j e c t P o s i t i o n ( f r o n t f o r c e s e n s o r , −1 ,{ i n F l o a t s [ 2 2 ] ,
inFloats [23] , inFloats [24]})
simSetObjectName ( f r o n t f o r c e s e n s o r , ’ d i f f f r o n t f o r c e s e n s o r
’)
simSetObjectInt32Parameter ( f r o n t f o r c e s e n s o r ,
sim objintparam visibility layer ,1024)
64
65
66
67
68
r e a r f o r c e s e n s o r=s i m C r e a t e F o r c e S e n s o r ( 0 0 , { 0 , 1 , 1 0 , 0 , 0 } , { 0 . 0 1
, 1 0 0 . 0 , 1 0 . 0 , 0 . 0 , 0 . 0 } ,NULL)
s i m S e t O b j e c t P o s i t i o n ( r e a r f o r c e s e n s o r , −1 ,{ i n F l o a t s [ 2 5 ] ,
inFloats [26] , inFloats [27]})
simSetObjectName ( r e a r f o r c e s e n s o r , ’ d i f f r e a r f o r c e s e n s o r ’ )
simSetObjectInt32Parameter ( r e a r f o r c e s e n s o r ,
sim objintparam visibility layer ,1024)
69
70
71
72
73
74
f r o n t s l i d e r=simCreatePureShape ( 1 , 0 1 1 1 0 , { i n F l o a t s [ 2 8 ] ,
i n F l o a t s [ 2 9 ] , i n F l o a t s [ 3 0 ] } , i n F l o a t s [ 3 1 ] ,NULL)
s i m S e t O b j e c t P o s i t i o n ( f r o n t s l i d e r , −1 ,{ i n F l o a t s [ 3 2 ] , i n F l o a t s
[33] , inFloats [34]})
s i m S e t O b j e c t O r i e n t a t i o n ( f r o n t s l i d e r , −1 ,{0 , − m a t h . p i / 2 , 0 } )
simSetObjectName ( f r o n t s l i d e r , ’ d i f f f r o n t s l i d e r ’ )
simSetObjectSpecialProperty ( f r o n t s l i d e r ,
s i m o b j e c t s p e c i a l p r o p e r t y m e a s u r a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y c o l l i d a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y r e n d e r a b l e+
Construcción de plataforma software para el diseño y la simulación...
169
B CÓDIGO LUA
75
76
77
78
Carlos Sosa Marrero
sim objectspecialproperty detectable all )
simSetObjectInt32Parameter ( f r o n t s l i d e r ,
sim shapeintparam static ,0)
simSetObjectInt32Parameter ( f r o n t s l i d e r ,
sim shapeintparam respondable , 1 )
s i m S e t S h a p e C o l o r ( f r o n t s l i d e r , NULL,
sim colorcomponent ambient diffuse ,{0 .15 , 0 .15 , 0 .15 })
simSetShapeMaterial ( f r o n t s l i d e r , noFrictionMaterial )
79
80
81
82
83
84
85
86
87
88
r e a r s l i d e r=simCreatePureShape ( 1 , 0 1 1 1 0 , { i n F l o a t s [ 2 8 ] ,
i n F l o a t s [ 2 9 ] , i n F l o a t s [ 3 0 ] } , i n F l o a t s [ 3 1 ] ,NULL)
s i m S e t O b j e c t P o s i t i o n ( r e a r s l i d e r , −1 ,{ i n F l o a t s [ 3 5 ] , i n F l o a t s
[36] , inFloats [37]})
s i m S e t O b j e c t O r i e n t a t i o n ( r e a r s l i d e r , −1 ,{0 , − m a t h . p i / 2 , 0 } )
simSetObjectName ( r e a r s l i d e r , ’ d i f f r e a r s l i d e r ’ )
simSetObjectSpecialProperty ( r e a r s l i d e r ,
s i m o b j e c t s p e c i a l p r o p e r t y m e a s u r a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y c o l l i d a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y r e n d e r a b l e+
sim objectspecialproperty detectable all )
simSetObjectInt32Parameter ( r e a r s l i d e r ,
sim shapeintparam static ,0)
simSetObjectInt32Parameter ( r e a r s l i d e r ,
sim shapeintparam respondable , 1 )
s i m S e t S h a p e C o l o r ( r e a r s l i d e r , NULL,
sim colorcomponent ambient diffuse ,{0 .15 , 0 .15 , 0 .15 })
simSetShapeMaterial ( f r o n t s l i d e r , noFrictionMaterial )
89
90
91
92
93
l e f t v i s i o n s e n s o r=s i m C r e a t e V i s i o n S e n s o r
(00000000 ,{1 ,1 ,0 ,0} ,{0 .01 , i n F l o a t s [ 3 8 ] , 0 .01 , 0 .01 , 0 .01 , 0
. 0 1 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 } ,NULL)
s i m S e t O b j e c t P o s i t i o n ( l e f t v i s i o n s e n s o r , −1 ,{ i n F l o a t s [ 3 9 ] ,
inFloats [40] , inFloats [41]})
simSetObjectName ( l e f t v i s i o n s e n s o r , ’ d i f f l e f t v i s i o n s e n s o r
’)
s i m S e t O b j e c t O r i e n t a t i o n ( l e f t v i s i o n s e n s o r , −1 ,{ math.pi , 0 , 0 } )
94
95
96
97
98
r i g h t v i s i o n s e n s o r=s i m C r e a t e V i s i o n S e n s o r
(00000000 ,{1 ,1 ,0 ,0} ,{0 .01 , i n F l o a t s [ 3 8 ] , 0 .01 , 0 .01 , 0 .01 , 0
. 0 1 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 } ,NULL)
s i m S e t O b j e c t P o s i t i o n ( r i g h t v i s i o n s e n s o r , −1 ,{ i n F l o a t s [ 4 2 ] ,
inFloats [43] , inFloats [44]})
simSetObjectName ( r i g h t v i s i o n s e n s o r , ’
diff right vision sensor ’)
s i m S e t O b j e c t O r i e n t a t i o n ( r i g h t v i s i o n s e n s o r , −1 ,{ math.pi
Construcción de plataforma software para el diseño y la simulación...
170
B CÓDIGO LUA
Carlos Sosa Marrero
,0 ,0})
99
m i d d l e v i s i o n s e n s o r=s i m C r e a t e V i s i o n S e n s o r
(00000000 ,{1 ,1 ,0 ,0} ,{0 .01 , math.sqrt (2∗5 .00 ˆ2) ,0 .01 , 0 .01 , 0
. 0 1 , 0 . 0 1 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 } ,NULL)
s i m S e t O b j e c t P o s i t i o n ( m i d d l e v i s i o n s e n s o r , −1 ,{ i n F l o a t s [ 4 5 ] ,
inFloats [46] , inFloats [47]})
simSetObjectName ( m i d d l e v i s i o n s e n s o r , ’
diff middle vision sensor ’)
s i m S e t O b j e c t O r i e n t a t i o n ( m i d d l e v i s i o n s e n s o r ,−1,{− m a t h . p i
/2 ,0 ,0})
100
101
102
103
104
simSetObjectParent ( l e f t w h e e l , left motor , true )
simSetObjectParent ( right wheel , right motor , true )
simSetObjectParent ( f r o n t s l i d e r , f r o n t f o r c e s e n s o r , true )
simSetObjectParent ( r e a r s l i d e r , r e a r f o r c e s e n s o r , true )
simSetObjectParent ( left motor , d i f f , true )
simSetObjectParent ( right motor , d i f f , true )
simSetObjectParent ( prox sensor , d i f f , true )
simSetObjectParent ( f r o n t f o r c e s e n s o r , d i f f , true )
simSetObjectParent ( r e a r f o r c e s e n s o r , d i f f , true )
simSetObjectParent ( l e f t v i s i o n s e n s o r , d i f f , true )
simSetObjectParent ( r i g h t v i s i o n s e n s o r , d i f f , true )
simSetObjectParent ( m i d d l e v i s i o n s e n s o r , d i f f , true )
105
106
107
108
109
110
111
112
113
114
115
116
117
return {} ,{} ,{} , ’ ’
118
119
end
120
121
122
123
124
125
126
127
128
129
−−Función que c r e a un r o b o t de t i p o t r i c i c l o en l a e s c e n a a
p a r t i r de l o s p a r á m e t r o s r e c i b i d o s d e s d e Matlab. Para una
i n f o r m a c i ó n
−−d e t a l l a d a a c e r c a de l o s argumentos de cada f u n c i ó n API ,
c o n s u l t a r e l l i s t a d o ’ R egu lar API f u n c t i o n s ’ .
C r e a t e T r i c y c l e f u n c t i o n=f u n c t i o n ( i n I n t s , i n F l o a t s , i n S t r i n g s ,
inBuffer )
t r i c y c l e=simCreatePureShape ( i n I n t s [ 1 ] , 0 1 1 1 0 , { i n F l o a t s [ 1 ] ,
i n F l o a t s [ 2 ] , i n F l o a t s [ 3 ] } , i n F l o a t s [ 4 ] , NULL)
s i m S e t O b j e c t P o s i t i o n ( t r i c y c l e , −1 ,{ i n F l o a t s [ 5 ] , i n F l o a t s [ 6 ] ,
inFloats [7]})
simSetObjectName ( t r i c y c l e , ’ t r i c y c l e ’ )
simSetObjectSpecialProperty ( t r i c y c l e ,
s i m o b j e c t s p e c i a l p r o p e r t y m e a s u r a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y c o l l i d a b l e+
Construcción de plataforma software para el diseño y la simulación...
171
B CÓDIGO LUA
130
131
132
Carlos Sosa Marrero
s i m o b j e c t s p e c i a l p r o p e r t y r e n d e r a b l e+
sim objectspecialproperty detectable all )
simSetObjectInt32Parameter ( t r i c y c l e , sim shapeintparam static
,0)
simSetObjectInt32Parameter ( t r i c y c l e ,
sim shapeintparam respondable , 1 )
s i m S e t S h a p e C o l o r ( t r i c y c l e , NULL,
sim colorcomponent ambient diffuse ,{0 .30 , 0 .61 , 0 .84 })
133
134
135
136
137
138
139
140
141
142
l e f t w h e e l=simCreatePureShape ( 2 , 0 1 1 1 0 , { i n F l o a t s [ 8 ] , i n F l o a t s
[ 9 ] , i n F l o a t s [ 1 0 ] } , i n F l o a t s [ 1 1 ] ,NULL)
s i m S e t O b j e c t P o s i t i o n ( l e f t w h e e l , −1 ,{ i n F l o a t s [ 1 2 ] , i n F l o a t s
[13] , inFloats [14]})
s i m S e t O b j e c t O r i e n t a t i o n ( l e f t w h e e l , −1 ,{0 , − m a t h . p i / 2 , 0 } )
simSetObjectName ( l e f t w h e e l , ’ t r i c y c l e l e f t w h e e l ’ )
simSetObjectSpecialProperty ( left wheel ,
s i m o b j e c t s p e c i a l p r o p e r t y m e a s u r a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y c o l l i d a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y r e n d e r a b l e+
sim objectspecialproperty detectable all )
simSetObjectInt32Parameter ( l e f t w h e e l ,
sim shapeintparam static ,0)
simSetObjectInt32Parameter ( l e f t w h e e l ,
sim shapeintparam respondable , 1 )
s i m S e t S h a p e C o l o r ( l e f t w h e e l , NULL,
sim colorcomponent ambient diffuse ,{0 .15 , 0 .15 , 0 .15 })
simSetShapeMaterial ( l e f t w h e e l , wheelMaterial )
143
144
145
146
147
148
149
150
151
r i g h t w h e e l=simCreatePureShape ( 2 , 0 1 1 1 0 , { i n F l o a t s [ 8 ] , i n F l o a t s
[ 9 ] , i n F l o a t s [ 1 0 ] } , i n F l o a t s [ 1 1 ] ,NULL)
s i m S e t O b j e c t P o s i t i o n ( r i g h t w h e e l , −1 ,{ i n F l o a t s [ 1 5 ] , i n F l o a t s
[16] , inFloats [17]})
s i m S e t O b j e c t O r i e n t a t i o n ( r i g h t w h e e l , −1 ,{0 , − m a t h . p i / 2 , 0 } )
simSetObjectName ( r i g h t w h e e l , ’ t r i c y c l e r i g h t w h e e l ’ )
simSetObjectSpecialProperty ( right wheel ,
s i m o b j e c t s p e c i a l p r o p e r t y m e a s u r a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y c o l l i d a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y r e n d e r a b l e+
sim objectspecialproperty detectable all )
simSetObjectInt32Parameter ( right wheel ,
sim shapeintparam static ,0)
simSetObjectInt32Parameter ( right wheel ,
sim shapeintparam respondable , 1 )
s i m S e t S h a p e C o l o r ( r i g h t w h e e l , NULL,
sim colorcomponent ambient diffuse ,{0 .15 , 0 .15 , 0 .15 })
Construcción de plataforma software para el diseño y la simulación...
172
B CÓDIGO LUA
152
Carlos Sosa Marrero
simSetShapeMaterial ( right wheel , wheelMaterial )
153
154
155
156
157
158
159
160
161
162
f r o n t w h e e l=simCreatePureShape ( 2 , 0 1 1 1 0 , { i n F l o a t s [ 8 ] , i n F l o a t s
[ 9 ] , i n F l o a t s [ 1 0 ] } , i n F l o a t s [ 1 1 ] ,NULL)
s i m S e t O b j e c t P o s i t i o n ( f r o n t w h e e l , −1 ,{ i n F l o a t s [ 1 8 ] , i n F l o a t s
[19] , inFloats [20]})
s i m S e t O b j e c t O r i e n t a t i o n ( f r o n t w h e e l , −1 ,{0 , − m a t h . p i / 2 , 0 } )
simSetObjectName ( f r o n t w h e e l , ’ t r i c y c l e f r o n t w h e e l ’ )
simSetObjectSpecialProperty ( front wheel ,
s i m o b j e c t s p e c i a l p r o p e r t y m e a s u r a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y c o l l i d a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y r e n d e r a b l e+
sim objectspecialproperty detectable all )
simSetObjectInt32Parameter ( front wheel ,
sim shapeintparam static ,0)
simSetObjectInt32Parameter ( front wheel ,
sim shapeintparam respondable , 1 )
s i m S e t S h a p e C o l o r ( f r o n t w h e e l , NULL,
sim colorcomponent ambient diffuse ,{0 .15 , 0 .15 , 0 .15 })
simSetShapeMaterial ( front wheel , wheelMaterial )
163
164
165
166
167
168
169
170
171
i n v f r o n t w h e e l=simCreatePureShape ( 2 , 0 1 1 1 0 , { i n F l o a t s [ 8 ] ,
i n F l o a t s [ 9 ] , i n F l o a t s [ 1 0 ] } , i n F l o a t s [ 1 1 ] ,NULL)
s i m S e t O b j e c t P o s i t i o n ( i n v f r o n t w h e e l , −1 ,{ i n F l o a t s [ 1 8 ] ,
inFloats [19] , inFloats [20]})
s i m S e t O b j e c t O r i e n t a t i o n ( i n v f r o n t w h e e l , −1 ,{0 , − m a t h . p i / 2 , 0 } )
simSetObjectName ( i n v f r o n t w h e e l , ’ t r i c y c l e i n v f r o n t w h e e l ’ )
simSetObjectInt32Parameter ( inv front wheel ,
sim shapeintparam static ,0)
simSetObjectInt32Parameter ( inv front wheel ,
sim shapeintparam respondable , 0 )
simSetObjectInt32Parameter ( inv front wheel ,
sim objintparam visibility layer ,1024)
simSetShapeMaterial ( inv front wheel , wheelMaterial )
172
173
174
175
176
177
178
l e f t m o t o r=s i m C r e a t e J o i n t ( s i m j o i n t r e v o l u t e s u b t y p e ,
s i m j o i n t m o d e f o r c e , 0 ,NULL, NULL,NULL)
s i m S e t O b j e c t P o s i t i o n ( l e f t m o t o r , −1 ,{ i n F l o a t s [ 1 2 ] , i n F l o a t s
[13] , inFloats [14]})
s i m S e t O b j e c t O r i e n t a t i o n ( l e f t m o t o r , −1 ,{0 , − m a t h . p i / 2 , 0 } )
simSetObjectName ( l e f t m o t o r , ’ t r i c y c l e l e f t m o t o r ’ )
simSetObjectInt32Parameter ( left motor ,
sim jointintparam motor enabled ,1 )
simSetObjectInt32Parameter ( left motor ,
sim jointintparam velocity lock ,1)
Construcción de plataforma software para el diseño y la simulación...
173
B CÓDIGO LUA
179
Carlos Sosa Marrero
simSetObjectInt32Parameter ( left motor ,
sim objintparam visibility layer ,1024)
180
181
182
183
184
185
186
187
r i g h t m o t o r=s i m C r e a t e J o i n t ( s i m j o i n t r e v o l u t e s u b t y p e ,
s i m j o i n t m o d e f o r c e , 0 ,NULL, NULL,NULL)
s i m S e t O b j e c t P o s i t i o n ( r i g h t m o t o r , −1 ,{ i n F l o a t s [ 1 5 ] , i n F l o a t s
[16] , inFloats [17]})
s i m S e t O b j e c t O r i e n t a t i o n ( r i g h t m o t o r , −1 ,{0 , − m a t h . p i / 2 , 0 } )
simSetObjectName ( r i g h t m o t o r , ’ t r i c y c l e r i g h t m o t o r ’ )
simSetObjectInt32Parameter ( right motor ,
sim jointintparam motor enabled ,1 )
simSetObjectInt32Parameter ( right motor ,
sim jointintparam velocity lock ,1)
simSetObjectInt32Parameter ( right motor ,
sim objintparam visibility layer ,1024)
188
189
190
191
192
193
194
195
f r o n t m o t o r=s i m C r e a t e J o i n t ( s i m j o i n t r e v o l u t e s u b t y p e ,
s i m j o i n t m o d e f o r c e , 0 ,NULL, NULL,NULL)
s i m S e t O b j e c t P o s i t i o n ( f r o n t m o t o r , −1 ,{ i n F l o a t s [ 1 8 ] , i n F l o a t s
[19] , inFloats [20]})
simSetObjectOrientation ( front motor , −1 ,{0 ,0 ,0})
simSetObjectName ( f r o n t m o t o r , ’ t r i c y c l e f r o n t m o t o r ’ )
simSetObjectInt32Parameter ( front motor ,
sim jointintparam motor enabled ,1 )
simSetObjectInt32Parameter ( front motor ,
sim jointintparam ctrl enabled ,1)
simSetObjectInt32Parameter ( front motor ,
sim objintparam visibility layer ,1024)
196
197
198
199
200
201
f r o n t w h e e l a x i s=s i m C r e a t e J o i n t ( s i m j o i n t r e v o l u t e s u b t y p e ,
s i m j o i n t m o d e f o r c e , 0 ,NULL, NULL,NULL)
s i m S e t O b j e c t P o s i t i o n ( f r o n t w h e e l a x i s , −1 ,{ i n F l o a t s [ 1 8 ] ,
inFloats [19] , inFloats [20]})
s i m S e t O b j e c t O r i e n t a t i o n ( f r o n t w h e e l a x i s , −1 ,{0 , − m a t h . p i
/2 ,0})
simSetObjectName ( f r o n t w h e e l a x i s , ’ t r i c y c l e f r o n t w h e e l a x i s
’)
simSetObjectInt32Parameter ( f r o n t w h e e l a x i s ,
sim objintparam visibility layer ,1024)
202
203
p r o x s e n s o r=s i m C r e a t e P r o x i m i t y S e n s o r (
sim proximitysensor disc subtype ,
sim objectspecialproperty detectable ultrasonic
,0000000000 ,{16 ,32 ,0 ,0 ,0 ,0 ,0 ,0} ,{0 .0 , i n F l o a t s [ 2 1 ] , 0 .0 , 0
.05 , 0 .0 , 0 .0 , 0 .0 , 0 .1 , 0 .0 , math.pi /2 ,0 .0 , 0 .0 , 0 .01 , 0 .0 , 0 . 0 } ,
Construcción de plataforma software para el diseño y la simulación...
174
B CÓDIGO LUA
204
205
206
Carlos Sosa Marrero
NULL)
s i m S e t O b j e c t P o s i t i o n ( p r o x s e n s o r , −1 ,{ i n F l o a t s [ 2 2 ] , i n F l o a t s
[23] , inFloats [24]})
s i m S e t O b j e c t O r i e n t a t i o n ( p r o x s e n s o r ,−1,{− m a t h . p i / 2 , 0 , 0 } )
simSetObjectName ( p r o x s e n s o r , ’ t r i c y c l e p r o x s e n s o r ’ )
207
208
209
210
211
l e f t v i s i o n s e n s o r=s i m C r e a t e V i s i o n S e n s o r
(00000000 ,{1 ,1 ,0 ,0} ,{0 .01 , i n F l o a t s [ 2 5 ] , 0 .01 , 0 .01 , 0 .01 , 0
. 0 1 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 } ,NULL)
s i m S e t O b j e c t P o s i t i o n ( l e f t v i s i o n s e n s o r , −1 ,{ i n F l o a t s [ 2 6 ] ,
inFloats [27] , inFloats [28]})
simSetObjectName ( l e f t v i s i o n s e n s o r , ’
tricycle left vision sensor ’)
s i m S e t O b j e c t O r i e n t a t i o n ( l e f t v i s i o n s e n s o r , −1 ,{ math.pi , 0 , 0 } )
212
213
214
215
216
r i g h t v i s i o n s e n s o r=s i m C r e a t e V i s i o n S e n s o r
(00000000 ,{1 ,1 ,0 ,0} ,{0 .01 , i n F l o a t s [ 2 5 ] , 0 .01 , 0 .01 , 0 .01 , 0
. 0 1 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 } ,NULL)
s i m S e t O b j e c t P o s i t i o n ( r i g h t v i s i o n s e n s o r , −1 ,{ i n F l o a t s [ 2 9 ] ,
inFloats [30] , inFloats [31]})
simSetObjectName ( r i g h t v i s i o n s e n s o r , ’
tricycle right vision sensor ’)
s i m S e t O b j e c t O r i e n t a t i o n ( r i g h t v i s i o n s e n s o r , −1 ,{ math.pi
,0 ,0})
217
218
219
220
221
m i d d l e v i s i o n s e n s o r=s i m C r e a t e V i s i o n S e n s o r
(00000000 ,{1 ,1 ,0 ,0} ,{0 .01 , math.sqrt (2∗5 .00 ˆ2) ,0 .01 , 0 .01 , 0
. 0 1 , 0 . 0 1 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 } ,NULL)
s i m S e t O b j e c t P o s i t i o n ( m i d d l e v i s i o n s e n s o r , −1 ,{ i n F l o a t s [ 3 2 ] ,
inFloats [33] , inFloats [34]})
simSetObjectName ( m i d d l e v i s i o n s e n s o r , ’
tricycle middle vision sensor ’)
s i m S e t O b j e c t O r i e n t a t i o n ( m i d d l e v i s i o n s e n s o r ,−1,{− m a t h . p i
/2 ,0 ,0})
222
223
224
225
226
227
228
229
230
231
232
simSetObjectParent ( l e f t w h e e l , left motor , true )
simSetObjectParent ( right wheel , right motor , true )
simSetObjectParent ( front wheel , f r o n t w h e e l a x i s , true )
simSetObjectParent ( f r o n t w h e e l a x i s , inv fro nt whe el , true )
simSetObjectParent ( i nv fro nt whe el , front motor , true )
simSetObjectParent ( left motor , t r i c y c l e , true )
simSetObjectParent ( right motor , t r i c y c l e , true )
simSetObjectParent ( front motor , t r i c y c l e , true )
simSetObjectParent ( prox sensor , t r i c y c l e , true )
Construcción de plataforma software para el diseño y la simulación...
175
B CÓDIGO LUA
Carlos Sosa Marrero
simSetObjectParent ( l e f t v i s i o n s e n s o r , t r i c y c l e , true )
simSetObjectParent ( r i g h t v i s i o n s e n s o r , t r i c y c l e , true )
simSetObjectParent ( m i d d l e v i s i o n s e n s o r , t r i c y c l e , true )
233
234
235
236
237
return {} ,{} ,{} , ’ ’
238
239
end
240
241
242
243
244
245
246
247
248
249
250
251
−−Función que c r e a un r o b o t de t i p o c u a t r i c i c l o en l a e s c e n a a
p a r t i r de l o s p a r á m e t r o s r e c i b i d o s d e s d e Matlab. Para una
i n f o r m a c i ó n
−−d e t a l l a d a a c e r c a de l o s argumentos de cada f u n c i ó n API ,
c o n s u l t a r e l l i s t a d o ’ R egu lar API f u n c t i o n s ’ .
C r e a t e Q u a d r i c y c l e f u n c t i o n=f u n c t i o n ( i n I n t s , i n F l o a t s , i n S t r i n g s ,
inBuffer )
q u a d r i c y c l e=simCreatePureShape ( i n I n t s [ 1 ] , 0 1 1 1 0 , { i n F l o a t s [ 1 ] ,
i n F l o a t s [ 2 ] , i n F l o a t s [ 3 ] } , i n F l o a t s [ 4 ] , NULL)
s i m S e t O b j e c t P o s i t i o n ( q u a d r i c y c l e , −1 ,{ i n F l o a t s [ 5 ] , i n F l o a t s
[6] , inFloats [7]})
simSetObjectName ( q u a d r i c y c l e , ’ q u a d r i c y c l e ’ )
simSetObjectSpecialProperty ( quadricycle ,
s i m o b j e c t s p e c i a l p r o p e r t y m e a s u r a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y c o l l i d a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y r e n d e r a b l e+
sim objectspecialproperty detectable all )
simSetObjectInt32Parameter ( quadricycle ,
sim shapeintparam static ,0)
simSetObjectInt32Parameter ( quadricycle ,
sim shapeintparam respondable , 1 )
s i m S e t S h a p e C o l o r ( q u a d r i c y c l e , NULL,
sim colorcomponent ambient diffuse ,{0 .19 , 0 .88 , 0 .34 })
252
253
254
255
256
257
r e a r l e f t w h e e l=simCreatePureShape ( 2 , 0 1 1 1 0 , { i n F l o a t s [ 8 ] ,
i n F l o a t s [ 9 ] , i n F l o a t s [ 1 0 ] } , i n F l o a t s [ 1 1 ] ,NULL)
s i m S e t O b j e c t P o s i t i o n ( r e a r l e f t w h e e l , −1 ,{ i n F l o a t s [ 1 2 ] ,
inFloats [13] , inFloats [14]})
s i m S e t O b j e c t O r i e n t a t i o n ( r e a r l e f t w h e e l , −1 ,{0 , − m a t h . p i / 2 , 0 } )
simSetObjectName ( r e a r l e f t w h e e l , ’
quadricycle rear left wheel ’ )
simSetObjectSpecialProperty ( rear left wheel ,
s i m o b j e c t s p e c i a l p r o p e r t y m e a s u r a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y c o l l i d a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y r e n d e r a b l e+
sim objectspecialproperty detectable all )
Construcción de plataforma software para el diseño y la simulación...
176
B CÓDIGO LUA
258
259
260
261
Carlos Sosa Marrero
simSetObjectInt32Parameter ( r e a r l e f t w h e e l ,
sim shapeintparam static ,0)
simSetObjectInt32Parameter ( r e a r l e f t w h e e l ,
sim shapeintparam respondable , 1 )
s i m S e t S h a p e C o l o r ( r e a r l e f t w h e e l , NULL,
sim colorcomponent ambient diffuse ,{0 .15 , 0 .15 , 0 .15 })
simSetShapeMaterial ( r e a r l e f t w h e e l , wheelMaterial )
262
263
264
265
266
267
268
269
270
271
r e a r r i g h t w h e e l=simCreatePureShape ( 2 , 0 1 1 1 0 , { i n F l o a t s [ 8 ] ,
i n F l o a t s [ 9 ] , i n F l o a t s [ 1 0 ] } , i n F l o a t s [ 1 1 ] ,NULL)
s i m S e t O b j e c t P o s i t i o n ( r e a r r i g h t w h e e l , −1 ,{ i n F l o a t s [ 1 5 ] ,
inFloats [16] , inFloats [17]})
s i m S e t O b j e c t O r i e n t a t i o n ( r e a r r i g h t w h e e l , −1 ,{0 , − m a t h . p i
/2 ,0})
simSetObjectName ( r e a r r i g h t w h e e l , ’
quadricycle rear right wheel ’ )
simSetObjectSpecialProperty ( rear right wheel ,
s i m o b j e c t s p e c i a l p r o p e r t y m e a s u r a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y c o l l i d a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y r e n d e r a b l e+
sim objectspecialproperty detectable all )
simSetObjectInt32Parameter ( r e a r r i g h t w h e e l ,
sim shapeintparam static ,0)
simSetObjectInt32Parameter ( r e a r r i g h t w h e e l ,
sim shapeintparam respondable , 1 )
s i m S e t S h a p e C o l o r ( r e a r r i g h t w h e e l , NULL,
sim colorcomponent ambient diffuse ,{0 .15 , 0 .15 , 0 .15 })
simSetShapeMaterial ( r e a r r i g h t w h e e l , wheelMaterial )
272
273
274
275
276
277
278
279
f r o n t l e f t w h e e l=simCreatePureShape ( 2 , 0 1 1 1 0 , { i n F l o a t s [ 8 ] ,
i n F l o a t s [ 9 ] , i n F l o a t s [ 1 0 ] } , i n F l o a t s [ 1 1 ] ,NULL)
s i m S e t O b j e c t P o s i t i o n ( f r o n t l e f t w h e e l , −1 ,{ i n F l o a t s [ 1 8 ] ,
inFloats [19] , inFloats [20]})
s i m S e t O b j e c t O r i e n t a t i o n ( f r o n t l e f t w h e e l , −1 ,{0 , − m a t h . p i
/2 ,0})
simSetObjectName ( f r o n t l e f t w h e e l , ’
quadricycle front left wheel ’)
simSetObjectSpecialProperty ( f r o n t l e f t w h e el ,
s i m o b j e c t s p e c i a l p r o p e r t y m e a s u r a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y c o l l i d a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y r e n d e r a b l e+
sim objectspecialproperty detectable all )
simSetObjectInt32Parameter ( f r o n t l e f t w h e e l ,
sim shapeintparam static ,0)
simSetObjectInt32Parameter ( f r o n t l e f t w h e e l ,
Construcción de plataforma software para el diseño y la simulación...
177
B CÓDIGO LUA
280
281
Carlos Sosa Marrero
sim shapeintparam respondable , 1 )
s i m S e t S h a p e C o l o r ( f r o n t l e f t w h e e l , NULL,
sim colorcomponent ambient diffuse ,{0 .15 , 0 .15 , 0 .15 })
simSetShapeMaterial ( f r o n t l e f t w h e e l , wheelMaterial )
282
283
284
285
286
287
288
289
290
i n v f r o n t l e f t w h e e l=simCreatePureShape ( 2 , 0 1 1 1 0 , { i n F l o a t s
[ 8 ] , i n F l o a t s [ 9 ] , i n F l o a t s [ 1 0 ] } , i n F l o a t s [ 1 1 ] ,NULL)
s i m S e t O b j e c t P o s i t i o n ( i n v f r o n t l e f t w h e e l , −1 ,{ i n F l o a t s [ 1 8 ] ,
inFloats [19] , inFloats [20]})
s i m S e t O b j e c t O r i e n t a t i o n ( i n v f r o n t l e f t w h e e l , −1 ,{0 , − m a t h . p i
/2 ,0})
simSetObjectName ( i n v f r o n t l e f t w h e e l , ’
quadricycle inv front left wheel ’)
simSetObjectInt32Parameter ( i n v f r o n t l e f t w h e e l ,
sim shapeintparam static ,0)
simSetObjectInt32Parameter ( i n v f r o n t l e f t w h e e l ,
sim shapeintparam respondable , 0 )
simSetObjectInt32Parameter ( i n v f r o n t l e f t w h e e l ,
sim objintparam visibility layer ,1024)
simSetShapeMaterial ( i n v f r o n t l e f t w h e e l , wheelMaterial )
291
292
293
294
295
296
297
298
299
300
f r o n t r i g h t w h e e l=simCreatePureShape ( 2 , 0 1 1 1 0 , { i n F l o a t s [ 8 ] ,
i n F l o a t s [ 9 ] , i n F l o a t s [ 1 0 ] } , i n F l o a t s [ 1 1 ] ,NULL)
s i m S e t O b j e c t P o s i t i o n ( f r o n t r i g h t w h e e l , −1 ,{ i n F l o a t s [ 2 1 ] ,
inFloats [22] , inFloats [23]})
s i m S e t O b j e c t O r i e n t a t i o n ( f r o n t r i g h t w h e e l , −1 ,{0 , − m a t h . p i
/2 ,0})
simSetObjectName ( f r o n t r i g h t w h e e l , ’
quadricycle front right wheel ’ )
simSetObjectSpecialProperty ( front right wheel ,
s i m o b j e c t s p e c i a l p r o p e r t y m e a s u r a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y c o l l i d a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y r e n d e r a b l e+
sim objectspecialproperty detectable all )
simSetObjectInt32Parameter ( f r o n t r i g h t w h e e l ,
sim shapeintparam static ,0)
simSetObjectInt32Parameter ( f r o n t r i g h t w h e e l ,
sim shapeintparam respondable , 1 )
s i m S e t S h a p e C o l o r ( f r o n t r i g h t w h e e l , NULL,
sim colorcomponent ambient diffuse ,{0 .15 , 0 .15 , 0 .15 })
simSetShapeMaterial ( f r o n t r i g h t w h e e l , wheelMaterial )
301
302
303
i n v f r o n t r i g h t w h e e l=simCreatePureShape ( 2 , 0 1 1 1 0 , { i n F l o a t s
[ 8 ] , i n F l o a t s [ 9 ] , i n F l o a t s [ 1 0 ] } , i n F l o a t s [ 1 1 ] ,NULL)
s i m S e t O b j e c t P o s i t i o n ( i n v f r o n t r i g h t w h e e l , −1 ,{ i n F l o a t s [ 2 1 ] ,
Construcción de plataforma software para el diseño y la simulación...
178
B CÓDIGO LUA
304
305
306
307
308
309
Carlos Sosa Marrero
inFloats [22] , inFloats [23]})
s i m S e t O b j e c t O r i e n t a t i o n ( i n v f r o n t r i g h t w h e e l , −1 ,{0 , − m a t h . p i
/2 ,0})
simSetObjectName ( i n v f r o n t r i g h t w h e e l , ’
quadricycle inv rear right wheel ’ )
simSetObjectInt32Parameter ( i n v f r o n t r i g h t w h e e l ,
sim shapeintparam static ,0)
simSetObjectInt32Parameter ( i n v f r o n t r i g h t w h e e l ,
sim shapeintparam respondable , 0 )
simSetObjectInt32Parameter ( i n v f r o n t r i g h t w h e e l ,
sim objintparam visibility layer ,1024)
simSetShapeMaterial ( i n v f r o n t r i g h t w h e e l , wheelMaterial )
310
311
312
313
314
315
316
317
r e a r l e f t m o t o r=s i m C r e a t e J o i n t ( s i m j o i n t r e v o l u t e s u b t y p e ,
s i m j o i n t m o d e f o r c e , 0 ,NULL, NULL,NULL)
s i m S e t O b j e c t P o s i t i o n ( r e a r l e f t m o t o r , −1 ,{ i n F l o a t s [ 1 2 ] ,
inFloats [13] , inFloats [14]})
s i m S e t O b j e c t O r i e n t a t i o n ( r e a r l e f t m o t o r , −1 ,{0 , − m a t h . p i / 2 , 0 } )
simSetObjectName ( r e a r l e f t m o t o r , ’
quadricycle rear left motor ’ )
simSetObjectInt32Parameter ( r e a r l e f t m o t o r ,
sim jointintparam motor enabled ,1 )
simSetObjectInt32Parameter ( r e a r l e f t m o t o r ,
sim jointintparam velocity lock ,1)
simSetObjectInt32Parameter ( r e a r l e f t m o t o r ,
sim objintparam visibility layer ,1024)
318
319
320
321
322
323
324
325
r e a r r i g h t m o t o r=s i m C r e a t e J o i n t ( s i m j o i n t r e v o l u t e s u b t y p e ,
s i m j o i n t m o d e f o r c e , 0 ,NULL, NULL,NULL)
s i m S e t O b j e c t P o s i t i o n ( r e a r r i g h t m o t o r , −1 ,{ i n F l o a t s [ 1 5 ] ,
inFloats [16] , inFloats [17]})
s i m S e t O b j e c t O r i e n t a t i o n ( r e a r r i g h t m o t o r , −1 ,{0 , − m a t h . p i
/2 ,0})
simSetObjectName ( r e a r r i g h t m o t o r , ’
quadricycle rear right motor ’ )
simSetObjectInt32Parameter ( rear right motor ,
sim jointintparam motor enabled ,1 )
simSetObjectInt32Parameter ( rear right motor ,
sim jointintparam velocity lock ,1)
simSetObjectInt32Parameter ( rear right motor ,
sim objintparam visibility layer ,1024)
326
327
328
f r o n t l e f t m o t o r=s i m C r e a t e J o i n t ( s i m j o i n t r e v o l u t e s u b t y p e ,
s i m j o i n t m o d e f o r c e , 0 ,NULL, NULL,NULL)
s i m S e t O b j e c t P o s i t i o n ( f r o n t l e f t m o t o r , −1 ,{ i n F l o a t s [ 1 8 ] ,
Construcción de plataforma software para el diseño y la simulación...
179
B CÓDIGO LUA
329
330
331
332
333
Carlos Sosa Marrero
inFloats [19] , inFloats [20]})
simSetObjectOrientation ( f r o n t l e f t m o t o r , −1 ,{0 ,0 ,0})
simSetObjectName ( f r o n t l e f t m o t o r , ’
quadricycle front left motor ’ )
simSetObjectInt32Parameter ( f r o n t l e f t m o t o r ,
sim jointintparam motor enabled ,1 )
simSetObjectInt32Parameter ( f r o n t l e f t m o t o r ,
sim jointintparam ctrl enabled ,1)
simSetObjectInt32Parameter ( f r o n t l e f t m o t o r ,
sim objintparam visibility layer ,1024)
334
335
336
337
338
339
f r o n t l e f t w h e e l a x i s=s i m C r e a t e J o i n t (
s i m j o i n t r e v o l u t e s u b t y p e , s i m j o i n t m o d e f o r c e , 0 ,NULL,
NULL,NULL)
s i m S e t O b j e c t P o s i t i o n ( f r o n t l e f t w h e e l a x i s , −1 ,{ i n F l o a t s [ 1 8 ] ,
inFloats [19] , inFloats [20]})
s i m S e t O b j e c t O r i e n t a t i o n ( f r o n t l e f t w h e e l a x i s , −1 ,{0 , − m a t h . p i
/2 ,0})
simSetObjectName ( f r o n t l e f t w h e e l a x i s , ’
quadricycle front left wheel axis ’)
simSetObjectInt32Parameter ( f r o n t l e f t w h e e l a x i s ,
sim objintparam visibility layer ,1024)
340
341
342
343
344
345
346
347
f r o n t r i g h t m o t o r=s i m C r e a t e J o i n t ( s i m j o i n t r e v o l u t e s u b t y p e ,
s i m j o i n t m o d e f o r c e , 0 ,NULL, NULL,NULL)
s i m S e t O b j e c t P o s i t i o n ( f r o n t r i g h t m o t o r , −1 ,{ i n F l o a t s [ 2 1 ] ,
inFloats [22] , inFloats [23]})
simSetObjectOrientation ( front right motor , −1 ,{0 ,0 ,0})
simSetObjectName ( f r o n t r i g h t m o t o r , ’
quadricycle front right motor ’ )
simSetObjectInt32Parameter ( front right motor ,
sim jointintparam motor enabled ,1 )
simSetObjectInt32Parameter ( front right motor ,
sim jointintparam ctrl enabled ,1)
simSetObjectInt32Parameter ( front right motor ,
sim objintparam visibility layer ,1024)
348
349
350
351
352
f r o n t r i g h t w h e e l a x i s=s i m C r e a t e J o i n t (
s i m j o i n t r e v o l u t e s u b t y p e , s i m j o i n t m o d e f o r c e , 0 ,NULL,
NULL,NULL)
s i m S e t O b j e c t P o s i t i o n ( f r o n t r i g h t w h e e l a x i s , −1 ,{ i n F l o a t s
[21] , inFloats [22] , inFloats [23]})
s i m S e t O b j e c t O r i e n t a t i o n ( f r o n t r i g h t w h e e l a x i s , −1 ,{0 , −
math.pi /2 ,0})
simSetObjectName ( f r o n t r i g h t w h e e l a x i s , ’
Construcción de plataforma software para el diseño y la simulación...
180
B CÓDIGO LUA
353
Carlos Sosa Marrero
quadricycle front right wheel axis ’ )
simSetObjectInt32Parameter ( f r o n t r i g h t w h e e l a x i s ,
sim objintparam visibility layer ,1024)
354
355
356
357
358
p r o x s e n s o r=s i m C r e a t e P r o x i m i t y S e n s o r (
sim proximitysensor disc subtype ,
sim objectspecialproperty detectable ultrasonic
,0000000000 ,{16 ,32 ,0 ,0 ,0 ,0 ,0 ,0} ,{0 .0 , i n F l o a t s [ 2 4 ] , 0 .0 , 0
.05 , 0 .0 , 0 .0 , 0 .0 , 0 .1 , 0 .0 , math.pi /2 ,0 .0 , 0 .0 , 0 .01 , 0 .0 , 0 . 0 } ,
NULL)
s i m S e t O b j e c t P o s i t i o n ( p r o x s e n s o r , −1 ,{ i n F l o a t s [ 2 5 ] , i n F l o a t s
[26] , inFloats [27]})
s i m S e t O b j e c t O r i e n t a t i o n ( p r o x s e n s o r ,−1,{− m a t h . p i / 2 , 0 , 0 } )
simSetObjectName ( p r o x s e n s o r , ’ q u a d r i c y c l e p r o x s e n s o r ’ )
359
360
361
362
363
l e f t v i s i o n s e n s o r=s i m C r e a t e V i s i o n S e n s o r
(00000000 ,{1 ,1 ,0 ,0} ,{0 .01 , i n F l o a t s [ 2 8 ] , 0 .01 , 0 .01 , 0 .01 , 0
. 0 1 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 } ,NULL)
s i m S e t O b j e c t P o s i t i o n ( l e f t v i s i o n s e n s o r , −1 ,{ i n F l o a t s [ 2 9 ] ,
inFloats [30] , inFloats [31]})
simSetObjectName ( l e f t v i s i o n s e n s o r , ’
quadricycle left vision sensor ’)
s i m S e t O b j e c t O r i e n t a t i o n ( l e f t v i s i o n s e n s o r , −1 ,{ math.pi , 0 , 0 } )
364
365
366
367
368
r i g h t v i s i o n s e n s o r=s i m C r e a t e V i s i o n S e n s o r
(00000000 ,{1 ,1 ,0 ,0} ,{0 .01 , i n F l o a t s [ 2 8 ] , 0 .01 , 0 .01 , 0 .01 , 0
. 0 1 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 } ,NULL)
s i m S e t O b j e c t P o s i t i o n ( r i g h t v i s i o n s e n s o r , −1 ,{ i n F l o a t s [ 3 2 ] ,
inFloats [33] , inFloats [34]})
simSetObjectName ( r i g h t v i s i o n s e n s o r , ’
quadricycle right vision sensor ’ )
s i m S e t O b j e c t O r i e n t a t i o n ( r i g h t v i s i o n s e n s o r , −1 ,{ math.pi
,0 ,0})
369
370
371
372
373
m i d d l e v i s i o n s e n s o r=s i m C r e a t e V i s i o n S e n s o r
(00000000 ,{1 ,1 ,0 ,0} ,{0 .01 , math.sqrt (2∗5 .00 ˆ2) ,0 .01 , 0 .01 , 0
. 0 1 , 0 . 0 1 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 , 0 . 0 } ,NULL)
s i m S e t O b j e c t P o s i t i o n ( m i d d l e v i s i o n s e n s o r , −1 ,{ i n F l o a t s [ 3 5 ] ,
inFloats [36] , inFloats [37]})
simSetObjectName ( m i d d l e v i s i o n s e n s o r , ’
quadricycle middle vision sensor ’ )
s i m S e t O b j e c t O r i e n t a t i o n ( m i d d l e v i s i o n s e n s o r ,−1,{− m a t h . p i
/2 ,0 ,0})
374
375
simSetObjectParent ( r e a r l e f t w h e e l , r e a r l e f t m o t o r , true )
Construcción de plataforma software para el diseño y la simulación...
181
B CÓDIGO LUA
Carlos Sosa Marrero
simSetObjectParent ( r e a r r i g h t w h e e l , rear right motor , true )
simSetObjectParent ( f r o n t l e f t w h e e l , f r o n t l e f t w h e e l a x i s ,
true )
simSetObjectParent ( f r o n t l e f t w h e e l a x i s ,
i nv f ro nt l ef t w he e l , true )
simSetObjectParent ( i n v f r o n t l e f t w h e e l , f r o n t l e f t m o t o r ,
true )
simSetObjectParent ( f r o n t r i g h t w h e e l , f r o n t r i g h t w h e e l a x i s ,
true )
simSetObjectParent ( f r o n t r i g h t w h e e l a x i s ,
inv front right wheel , true )
simSetObjectParent ( i n v f r o n t r i g h t w h e e l , fr ont ri gh t mo to r ,
true )
simSetObjectParent ( r e a r l e f t m o t o r , quadricycle , true )
simSetObjectParent ( rear right motor , quadricycle , true )
simSetObjectParent ( f r o n t l e f t m o t o r , quadricycle , true )
simSetObjectParent ( fro nt ri ght mo to r , quadricycle , true )
simSetObjectParent ( prox sensor , quadricycle , true )
simSetObjectParent ( l e f t v i s i o n s e n s o r , quadricycle , true )
simSetObjectParent ( r i g h t v i s i o n s e n s o r , quadricycle , true )
simSetObjectParent ( m i d d l e v i s i o n s e n s o r , quadricycle , true )
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
return {} ,{} ,{} , ’ ’
393
394
end
395
396
397
398
399
−−Función que c r e a un o b s t á c u l o en l a e s c e n a a p a r t i r de l o s
p a r á m e t r o s r e c i b i d o s d e s d e Matlab. Para una i n f o r m a c i ó n
−−d e t a l l a d a a c e r c a de l o s argumentos de cada f u n c i ó n API ,
c o n s u l t a r e l l i s t a d o ’ R egu lar API f u n c t i o n s ’ .
C r e a t e O b s t a c l e f u n c t i o n=f u n c t i o n ( i n I n t s , i n F l o a t s , i n S t r i n g s ,
inBuffer )
400
401
402
403
404
405
o b s t a c l e=simCreatePureShape ( i n I n t s [ 1 ] , 0 1 1 1 0 , { i n F l o a t s [ 1 ] ,
i n F l o a t s [ 2 ] , i n F l o a t s [ 3 ] } , i n F l o a t s [ 7 ] , NULL)
s i m S e t O b j e c t P o s i t i o n ( o b s t a c l e , −1 ,{ i n F l o a t s [ 4 ] , i n F l o a t s [ 5 ] ,
inFloats [6]})
simSetObjectName ( o b s t a c l e , i n S t r i n g s [ 1 ] )
simSetObjectSpecialProperty ( obstacle ,
s i m o b j e c t s p e c i a l p r o p e r t y m e a s u r a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y c o l l i d a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y r e n d e r a b l e+
sim objectspecialproperty detectable all )
simSetObjectInt32Parameter ( obstacle ,
Construcción de plataforma software para el diseño y la simulación...
182
B CÓDIGO LUA
sim shapeintparam respondable , 1 )
s i m S e t S h a p e C o l o r ( o b s t a c l e , NULL,
sim colorcomponent ambient diffuse ,{0 .95 , 0 .95 , 0 .95 })
return { obstacle } ,{} ,{} , ’ ’
406
407
408
Carlos Sosa Marrero
end
409
410
411
412
413
−−Función que c r e a una b a l i z a en l a e s c e n a a p a r t i r de l o s
p a r á m e t r o s r e c i b i d o s d e s d e Matlab. Para una i n f o r m a c i ó n
−−d e t a l l a d a a c e r c a de l o s argumentos de cada f u n c i ó n API ,
c o n s u l t a r e l l i s t a d o ’ R egu lar API f u n c t i o n s ’ .
C r e a t e M a r k e r f u n c t i o n=f u n c t i o n ( i n I n t s , i n F l o a t s , i n S t r i n g s ,
inBuffer )
414
marker=simCreatePureShape ( 2 , 0 1 1 1 0 , { 0 . 2 5 , 0 . 2 5 , 0 . 5 } , 1 . 0 ,NULL)
s i m S e t O b j e c t P o s i t i o n ( marker , −1 ,{ i n F l o a t s [ 1 ] , i n F l o a t s [ 2 ] ,
inFloats [3]})
simSetObjectName ( marker , i n S t r i n g s [ 1 ] )
s i m S e t O b j e c t S p e c i a l P r o p e r t y ( marker ,
s i m o b j e c t s p e c i a l p r o p e r t y m e a s u r a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y c o l l i d a b l e+
s i m o b j e c t s p e c i a l p r o p e r t y r e n d e r a b l e+
sim objectspecialproperty detectable all )
s i m S e t O b j e c t I n t 3 2 P a r a m e t e r ( marker ,
sim shapeintparam respondable , 1 )
s i m S e t S h a p e C o l o r ( marker , NULL,
sim colorcomponent ambient diffuse ,{0 .0 , 1 .0 , 0 . 0 })
r e t u r n { marker } , { } , { } , ’ ’
415
416
417
418
419
420
421
422
end
423
424
425
426
427
428
429
430
−−Función que p o s i c i o n a un o b s t á c u l o o b a l i z a en l a e s c e n a a
p a r t i r de l o s p a r á m e t r o s r e c i b i d o s d e s d e Matlab. Para una
i n f o r m a c i ó n
−−d e t a l l a d a a c e r c a de l o s argumentos de l a f u n c i ó n API ,
c o n s u l t a r e l l i s t a d o ’ R egu lar API f u n c t i o n s ’ .
P o s i t i o n f u n c t i o n=f u n c t i o n ( i n I n t s , i n F l o a t s , i n S t r i n g s , i n B u f f e r )
simSetObjectPosition ( in Int s [1] , −1 ,{ inFloats [ 1 ] , inFloats [ 2 ] ,
inFloats [3]})
return {} ,{} ,{} , ’ ’
end
431
432
433
−−Función que o c u l t a l o s s e n s o r e s que no s e u t i l i z a n en l a
e s c e n a a p a r t i r de l o s p a r á m e t r o s r e c i b i d o s d e s d e Matlab.
Construcción de plataforma software para el diseño y la simulación...
183
B CÓDIGO LUA
434
435
436
437
438
439
440
441
442
443
444
445
446
447
Carlos Sosa Marrero
Para una i n f o r m a c i ó n
−−d e t a l l a d a a c e r c a de l o s argumentos de cada f u n c i ó n API ,
c o n s u l t a r e l l i s t a d o ’ R egu lar API f u n c t i o n s ’ .
H i d e f u n c t i o n=f u n c t i o n ( i n I n t s , i n F l o a t s , i n S t r i n g s , i n B u f f e r )
i f ( i n I n t s [1]==1) then
simSetObjectInt32Parameter ( l e f t v i s i o n s e n s o r ,
sim objintparam visibility layer ,1024)
simSetObjectInt32Parameter ( r i g h t v i s i o n s e n s o r ,
sim objintparam visibility layer ,1024)
simSetObjectInt32Parameter ( m i d d l e v i s i o n s e n s o r ,
sim objintparam visibility layer ,1024)
e l s e i f ( i n I n t s [1]==2) then
simSetObjectInt32Parameter ( m i d d l e v i s i o n s e n s o r ,
sim objintparam visibility layer ,1024)
e l s e i f ( i n I n t s [1]==3) then
simSetObjectInt32Parameter ( l e f t v i s i o n s e n s o r ,
sim objintparam visibility layer ,1024)
simSetObjectInt32Parameter ( r i g h t v i s i o n s e n s o r ,
sim objintparam visibility layer ,1024)
end
return {} ,{} ,{} , ’ ’
end
Construcción de plataforma software para el diseño y la simulación...
184