Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

Baquero Juan, Galvis Juan, Lpez Diego, Mesa Juan, Villalba Camilo, Villamizar Csar
(02285993, 02285854, 02285811, 02285851, 022548891, 02286037)
Ingeniera Mecatrnica - Robtica
Proyecto final de Robtica
9 de Diciembre, 2015
1. Proceso de robtica industrial
a) Descripcin
El objetivo del proyecto fue realizar una rutina de pick and place utilizando
el brazo robtico IRB140 de ABB encontrado en el LabSIR de la Universidad
Nacional de Colombia.
Como parte de los requerimientos de realizacin del proyecto se dise y fabric
una herramienta de sujeccin, gripper, tal que fuera capaz de agarrar unas
determinadas formas y se pudiese controlar a partir de las salidas digitales del
controlador del robot.
Esta clase de tareas son comnmente realizadas en la actualidad por robots
debido a que son repetitivas y pueden ser ejecutadas de una forma ms rpida
por un sistema automatizado que por personas.

Figura 1: Estante y figuras a sostener y re-ubicar

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

b) Diagrama del robot y tabla de parmetros DH

En las Figuras 2 y 3 se presentan la ubicacin y orientacin de los sistemas de
coordenadas en las articulaciones del robot, as como las medidas necesarias
para poder describir la cinemtica del brazo.

Figura 2: Ubicacin de sistemas de coordenadas del robot.

Figura 3: Medidas entre las articulaciones del robot.

2

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

Como parte del anlisis de la cinemtica se procede a realizar la tabla de

parmetros Denavit-Hartenberg para el robot, presentada en el Cuadro 1.
Cuadro 1: Parmetros DH del robot industrial IRB140
i 1
di ai1
i1
1 q1 352
0
0
2 q2
0
70 /2
3 q3
0 360
0
4 q4 380
0 /2
5 q5
0
0
/2
6 q6
65
0 /2
c) Diseo de herramienta
Para el diseo del gripper requerido se decide hacer uso de un servomotor TowerPro MG-90S ajustado a un mecanismo previamente diseado en un software
CAD.
El mecanismo consta de dos parejas de pinzas mviles conectadas por un sistema de cuatro barras al servomotor. Una pareja de engranes acta para transmitir el movimiento entre las dos pinzas del gripper y mantener dicho movimiento
sincronizado.

Figura 4: Microservomotor MG-90S usado para mover el sistema del gripper.

El servomotor tiene un torque de hasta 2.2Kgf*cm y su posicin es controlada
mediante una tarjeta Arduino UNO.

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

Figura 5: Modelo de gripper diseado en Autodesk Inventor

Se establece el TCP de la herramienta respecto al tool0 del robot para cuando
se sostiene un objeto de 50mm de ancho (medidas de los elementos a sujetar).
Las coordenadas en milmetros son:
T CPx = 0
T CPy = 0
T CPz = 113
Con el fin de que la herramienta pueda ser controlada desde el proceso diseado
en el software RobotStudio, se asignan dos salidas digitales del controlador para
este fin.

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

Figura 6: Panel de conexiones de entrada/salida del controlador del robot IRB140

Conectado mediante cables hacia el panel de entradas y salidas del controlador,
se dispuso un relay, de modo que ste pudiera conmutar sus contactos para tener
una seal digital de 0 o 5 voltios dirigida a la tarjeta Arduino, la cual realiza
la accin de control sobre la posicin del servomotor para obtener la apertura
o el cierre adecuado del gripper.
d) Descripcin del proceso e interaccin con el usuario
El proceso de pick and place consta de dos partes, primero el piking en el cual
se selecciona manualmente por medio del FlexPendant, seleccionando la posicin en la cual se encuentran ubicadas cada una de la piezas, donde la posicin
1 es la posicin baja izquierda del armario. Despus de tomar la pieza realiza
la segunda parte, la cual es el placing, la rutina esta programada para dejar
cada una de las piezas en una posicin diferente como se muestra en el video.
Cada una de las rutinas del picking tiene una posicin de acercamiento, la cual
se encuentra frente a la pieza para cada una de las posiciones del armario, a
5

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

la cual llega con una velocidad alta y a partir de la cual se desplaza a una
velocidad lenta con el objetivo de ser cuidadoso a la hora de tomar la pieza,
seguido de esto eleva la pieza y regresa a la posicin de acercamiento. Para
llegar a la posicin de place pasa por una posicin intermedia, la cual es una
posicin con la cual se evitan singularidades al desplazarse hacia cada una de
las posiciones.
Las tres posiciones del placing tienen tambin una posicin de acercamiento la
cual se encuentra sobre la posicin en la cual dejara la pieza y a partir de la
cual se desplaza con una velocidad lenta, esto tambin con el objetivo de ser
cuidadosos a la hora de depositar las piezas.
En la simulacin en RobotStudio se crearon 2 objetos de trabajo, uno para los
puntos del picking y otro para los puntos del placing, con esto se logra modificar independientemente la posicin de recogida de los objetos en el armario y
la posicin de dejar los objetos, esto tambin con el objetivo de poder calibrar
las posiciones de pick y place de una forma fcil.

Figura 7: Simulacin del proyecto en RobotStudio.

En la figura 8 se muestra la pantalla del FlexPendant en la cual se selecciona la
posicin en la cual se realizara picking, como se menciono anteriormente esta
posicin esta entre 1 y 6.

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

Figura 8: Interfaz en el FlexPendant para seleccionar posicin de picking.

1
2
3

MODULE Module1
PERS tooldata Gripper :=[ TRUE ,[[0 ,0 ,115] ,[0.707106781 ,0 ,0 , -0.707106781]] ,[1 ,[0 ,0 ,1] ,[1 ,0 ,

PERS wobjdata Workobject_1 :=[ FALSE , TRUE ,"" ,[[750 ,450 ,100] ,[1 ,0 ,0 ,0]] ,[[0 ,0 ,0] ,[1 ,0 ,0 ,0]]];-

PERS wobjdata Workobject_2 :=[ FALSE , TRUE ,"" ,[[200 , -650 ,150] ,[1 ,0 ,0 ,0]] ,[[0 ,0 ,0] ,[1 ,0 ,0 ,0]]];-

CONST robtarget Aprox11:=[[ -100 , -122 ,30] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget Pick11:=[[50 , -122 ,30] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget Aprox12:=[[ -100 , -366 ,30] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget Pick12:=[[50 , -366 ,30] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget Aprox21:=[[ -100 , -122 ,170] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget Pick21:=[[50 , -122 ,170] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget Aprox22-

10

11

12

Informe de proyecto final de Robtica 2015-II

13

14

15

16

17

18

19

20

21

22

23

24

25
26
27
28
29
30
31

Proyecto final de robtica

:=[[ -100 , -366 ,170] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget Pick22:=[[50 , -366 ,170] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget Aprox31:=[[ -100 , -122 ,300] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget Pick31:=[[50 , -122 ,300] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget Aprox32:=[[ -100 , -366 ,300] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget Pick32:=[[50 , -366 ,300] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget AproxCua:=[[0 ,0 ,100] ,[0 ,0 , -0.707106781 ,0.707106781] ,[ -1 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget PlaceCua:=[[0 ,0 ,0] ,[0 ,0 , -0.707106781 ,0.707106781] ,[ -1 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget AproxCil:=[[ -150 ,0 ,100] ,[0 ,0 , -0.707106781 ,0.707106781] ,[ -1 ,0 , -1 ,1] ,[
E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget PlaceCil:=[[ -150 ,0 ,0] ,[0 ,0 , -0.707106781 ,0.707106781] ,[ -1 ,0 , -1 ,1] ,[9
E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget AproxEsf:=[[ -300 ,0 ,100] ,[0 ,0 , -0.707106781 ,0.707106781] ,[ -2 , -1 ,0 ,1] ,[
E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget PlaceEsf:=[[ -300 ,0 ,0] ,[0 ,0 , -0.707106781 ,0.707106781] ,[ -2 , -1 ,0 ,1] ,[9
E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget Trans:=[[450 , -450 ,400] ,[0.27059805 , -0.27059805 ,0.653281482 , -0.653
E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST jointtarget Home :=[[0 ,0 ,0 ,0 ,45 ,0] ,[9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
VAR NUM posPickCil ;
VAR NUM posPickCua ;
VAR NUM posPickCir ;
VAR NUM state ;
CONST robtarget paso_108

Informe de proyecto final de Robtica 2015-II

32

33

34

35

36

37

38

39

40

41

42

43

44
45
46
47
48
49
50

Proyecto final de robtica

:=[[ -250 , -500 ,400] ,[0.5 , -0.5 ,0.5 , -0.5] ,[ -1 , -1 ,0 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget RePick11:=[[50 , -122 ,50] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget Reprox11:=[[ -100 , -122 ,50] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget RePick12:=[[50 , -366 ,50] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget Reprox12:=[[ -100 , -366 ,50] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget RePick21:=[[50 , -122 ,190] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget Reprox21:=[[ -100 , -122 ,190] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget RePick22:=[[50 , -366 ,190] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget Reprox22:=[[ -100 , -366 ,190] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget RePick31:=[[50 , -122 ,320] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget Reprox31:=[[ -100 , -122 ,330] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget RePick32:=[[50 , -366 ,320] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
CONST robtarget Reprox32:=[[ -100 , -366 ,320] ,[0.5 , -0.5 ,0.5 , -0.5] ,[0 ,0 , -1 ,1] ,[9E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ,9 E9 ]];
PROC Main ()
MoveAbsJ Home , v1000 , z100 , Gripper \ WObj := wobj0;
WaitTime 2;
SetDO DO10_6 ,0;
WaitTime 2;
state := 0;
9

Informe de proyecto final de Robtica 2015-II

51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86

Proyecto final de robtica

picking ;
ENDPROC
PROC Pick1 ()
WaitTime 2;
SetDO DO10_6 ,0;
WaitTime 2;
MoveJ Aprox11 , v1000 , z10 , Gripper \ WObj :=Workobject_1 ;
MoveL Pick11 , v200 , z10 , Gripper \ WObj :=Workobject_1 ;
WaitTime 2;
SetDO DO10_6 ,1;
WaitTime 2;
MoveL RePick11 , v200 , z10 , Gripper \ WObj :=Workobject_1 ;
MoveL Reprox11 , v200 , z10 , Gripper \ WObj :=Workobject_1 ;
ENDPROC
PROC Pick2 ()
WaitTime 2;
SetDO DO10_6 ,0;
WaitTime 2;
MoveJ Aprox12 , v1000 , z10 , Gripper \ WObj :=Workobject_1 ;
MoveL Pick12 , v200 , z10 , Gripper \ WObj :=Workobject_1 ;
WaitTime 2;
SetDO DO10_6 ,1;
WaitTime 2;
MoveL RePick12 , v200 , z10 , Gripper \ WObj :=Workobject_1 ;
MoveL Reprox12 , v200 , z10 , Gripper \ WObj :=Workobject_1 ;
ENDPROC
PROC Pick3 ()
WaitTime 2;
SetDO DO10_6 ,0;
WaitTime 2;
MoveJ Aprox21 , v1000 , z10 , Gripper \ WObj :=Workobject_1 ;
MoveL Pick21 , v200 , z10 , Gripper \ WObj :=Workobject_1 ;
WaitTime 2;
SetDO DO10_6 ,1;
WaitTime 2;
10

Informe de proyecto final de Robtica 2015-II

87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120

Proyecto final de robtica

MoveL RePick21 , v200 , z10 , Gripper \ WObj :=Workobject_1 ;

MoveL Reprox21 , v200 , z10 , Gripper \ WObj :=Workobject_1 ;
ENDPROC
PROC Pick4 ()
WaitTime 2;
SetDO DO10_6 ,0;
WaitTime 2;
MoveJ Aprox22 , v1000 , z10 , Gripper \ WObj :=Workobject_1 ;
MoveL Pick22 , v200 , z10 , Gripper \ WObj :=Workobject_1 ;
WaitTime 2;
SetDO DO10_6 ,1;
WaitTime 2;
MoveL RePick22 , v200 , z10 , Gripper \ WObj :=Workobject_1 ;
MoveL Reprox22 , v200 , z10 , Gripper \ WObj :=Workobject_1 ;
ENDPROC
PROC Pick5 ()
WaitTime 2;
SetDO DO10_6 ,0;
WaitTime 2;
MoveJ Aprox31 , v1000 , z10 , Gripper \ WObj :=Workobject_1 ;
MoveL Pick31 , v200 , z10 , Gripper \ WObj :=Workobject_1 ;
WaitTime 2;
SetDO DO10_6 ,1;
WaitTime 2;
MoveL RePick31 , v200 , z10 , Gripper \ WObj :=Workobject_1 ;
MoveL Reprox31 , v200 , z10 , Gripper \ WObj :=Workobject_1 ;
ENDPROC
PROC Pick6 ()
WaitTime 2;
SetDO DO10_6 ,0;
WaitTime 2;
MoveJ Aprox32 , v1000 , z10 , Gripper \ WObj :=Workobject_1 ;
MoveL Pick32 , v200 , z10 , Gripper \ WObj :=Workobject_1 ;
WaitTime 2;
11

Informe de proyecto final de Robtica 2015-II

121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154

Proyecto final de robtica

SetDO DO10_6 ,1;

WaitTime 2;
MoveL RePick32 , v200 , z10 , Gripper \ WObj :=Workobject_1 ;
MoveL Reprox32 , v200 , z10 , Gripper \ WObj :=Workobject_1 ;
ENDPROC
PROC Place1 ()
MoveJ AproxCua , v1000 , z10 , Gripper \ WObj :=Workobject_2 ;
MoveL PlaceCua , v200 , z10 , Gripper \ WObj :=Workobject_2 ;
WaitTime 2;
SetDO DO10_6 ,0;
WaitTime 2;
MoveL AproxCua , v200 , z10 , Gripper \ WObj :=Workobject_2 ;
ENDPROC
PROC Place2 ()
MoveJ AproxCil , v1000 , z10 , Gripper \ WObj :=Workobject_2 ;
MoveL PlaceCil , v200 , z10 , Gripper \ WObj :=Workobject_2 ;
WaitTime 2;
SetDO DO10_6 ,0;
WaitTime 2;
MoveL AproxCil , v200 , z10 , Gripper \ WObj :=Workobject_2 ;
ENDPROC
PROC Place3 ()
MoveJ AproxEsf , v1000 , z10 , Gripper \ WObj :=Workobject_2 ;
MoveL PlaceEsf , v200 , z10 , Gripper \ WObj :=Workobject_2 ;
WaitTime 2;
SetDO DO10_6 ,0;
WaitTime 2;
MoveL AproxEsf , v200 , z10 , Gripper \ WObj :=Workobject_2 ;
ENDPROC
PROC transision ()
MoveJ paso_10 , v1000 , z10 , Gripper \ WObj :=Workobject_1 ;
ENDPROC
PROC picking ()
12

Informe de proyecto final de Robtica 2015-II

155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197

Proyecto final de robtica

transision ;
IF state =0 THEN
TPReadNum posPickCil ," Ingrese la posicion del cubo ";
ELSEIF state =1 THEN
TPReadNum posPickCil ," Ingrese la posicion del cilindro ";
ELSEIF state =2 THEN
TPReadNum posPickCil ," Ingrese la posicion dela esfera ";
ENDIF
IF posPickCil =1 THEN
Pick1 ;
ELSEIF posPickCil =2 THEN
Pick2 ;
ELSEIF posPickCil =3 THEN
Pick3 ;
ELSEIF posPickCil =4 THEN
Pick4 ;
ELSEIF posPickCil =5 THEN
Pick5 ;
ELSEIF posPickCil =6 THEN
Pick6 ;
ENDIF
placing ;
ENDPROC
PROC placing ()
transision ;
IF state =0 THEN
Place1 ;
ELSEIF state =1 THEN
Place2 ;
ELSEIF state =2 THEN
Place3 ;
ENDIF
state := state +1;
IF state <3 THEN
picking ;
ELSE
TPWrite " Fin Del Programa ";
MoveAbsJ Home , v1000 , z100 , Gripper \ WObj := wobj013

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

;
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227

ENDIF
ENDPROC
PROC inicio ()
MoveJ Trans , v1000 , z10 , Gripper \ WObj := wobj0 ;
ENDPROC
PROC PruebaPicking ()
transision ;
Pick1 ;
transision ;
Pick2 ;
transision ;
Pick3 ;
transision ;
Pick4 ;
transision ;
Pick5 ;
transision ;
Pick6 ;
transision ;
ENDPROC
PROC PruebaPlacing ()
transision ;
Place1 ;
transision ;
Place2 ;
transision ;
Place3 ;
transision ;
ENDPROC
ENDMODULE

e) Diagrama de movimientos
Cada rutina de pick llamada pickn(), donde n es un nmero del 1 al 6 consta
de su respectiva posicin de aproximacin, posicin de agarre con el gripper
abierto, comando para cerrar el gripper, elevar la pieza r regresar a la posicin
de aproximacin. Cada rutina de place por otro lado, llamadas placen(), donde
n es un nmero del 1 al 3 consta de un movimiento de acercamiento, un movimiento hasta la posicin del place, apertura del gripper y retorno a la posicin
de acercamient.
Una rutina llamada transition(), es un movimiento rpido a una posicin intermedia entre las posiciones de pick y place. La seleccin de la posicin en la cual
se va a realizar el pick se encuentra en la rutina picking(), al inicio de la rutina
pregunta por un valor en pantalla la cual ser la posicin en la cual recoger la
14

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

pieza, guarda este valor en la variable posPickCil, compara este valor usando
el comando if, para saber en qu posicin debe recoger la pieza.y finalmente la
rutina tiene un contador el cual le permite realizar la operacin nicamente 3
veces.
f ) Video de funcionamiento
https://www.dropbox.com/s/s9zfvxtn1c96bt1/video
g) Observaciones de seguridad
Es importante tener en cuenta que el robot no tiene sensores para detectar si se
colisiona con algn objeto, o si algn usuario ingresa en el campo de trabajo del
robot no hay ningn sensor que alerte al robot, por lo cual es importante tener
sistemas de seguridad en el entorno de trabajo, evitando as posibles accidentes
2. Desarrollo de control para robots En esta seccin se procedi a realizar la interfaz grfica que permite controlar el robot mediante un gamepad tanto en cinemtica
directa como inversa y a su vez implementar una rutina de pick & place.
a) Integracin de controlador de juegos (Gamepad).
Con el fin de realizar la operacion del robot mediante un controlador de juegos o Gamepad, se implement una aplicacin de MATLAB enlazada con la
librera open-source SDL 2 que permite detectar y manipular el estado de diferentes dispositivos seriales. El programa utlizado es basado en JoyMEX 2,
un programa que enlaza una rutina escrita en lenguaje C con un programa
MATLAB. Se realizaron modificaciones sobre ese desarrollo como la versin de
SDL que utiliza por defecto (mejorando a SDL 2.0) para evitar problemas de
compatibilidad con sistemas de 64 bits.
El cdigo del programa para la deteccin e implementacin del controlador
de juegos se distribuye en varios archivos: Un archivo MEX de MATLAB que
permite enlazar las libreras dinmicas, ficheros en lenguaje C y cdigo en
MATLAB para poder leer los estados del dispositivo serial a travs de la rutina
central de la GUI que gobierna otras funciones como las del clculo de la cinmatica y los parmetros para los servomotores Dynamixel del robot Pincher.
Para poder ejecutar el archivo buildde JoyMEX 2 se debe contar con un
compilador C como Windows SDK, si se omite este proceso, no podrn enlazarse los archivos de configuracin en lenguaje C, las libreras de SDL y el
entorno de MATLAB. A continuacin se presenta el cdigo utilizado para la
implementacin en cdigo C (joymex2.c), el cual corresponde a la implementacin realizada por Escabe - Development Projects modificada para usar SDL
2:
15

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

1 /*
2
Copyright 2011 Martijn Aben
3
4
Licensed under the Apache License , Version 2.0 ( the "License ") ;
5
you may not use this file except in compliance with the License .
6
You may obtain a copy of the License at
7
8
http :// www . apache . org / licenses / LICENSE -2.0
9
10
Unless required by applicable law or agreed to in writing , software
11
distributed under the License is distributed on an "AS IS " BASIS ,
12
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND , either express or implied .
13
See the License for the specific language governing permissions and
14
limitations under the License .
15 */
16
17 # include " mex . h "
18 # include < SDL / SDL .h >
19 # include < SDL / SDL_haptic .h >
20 # include < SDL / SDL_joystick .h >
21
22 SDL_Joystick ** joy = NULL ;
23
24 int * naxes = NULL ;
25 int * nbuttons = NULL ;
26 int * nhats = NULL ;
27 int njoysticks =0;
28
29 /* Elimina el objeto creado para los joystick que se conecten */
30 static void onUnload () {
31
int i ;
32
for ( i =0; i < njoysticks ; i ++) {
33
if ( joy [ i ]) {
34
SDL_ Joysti ckClos e ( joy [ i ]) ;
35
mexPrintf (" Unloaded Joystick %d \ n " , i ) ;
36
}
37
}
38
if ( joy )
16

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

39
free ( joy ) ;
40
if ( nbuttons )
41
free ( nbuttons ) ;
42
if ( naxes )
43
free ( naxes ) ;
44
if ( nhats )
45
free ( nhats ) ;
46
47 }
48
49 void vibrate () {
50
SDL_Haptic * haptic ;
51
SDL_HapticEffect effect ;
52
int effect_id ;
53
54
55
56 }
57
58 % Inicializacion de joystick , se guarda informacion del nmero de ejes y botones del dispositivo
59 void Init ( int id ) {
60
/* Check if ID is valid at all */
61
if ( id >= njoysticks ) {
62
mexErrMsgTxt (" Joystick with specified id does not exist .") ;
63
}
64
/* Open the joystick */
65
joy [ id ]= SDL_JoystickOpen ( id ) ;
66
67
if ( joy [ id ]) /* If succesfull */
68
{
69
mexPrintf (" Opened Joystick %d \ n " , id ) ;
70
/* Query information about number of axes and buttons */
71
naxes [ id ] = S D L _J o y st ickNumAxes ( joy [ id ]) ;
72
nbuttons [ id ] = S D L _ J o y st ick Nu mB ut ton s ( joy [ id ]) ;
73
nhats [ id ] = S D L _J o y st ickNumHats ( joy [ id ]) ;
74
75
mexPrintf (" Name : %s \ n " , S D L _ Jo y s t ic k N a me F o r In d e x ( id ) ) ;
76
mexPrintf (" Number of Axes : %d \ n " , naxes [ id ]) ;
77
mexPrintf (" Number of Buttons : %d \ n " , nbuttons [ id]) ;
78
mexPrintf (" Number of Hats : %d \ n " , nhats [ id ]) ;
79
17

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

80
}
81
else {
82
mexErrMsgTxt (" Couldn t open Joystick ") ;
83
}
84 }
85
86 /* Leer estado del joystick */
87 mxArray * Query ( int id ) {
88
mxArray * out ;
89
mxArray * d ;
90
short * dd ;
91
unsigned char * db ;
92
mwSize dims [1];
93
int nfields ;
94
const char * fields [] = {" axes " ," buttons " ," hats "};
95
const char * hatfields [] = {" up " ," right " ," down " ," left"};
96
int i ;
97
short hat ;
98
99
/* Make sure ID is valid at all */
100
if ( id >= njoysticks ) {
101
mexErrMsgTxt (" Joystick with specified id does not exist .") ;
102
}
103
/* Make sure joystick is initialized */
104
if (! joy [ id ]) {
105
mexErrMsgTxt (" Joystick not initialized ") ;
106
}
107
/* Read registers from Joystick */
108
SD L_ Jo ys ti ck Up da te () ;
109
110
/* Create an output struct */
111
dims [0] = 1;
112
out = m x C re a t eS t r uc t A r ra y (1 , dims ,3 , fields ) ;
113
114
/* Initialize output array for axes information */
115
d = m x C r e a t e N u m e r i c M a t r i x (1 , naxes [ id ] , mxINT16_CLASS ,mxREAL ) ;
116
dd = ( short *) mxGetData ( d ) ;
117
/* For each axes read the state */
118
for ( i =0; i < naxes [ id ]; i ++) {
119
* dd ++= S D L_ Jo ys t i ck G e tAxis ( joy [ id ] , i ) ;
120
}
121
mxSetField ( out ,0 ," axes " , d ) ;
122
18

Informe de proyecto final de Robtica 2015-II

123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139

Proyecto final de robtica

/* Do the same for all buttons */

d = m x C r e a t e N u m e r i c M a t r i x (1 , nbuttons [ id ] ,mxUINT8_CLASS , mxREAL ) ;
db = ( unsigned char *) mxGetData ( d ) ;
for ( i =0; i < nbuttons [ id ]; i ++) {
* db ++ = S D L _ J o y s t i c k G e tBu tton ( joy [ id ] , i ) ;
}
mxSetField ( out ,0 ," buttons " , d ) ;
/* Do the same for all hats */
dims [0] = nhats [ id ];
d = m xC r e a te S t ru c t Ar r a y (1 , dims ,4 , hatfields ) ;
for ( i =0; i < nhats [ id ]; i ++) {
hat = S DL _Joy st ic kGe tHat ( joy [ id ] , i ) ;
mxSetField (d ,i ," up " , m x Cr eateL ogic alSc alar ( ( hat & SDL_HAT_UP ) != 0 ) ) ;
mxSetField (d ,i ," right " , mxCr eateL ogic alSc alar ( (hat & SDL_HAT_RIGHT ) != 0 ) ) ;
mxSetField (d ,i ," down " , mxCr eateL ogic alSc alar ( (hat & SDL_HAT_DOWN ) != 0 ) ) ;
mxSetField (d ,i ," left " , mxCr eateL ogic alSc alar ( (hat & SDL_HAT_LEFT ) !=0 ) ) ;
}
mxSetField ( out ,0 ," hats " , d ) ;

140
141
142
143
return out ;
144 }
145
146 /* Putno de entrada a la MEX - function */
147 void mexFunction ( int nlhs , mxArray * plhs [] , int nrhs , const mxArray * prhs []) {
148
char * action ;
149
/* Check inputs */
150
if ( nrhs ==0)
151
mexErrMsgTxt (" Too few input parameters .") ;
152
/* Initialize arrays if called for first time */
153
if ( joy == NULL ) {
154
SDL_I nitSubSystem ( SDL_INIT_JOYSTICK ) ;
155
njoysticks = SDL_NumJoysticks () ;
156
joy = ( SDL_Joystick **) calloc ( njoysticks , sizeof( SDL_Joystick *) ) ;
157
naxes = ( int *) malloc ( njoysticks * sizeof ( int ) ) ;
158
nbuttons = ( int *) malloc ( njoysticks * sizeof ( int));
159
nhats = ( int *) malloc ( njoysticks * sizeof ( int ) ) ;
160
}
19

Informe de proyecto final de Robtica 2015-II

161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183 }

Proyecto final de robtica

/* Determine what user is requesting */

action = mxArrayToString ( prhs [0]) ;
switch ( action [0]) {
case o : /* open */
if ( nrhs !=2) mexErrMsgTxt (" You need to specify device ID .") ;
Init (* mxGetPr ( prhs [1]) ) ;
break ;
case c : /* close */
onUnload () ;
break ;
case v : /* close */
vibrate () ;
break ;
case q : /* query */
if ( nrhs !=2) mexErrMsgTxt (" You need to specify device ID .") ;
plhs [0] = Query (* mxGetPr ( prhs [1]) ) ;
break ;
default :
mexErrMsgTxt (" No valid action specified ") ;
}
/* Make sure devices are released when MEX - file is cleared */
mexAtExit ( onUnload ) ;

La versin 2.0 de la librera SDL se descarg de la pgina oficial del proyecto y

se realizaron cambios en el cdigo de JoyMEX2 para poder implementarla. El
archivo build.m utilizado para generar el MEX-file modificado para permitir el
uso de SDL 2.0 es el siguiente:
1 if isunix % Construr en Linux
2
mex ( -v , src / joymex2 . c , - lSDL2 ) ;
3 else % Construr en Windows
4
mex ( -v , src / joymex2 . c , - ISDL / include ,[ - LSDL / bin/ char ( win32 ) ] , - lSDL2 ) ;
5
copyfile ( SDL / bin / win32 / SDL2 . dll , . ) ;
6 end
7
%
Copyright 2014 The MathWorks , Inc .

Se utiliz un controlador marca Genius, modelo Maxfire Blaze 3 con conexin

USB compatible con Windows 8.1. En la Figura 9 se puede apreciar la distribucin de las diferentes funciones del programa de operacin del robot en los
botones del Gamepad. En el Cuadro 2 se puede apreciar la tabla de funciones
y botones asignadas al controlador.
20

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

Cuadro 2: Tabla de funciones en los botones del controlador

Nmero de botn
Funcin
1
Modo Cinemtica Directa
2
Ninguna
3
Mover a Home
4
Modo Cinemtica Inversa
5
Rutina Pick and Place
6 - Vertical
Motor 4 (Cin. Inv.)/ (Cin. Dir.)
6 - Horizontal
Motor 3 (Cin. Inv.)/ Z (Cin. Dir.)
7 - Vertical
Motor 2 (Cin. Inv.)/ Y (Cin. Dir.)
7 - Horizontal
Motor 1 (Cin. Inv.)/ X (Cin. Dir.)
8
Apagar Motores
9
Encender Motores
10
Abrir Gripper
11
Cerrar Gripper

Figura 9: Distribucin de funciones en el controlador de juegos.

21

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

b) Diseo de la interfaz grfica. Se dise la interfaz grfica que se puede

visualizar en la figura 10 mediante la herramienta guide de Matlab.Mediante
esta interfaz, el usuario puede ingresar y modificar valores como el torque
mximo, la velocidad mxima (la velocidad mnima es siempre cero), el nmero
de puerto de la conexin entre otros parmetros y navegar entre las opciones.
Los componentes de la interfaz se explican a continuacin:

Figura 10: Interfaz grfica del programa.

1. Se ingresa el nmero de puerto en donde se encuentra conectado el Robot
Pincher, en caso de que no se establezca comunicacin, surgir un mensaje
de error cuando se intente ejecutar algn movimiento como se muestra en
la figura 11. A su vez el botn terminar permite que en el momento cuando
se termine el trabajo con el robot, terminar la conexin para esta manera
22

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

liberar el puerto y poder usar el robot en otros programas como Roboplus.

Figura 11: Error de comunicacin

2. Permite Encender o Apagar el robot Pincher o mostrar el estado del robot
cuando se opera mediante el gamepad.
3. Permite abrir o cerrar el Gripper o mostrar el estado del mismo cuando se
opera mediante el gamepad.
4. Permite introducir el valor en porcentaje del Torque mximo para cada
uno de los motores as como el valor en porcentaje de la velocidad mxima
para cada uno de los motores.
5. En este recuadro se pueden visualizar los valores de las articulaciones como
de las coordenadas x, y, z y actuales del robot.
6. Se puede visualizar el desplazamiento del robot, este funciona en tiempo
real, leyendo la posicin actual de cada servomotor para graficar la configuracin del mismo.
7. En este recuadro se puede activar el modo manual, en donde a travs del
gamepad se puede seleccionar los distintos modos de control del robot, es
decir cinemtica directa, cinemtica inversa o el modo de ejecucin de rutina de pick and place.
Si en algn momento, controlando el robot de manera manual con el gamepad se llega a enviar una coordenada o desplazamiento que se encuentre
fuera del espacio de trabajo del robot, aparecer un aviso de error en la
pantalla como se muestra en la figura 12.
8. Permite ejecutar la rutina predefinida de prueba del espacio de trabajo
como a su vez desplazar el robot a la posicin de Home.
c) Limitar Torque
Con el fin de limitar el torque de cada uno de los servomotores se procedi a
crear una funcin en Matlab que permitiera restringir cada uno de una manera
sencilla. Este valor es ledo de la interfaz grfica, despus de ser ingresado por
el usuario.

23

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

Figura 12: Aviso de Error cuando se trata de desplazar el robot fuera del espacio de trabajo
El cdigo desarrollado se encuentra escrito a continuacin:
1
2
3
4
5
6
7
8
9

function limitar_torque (t , ID )
%Valores de torque en porcentaje del torque mximo
calllib ( dynamixel , dxl_write_word ,ID ,34 , t *1023/100) ;
motor = ID
T_max = t
%

Copyright 2014 The MathWorks , Inc .

Esta funcin recibe el torque asignado al motor a manera de porcentaje para

despus modificar el valor del registro de lmite de torque del motor con el ID
determinado.
d) Limitar Velocidad
24

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

Con el fin de poder limitar la velocidad mxima de cada uno de los servomotores, se procedi a realizar una funcin en Matlab que recibe el id del motor
y el valor porcentual de la velocidad deseada. Posterior a esto se procede a
modificar el valor del registro dentro del microcontrolador del servomotor correspondiente a dicho parmetro. El valor porcentual de la velocidad mxima
es leda de la interfaz grfica y este es ingresado por el usuario.
La funcin en Matlab es la siguiente:
1
2
3
4
5
6
7

function limitar_vel (v , ID )
%Valores de torque en porcentaje del torque mximo
calllib ( dynamixel , dxl_write_word ,ID ,32 , v *1023/100) ;
motor = ID
V_max = v
%

Copyright 2014 The MathWorks , Inc .

e) Apagar y Encender Motores

Para prender y apagar los motores al presionar los botones de la GUI se crea
una funcin en Matlab que modifique el registro correspondiente en los motores,
esta funcin se muestra a continuacin:
1 function encender_motores ( flag )
2 %Si flag =1 habilita el par , si flag =0 deshabilita el par
3
calllib ( dynamixel , dxl_write_word ,1 ,24 , flag ) ;
4
calllib ( dynamixel , dxl_write_word ,2 ,24 , flag ) ;
5
calllib ( dynamixel , dxl_write_word ,3 ,24 , flag ) ;
6
calllib ( dynamixel , dxl_write_word ,4 ,24 , flag ) ;
7
calllib ( dynamixel , dxl_write_word ,5 ,24 , flag ) ;
8 a = flag
9
%
Copyright 2014 The MathWorks , Inc .

El nombre del registro que se modifica es Habilitacin del par y este deshabilita
el torque de los motores cuando tiene el valor cero y lo habilita cuando tiene
el valor 1.
En el cdigo anterior se puede ver que la funcin recibe un flag el cual si corresponde a prender motores es igual a 1 o si es para apagarlos es igual a 0.
f ) Configuracin de puerto y configuracin inicial
Al iniciar el programa el usuario debe ingresar el valor del puerto en la interfaz
grfica, cuyo nmero es asignado por la computadora al conectar el adaptador
uSB2Dynamixel.
Posteriormente se procede a evaluar si hay comunicacin con el robot, en el
caso que no lo sea en la interfaz grfica se muestra una ventana informando de
este error.

25

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

Si es posible entablar la comunicacin se procede a realizar una limitacin de

torques inicial, y luego se procede a mostrar los valores iniciales de torques y
velocidades en la interfaz grfica.
El cdigo que se ejecuta en Matlab al configurar el puerto es el siguiente:
1 function edit1_Callback ( hObject , eventdata , handles )
2 assignin ( base , COM , str2num ( get ( hObject , String ) ) ) ;
3 COM = evalin ( base , COM ) ;
4 % % %======== Iniciar Comunicacion ===============
5 if str2num ( get ( hObject , String ) ) >0 && str2num ( get (hObject , String ) ) <10
6
loadlibrary ( dynamixel , dynamixel . h ) ;
7
libfunctions ( dynamixel ) ;
8
BAUDNUM = 1;
9
Prueba_Coneccion = calllib ( dynamixel , dxl_initialize ,COM , BAUDNUM ) ;
10
assignin ( base , comunicacion , Prueba_Coneccion ) ;
11
12
%Se hace una limitacin inicial de torque
13
limitar_torque ( evalin ( base , torMaxMotor1 ) ,1) ;
14
limitar_torque ( evalin ( base , torMaxMotor2 ) ,2) ;
15
limitar_torque ( evalin ( base , torMaxMotor3 ) ,3) ;
16
limitar_torque ( evalin ( base , torMaxMotor4 ) ,4) ;
17
limitar_torque ( evalin ( base , torMaxMotor5 ) ,5) ;
18
19
%Se muestra en la GUI los torques y velocidades
20
set ( handles . edit2 , String , evalin ( base , torMaxMotor1 ) ) ;
21
set ( handles . edit4 , String , evalin ( base , torMaxMotor2 ) ) ;
22
set ( handles . edit6 , String , evalin ( base , torMaxMotor3 ) ) ;
23
set ( handles . edit8 , String , evalin ( base , torMaxMotor4 ) ) ;
24
25
set ( handles . edit3 , String , evalin ( base , velMaxMotor1 ) ) ;
26
set ( handles . edit5 , String , evalin ( base , velMaxMotor2 ) ) ;
27
set ( handles . edit7 , String , evalin ( base , velMaxMotor3 ) ) ;
28
set ( handles . edit9 , String , evalin ( base , velMaxMotor4 ) ) ;
29
30
31
26

Informe de proyecto final de Robtica 2015-II

32
33

Proyecto final de robtica

%Se lee la posicion inicial y se permanece en ella

[ pos_in1 , pos_in2 , pos_in3 , pos_in4 ] = leer_posicion;
pos_in = [ pos_in1 , pos_in2 , pos_in3 , pos_in4 ];
for i =1:4
calllib ( dynamixel , dxl_write_word ,i ,30 ,write_angle ( pos_in ( i ) , pos_in ( i ) ,i ) ) ;
end

34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50

%Encender motores por defecto ;

assignin ( base , encendido ,1) ;
set ( handles . radiobutton2 , Value ,0) ;
set ( handles . radiobutton1 , Value ,1) ;
encender_motores (1) ;
%Abrir gripper por defecto
set ( handles . radiobutton12 , Value ,1) ;
set ( handles . radiobutton13 , Value ,0) ;
gripper (1) ;

51
52
53
54
55 end
56

%se guardan los valores de posicin inicial en las variables globales

assignin ( base , q1 , pos_in (1) ) ;
%assignin ( base , q2 , pos_in (2) ) ;
%assignin ( base , q3 , pos_in (3) ) ;
assignin ( base , q4 , pos_in (4) ) ;
%

Copyright 2014 The MathWorks , Inc .

g) Cinemtica Directa
Para realizar el movimiento en el modo de cinemtica Directa se leen los valores provenientes del gamepad, los cuales son sumados a los valores actuales de las artriculaciones. Estos valores resultantes son enviados a la funcin
cinematicad ir la cual se encarga de calcular las matrices de transformacin
Homogenea,para posteriormente calcular los valores de las coordenadas x, y,
z y . Estos valores son retornados a la rutina principal, para validar si se
encuentra en el espacio de trabajo definido para el robot, en el caso que no se
encuentre mostrar en la interfaz grfica una ventana avisndole al usuario. Si
las coordenadas si se encuentran en el espacio de trabajo del robot, los valores
de las articulaciones son enviados a cada uno de los motores para generar su
desplazamiento.
El cdigo generado en Matlab para el clculo de la Cinemtica directa es el
siguiente:

27

Informe de proyecto final de Robtica 2015-II

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

Proyecto final de robtica

function [X ,Y ,Z , Theta ] = cinematica_dir ( q1 , q2 , q3 , q4 )

%Parmetros del Pincher
L1 =13.85;
L2 =10.5;
L3 =10.5;
L4 =9.25;
%Prueba
% q1 =0;
% q2 =0;
% q3 =0;
% q4 =0;
%Clculo de las matrices de transformacin homogenea
T1
T2
T3
T4
T5

=
=
=
=
=

T_homogenea
T_homogenea
T_homogenea
T_homogenea
T_homogenea

( q1 , L1 , 0 , 0 ) ;
( q2 , 0 , 0 , pi /2 ) ;
( q3 , 0 , L2 , 0 ) ;
( q4 + pi /2 , 0 , L3 , 0 ) ;
(0 , L4 , 0 , pi /2 ) ;

T = T1 * T2 * T3 * T4 * T5 ;
%Clculo de posicin y orientacin
X = T (1 ,4) ;
Y = T (2 ,4) ;
Z = T (3 ,4) ;
Theta = q2 + q3 + q4 ;

Copyright 2014 The MathWorks , Inc .

A travs de la funcin moverC inD irecta es la funcin que se encarga de enviarle

los nuevos valores de q a cada uno de los motores y proceder a realizar la grfica
del robot por medio del diagrama de cables con estas posiciones. El cdigo de
esta funcin generado en Matlab es el siguiente:
1
2
3
4
5
6

function mover_cinDirecta ( Q )
L1 =13.85;
L2 =10.5;
L3 =10.5;
L4 =9.25;
%Lee la posicin en caso de que se incumpla una restriccin de ngulo para
7 %mantenerla
8 [ pos_in1 , pos_in2 , pos_in3 , pos_in4 ] = leer_posicion () ;
28

Informe de proyecto final de Robtica 2015-II

9
10
11
12

pos_in = [ pos_in1 , pos_in2 , pos_in3 , pos_in4 ];

for j =1:4 %movimiento en cada articulacin
calllib ( dynamixel , dxl_write_word ,j ,30 ,write_angle ( Q ( j ) , pos_in ( j ) ,j ) ) ;
end

13
14
15
16
17
18
19
20
21

%Se pasa la posicin actual a radianes

q1 =( Q (1) -150) * pi /180;
q2 =( Q (2) -60) * pi /180;
q3 =( Q (3) -150) * pi /180;
q4 =( Q (4) -150) * pi /180;
%Se hallan los puntos que definen cada una de las articulaciones para
%dibujarla
p0 = [0 , 0 , L1 ];
p1 = [ L2 * cos ( q2 ) * cos ( q1 ) , L2 * cos ( q2 ) * sin ( q1 ) , L2 * sin( q2 ) ];
p2 = [ L3 * cos ( q2 + q3 ) * cos ( q1 ) , L3 * cos ( q2 + q3 ) * sin ( q1 ) , L3 * sin ( q2 + q3 ) ];
p3 = [ L4 * cos ( q2 + q3 + q4 ) * cos ( q1 ) , L4 * cos ( q2 + q3 + q4 ) * sin (q1 ) , L4 * sin ( q2 + q3 + q4 ) ];

22
23
24
25
26
27
28
29
30
31
32
33
34

35

Proyecto final de robtica

%Se grafica la configuracin del robot

plot3 ([0 p0 (1) ] , [0 p0 (2) ] , [0 p0 (3) ] , -b * , linewidth ,2)
grid on
axis ([0 30 -30 30 0 40])
hold on
plot3 ([ p0 (1) p0 (1) + p1 (1) ] ,[ p0 (2) p0 (2) + p1 (2) ] , [ p0(3) p0 (3) + p1 (3) ] , -r * , linewidth ,2) ;
plot3 ([ p0 (1) + p1 (1) p0 (1) + p1 (1) + p2 (1) ] ,[ p0 (2) + p1 (2) p0 (2) + p1 (2) + p2 (2) ] ,[ p0 (3) + p1 (3) p0 (3) + p1 (3) + p2 (3)] , -g * , linewidth ,2) ;
plot3 ([ p0 (1) + p1 (1) + p2 (1) p0 (1) + p1 (1) + p2 (1) + p3 (1) ] ,[p0 (2) + p1 (2) + p2 (2) p0 (2) + p1 (2) + p2 (2) + p3 (2) ] , [ p0(3) + p1 (3) + p2 (3) p0 (3) + p1 (3) + p2 (3) + p3 (3) ] , -b * , linewidth ,2) ;
hold off
pause (0.01) ; %Espera para llegar al punto

36
37
38
39 end
40
41
42

Copyright 2014 The MathWorks , Inc .

29

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

h) Cinemtica Inversa
El clculo de la cinemtica inversa se realiza por medio del mtodo geomtrico.
Para ello partiremos del diagrama descrito en la figura 13.

Figura 13: Diagrama utilizado para el clculo q1 de la Cinemtica Inversa.

Debido a que q1 se encuentra sobre el plano xy, es decir que su rotacin la hace
a lo largo del eje z. Tenemos que:
y
q1 = arctan( )
x
Para el clculo de q2 ,q3 y q4 partiremos de la figura14, donde:

Figura 14: Diagrama utilizado para el clculo q2 ,q3 y q4 de la Cinemtica Inversa.

30

(1)

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

(x2 + y 2 )

x1 =

(2)

A su vez definimos que :

x2 = x1 L4 cos()

(3)

z1 = L4 sin() L1

(4)

Con esto procedemos a la definicin de las siguientes ecuaciones:

R2 = x22 + z12

(5)

R2 = l22 + l3 + 2l2 l3 cos(q3 )

(6)

Por teorema del coseno:

Para hallar q3, se despeja coseno:

R2 L22 L32
cos(q3 ) =
2L2L32

(7)

Adems se tiene que (sin(q3 ) = 1 cos2 (q3 )) (Considerando valores positivos

y negativos de la raiz). Por lo que:
(

sin(q3 )
q3 = arctan
cos(q3 )

(8)

Por otro lado, se tiene que:

= arctan(

z1
)
x2

(9)

l2 + l3 cos(q3 )
R
l3 sin(q3 )
sin() =
R

cos() =

(10)
(11)

Entonces:
(

l3 sin(q3 )
= arctan
l2 + l3 cos(q3 )

(12)

y Para hallar q2 :
q2 =
31

(13)

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

Ya con q1 , q2 , y q3 calculados, procedemos a calcular q4 en donde:

= q2 + q3 + q4

(14)

q4 = q2 q3

(15)

Por lo tanto:
Dado el orden cuadrtico de la ecuacin 7 y de igual manera a los valores
positivos y negativos que puede tomar el seno de q3 , podemos ver que existen
dos soluciones para la cinemtica inversa del brazo del Pincher (codo arriba y
codo abajo).
Con el fin de evitar colisiones con el tablero o base, siempre se consider tomar
la solucin de codo arriba. Esto se debi a que en el momento de realizar las
pruebas con el cdigo en donde este seleccionaba la solucin mas optima por
medio de la distancia mnima, se obtuvieron varias que estaban fuera de los
intervalos de seguridad de operacin de cada servomotor.
Ya con esto a travs de la interfaz grfica se procedi a leer los datos recibidos
del Gamepad, lo cual permite que se pueda aumentar o disminuir los valores
de x, y, z y . El cdigo elaborado en Matlab es el siguiente:
1
2
3
4
5
6
7
8
9
10
11
12
13

function [ q1 , q2 , q3 , q4 ] = cinematica_inv (x , y , z , Theta , L1 , L2 , L3 , L4 )

%Se halla q1
q1 = atan2 (y , x ) ;
%Para hallar q2 , q3 y q4
x_p = sqrt ( x 2+ y 2) ;
%Posicin de la mueca
x_p1 = x_p - L4 * cos ( Theta ) ;
z1 = z - L4 * sin ( Theta ) - L1 ;

R14 = sqrt ( x_p1 2 + z1 2) ; % comprobacion de espacio de trabajo

14 if ( R14 >( L2 + L3 ) || R14 <( L2 - L3 ) )
15 error ( Coordenadas fuera del espacio de trabajo ) ;
16 else
17
18 % A continuacin se escriben las ecuaciones de cinemtica inversa halladas
19 % anteriormente
20
21 c3 = ( x_p1 2 + z1 2 - L2 2 - L3 2) /(2* L2 * L3 ) ;
22
23 %Se hallan valores q2 , q3 z q4 para solucin codo arriba
32

Informe de proyecto final de Robtica 2015-II

24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46

Proyecto final de robtica

s3 = sqrt (1 - c3 2) ;
q3_U = atan2 ( s3 , c3 ) ;
alpha = atan2 ( z1 , x_p1 ) ;
beta = atan2 ( L3 * sin ( q3_U ) , L2 + L3 * cos ( q3_U ) ) ;
q2_U = alpha - beta ;
q4_U = Theta - q2_U - q3_U ;
%Se hallan valores q2 , q3 z q4 para solucin codo abajo
s3 = - sqrt (1 - c3 2) ;
q3_D = atan2 ( s3 , c3 ) ;
alpha = atan2 ( z1 , x_p1 ) ;
beta = atan2 ( L3 * sin ( q3_D ) , L2 + L3 * cos ( q3_D ) ) ;
q2_D = alpha - beta ;
q4_D = Theta - q2_D - q3_D ;
%Se define la suma de las distancias articulares ponderadas
% Cond1 = abs ( q2_U - q2_A ) + abs ( q3_U - q3_A ) +1* abs ( q4_U - q4_A ) ;
% Cond2 = abs ( q2_D - q2_A ) + abs ( q3_D - q3_A ) +1* abs ( q4_D - q4_A ) ;
%Siempre codo arriba
Cond1 =1;
Cond2 =0;
%Se definen las condiciones para escoger entre codo arriba z codo abajo
if Cond1 < Cond2
q2 = q2_U ;
q3 = q3_U ;
q4 = q4_U ;
else
q2 = q2_D ;
q3 = q3_D ;
q4 = q4_D ;
end

47
48
49
50
51
52
53
54
55
56
57 end
58
59
60

Copyright 2014 The MathWorks , Inc .

Para generar las trayectorias entre dos puntos de manera lineal, se utiliza la
funcin moverl ineal, la cual recibe el punto inicial, el punto final y el numero
de puntos intermedios con los que se desea realizar la trayectoria. Esta funcin
crea un arreglo de puntos los cuales aumentan en x, y, z y de una manera
constante hasta llegar a la posicin y orientacin final deseada. Ya con el arreglo
de puntos se procede a realizar la cinemtica inversa a cada uno para as poder
33

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

enviarle a los motores el nuevo valor de q para as realizar la trayectoria al

igual que se va mostrando la posicin actual del robot a travs de un diagrama
de cables en la Interfaz Grfica. El cdigo de esta funcin en Matlab es el
siguiente:
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
36
37
38

function mover_lineal ( P1 , P2 , n )
L1 =13.85;
L2 =10.5;
L3 =10.5;
L4 =9.25;
%Se crea el conjunto de valores de las coordenadas generalizadas a partir
%de los dos puntos y el nmero de puntos deseado
if P1 (1) == P2 (1)
X = P1 (1) * ones (1 , n ) ;
else
X = P1 (1) :( P2 (1) - P1 (1) ) / n : P2 (1) ; %Conjunto de puntos
end
if P1 (2) == P2 (2)
Y = P1 (2) * ones (1 , n ) ;
else
Y = P1 (2) :( P2 (2) - P1 (2) ) / n : P2 (2) ;
end
if P1 (3) == P2 (3)
Z = P1 (3) * ones (1 , n ) ;
else
Z = P1 (3) :( P2 (3) - P1 (3) ) / n : P2 (3) ;
end
if P1 (4) == P2 (4)
theta = P1 (4) * ones (1 , n ) ;
else
theta = P1 (4) :( P2 (4) - P1 (4) ) / n : P2 (4) ;
end
%Se realiza la cinemtica inversa y se guardan los valores de las
%articulaciones para cada punto en una matriz
Q = zeros (n ,4) ;
for i =1: n
%Se realiza la cinemtica inversa
[ q1 , q2 , q3 , q4 ]= cinematica_inv ( X ( i ) , Y ( i ) , Z ( i ) , theta( i ) ,L1 , L2 , L3 , L4 ) ;
34

Informe de proyecto final de Robtica 2015-II

39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75

Proyecto final de robtica

%Se aplica el offset que tienen los motores dinamixel

%Se acomoda el vector q a los valores iniciales definidos para la posicin
%de home
Q (i ,1) = q1 *180/ pi +150
Q (i ,2) = q2 *180/ pi +60
Q (i ,3) = q3 *180/ pi +150
Q (i ,4) = q4 *180/ pi +150
end
%Movimiento del robot en los n puntos
for i =1: n
[ pos_in1 , pos_in2 , pos_in3 , pos_in4 ] = leer_posicion () ;
pos_in = [ pos_in1 , pos_in2 , pos_in3 , pos_in4 ];
for j =1:4 %movimiento en cada articulacin
calllib ( dynamixel , dxl_write_word ,j ,30 ,write_angle ( Q (i , j ) , pos_in ( j ) ,j ) ) ;
end
%
q1 =( pos_in1 -150) * pi /180;
%
q2 =( pos_in2 -60) * pi /180;
%
q3 =( pos_in3 -150) * pi /180;
%
q4 =( pos_in4 -150) * pi /180;
q1 =( Q (i ,1) -150) * pi /180;
q2 =( Q (i ,2) -60) * pi /180;
q3 =( Q (i ,3) -150) * pi /180;
q4 =( Q (i ,4) -150) * pi /180;

p0 = [0 , 0 , L1 ];
p1 = [ L2 * cos ( q2 ) * cos ( q1 ) , L2 * cos ( q2 ) * sin ( q1 ) ,L2 * sin ( q2 ) ];
p2 = [ L3 * cos ( q2 + q3 ) * cos ( q1 ) , L3 * cos ( q2 + q3 ) * sin( q1 ) , L3 * sin ( q2 + q3 ) ];
p3 = [ L4 * cos ( q2 + q3 + q4 ) * cos ( q1 ) , L4 * cos ( q2 + q3 +q4 ) * sin ( q1 ) , L4 * sin ( q2 + q3 + q4 ) ];

plot3 ([0 p0 (1) ] , [0 p0 (2) ] , [0 p0 (3) ] , -b * , linewidth ,2)

grid on
axis ([0 30 -30 30 0 40])
hold on
plot3 ([ p0 (1) p0 (1) + p1 (1) ] ,[ p0 (2) p0 (2) + p1 (2)] , [ p0 (3) p0 (3) + p1 (3) ] , -r * , linewidth 35

Informe de proyecto final de Robtica 2015-II

,2) ;
plot3 ([ p0 (1) + p1 (1) p0 (1) + p1 (1) + p2 (1) ] ,[ p0 (2)+ p1 (2) p0 (2) + p1 (2) + p2 (2) ] ,[ p0 (3) + p1 (3) p0(3) + p1 (3) + p2 (3) ] , -g * , linewidth ,2) ;
plot3 ([ p0 (1) + p1 (1) + p2 (1) p0 (1) + p1 (1) + p2 (1) +p3 (1) ] ,[ p0 (2) + p1 (2) + p2 (2) p0 (2) + p1 (2) + p2(2) + p3 (2) ] , [ p0 (3) + p1 (3) + p2 (3) p0 (3) + p1(3) + p2 (3) + p3 (3) ] , -b * , linewidth ,2) ;
hold off
pause (0.01) ; %Espera para llegar al punto

76

77

78
79
80
81
82

Proyecto final de robtica

end
%

Copyright 2014 The MathWorks , Inc .

i) Rutina pick and place

Para realizar la rutina de pick and place se definieron 5 puntos los cuales son :
1. Punto de aproximacin al punto inicial.
2. Punto inicial el cual corresponde a la posicin del objeto a agarrar.
3. Punto de aproximacin al punto donde se va a dejar el objeto desplazado
por el robot.
4. Punto de place, el cual corresponde al punto en donde se va a soltar el
objeto.
5. Punto intermedio en donde el robot se va a desplazar despus de haber
dejado el objeto y antes de dirigirse a la posicin home.
Con los puntos ya definidos, se procede a realizar el desplazamiento, para el
cual se definen trayectorias lineales con 50 puntos intermedios entre el punto
inicial y el punto final. Mientras se va realizando el desplazamiento se van
actualizando los valores de las articulaciones y de las coordenadas x, y, z y
partiendo de la posicin actual del robot.
La rutina esta compuesta por los siguientes desplazamientos:
1. Home al Punto de aproximacin del punto inicial.
2. Punto de aproximacin al Punto inicial (Pick) En el momento que el robot
llega a este ltimo punto procede a cerrar el gripper.
3. Punto inicial (Pick) al punto de aproximacin del punto inicial.
4. Punto de aproximacin del punto inicial al Punto de aproximacin al punto
de place.
5. Punto de aproximacin del punto final al Punto de place,
6. Punto final (Place) al Punto de aproximacin del punto de place.
7. Punto de aproximacin del punto de place al home.

36

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

En las figuras 15 a la 17 se puede ver el robot en distintas etapas de la rutina.

Figura 15: Rutina de Pick & Place: Picking del objeto.

Figura 16: Rutina de Pick & Place: Robot desplazando al objeto.

37

Informe de proyecto final de Robtica 2015-II

Proyecto final de robtica

Figura 17: Rutina de Pick & Place: Robot dejando (Placing) el objeto.
El cdigo generado en Matlab es el siguiente:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

%Punto de home
P0 =[19 ,0 ,24.5 ,0];
%Punto de aproximacin para pick
P1 =[13 ,2 ,10 , - pi /2];
%Punto para pick
P2 =[13 2 8 - pi /2];
%Punto
P3 =[13
%Punto
P4 =[13
n =100;

de aproximacin para place

-2 10 - pi /2];
para pick
-2 8 - pi /2];
%nmero de puntos para mover el pincher

%Primero se mueve a home

mover_a_home () ;
[ q1 , q2 , q3 , q4 ] = cinematica_inv ( P0 (1) , P0 (2) , P0 (3) , P0 (4) ,13.85 , 10.5 , 10.5 , 9.25) ;
%Mostrar los valores de las articulaciones y de las coordenadas
set ( handles . q1 , String , round ( q1 *180/ pi ) ) ;
set ( handles . q_2 , String , round ( q2 *180/ pi ) ) ;
set ( handles . q_3 , String , round ( q3 *180/ pi ) ) ;
set ( handles . q_4 , String , round ( q4 *180/ pi ) ) ;

20
21
22
23
24
25 set ( handles .X , String , P0 (1) ) ;
38

Informe de proyecto final de Robtica 2015-II

26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59

Proyecto final de robtica

set ( handles .Y , String , P0 (2) ) ;

set ( handles .Z , String , P0 (3) ) ;
set ( handles . Theta , String , round ( P0 (4) *180/ pi ) ) ;
%Rutina de movimiento
mover_lineal ( P0 , P1 , n /2) ;
[ q1 , q2 , q3 , q4 ] = cinematica_inv ( P1 (1) , P1 (2) , P1 (3) , P1 (4) ,13.85 , 10.5 , 10.5 , 9.25) ;
%Mostrar los valores de las articulaciones y de las coordenadas
set ( handles . q1 , String , round ( q1 *180/ pi ) ) ;
set ( handles . q_2 , String , round ( q2 *180/ pi ) ) ;
set ( handles . q_3 , String , round ( q3 *180/ pi ) ) ;
set ( handles . q_4 , String , round ( q4 *180/ pi ) ) ;
set ( handles .X , String , P1 (1) ) ;
set ( handles .Y , String , P1 (2) ) ;
set ( handles .Z , String , P1 (3) ) ;
set ( handles . Theta , String , round ( P1 (4) *180/ pi ) ) ;
mover_lineal ( P1 , P2 , n /2) ;
[ q1 , q2 , q3 , q4 ] = cinematica_inv ( P2 (1) , P2 (2) , P2 (3) , P2 (4) ,13.85 , 10.5 , 10.5 , 9.25) ;
%Mostrar los valores de las articulaciones y de las coordenadas
set ( handles . q1 , String , round ( q1 *180/ pi ) ) ;
set ( handles . q_2 , String , round ( q2 *180/ pi ) ) ;
set ( handles . q_3 , String , round ( q3 *180/ pi ) ) ;
set ( handles . q_4 , String , round ( q4 *180/ pi ) ) ;
set ( handles .X , String , P2 (1) ) ;
set ( handles .Y , String , P2 (2) ) ;
set ( handles .Z , String , P2 (3) ) ;
set ( handles . Theta , String , round ( P2 (4) *180/ pi ) ) ;
mover_lineal ( P2 , P1 , n /2) ;
[ q1 , q2 , q3 , q4 ] = cinematica_inv ( P1 (1) , P1 (2) , P1 (3) , P1 (4) ,13.85 , 10.5 , 10.5 , 9.25) ;
%Mostrar los valores de las articulaciones y de las coordenadas
set ( handles . q1 , String , round ( q1 *180/ pi ) ) ;
set ( handles . q_2 , String , round ( q2 *180/ pi ) ) ;
set ( handles . q_3 , String , round ( q3 *180/ pi ) ) ;
set ( handles . q_4 , String , round ( q4 *180/ pi ) ) ;

60
61
62
63
64
65 set ( handles .X , String , P1 (1) ) ;
39

Informe de proyecto final de Robtica 2015-II

66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84

Proyecto final de robtica

set ( handles .Y , String , P1 (2) ) ;

set ( handles .Z , String , P1 (3) ) ;
set ( handles . Theta , String , round ( P1 (4) *180/ pi ) ) ;
mover_lineal ( P1 , P3 , n /2) ;
[ q1 , q2 , q3 , q4 ] = cinematica_inv ( P3 (1) , P3 (2) , P3 (3) , P3 (4) ,13.85 , 10.5 , 10.5 , 9.25) ;
%Mostrar los valores de las articulaciones y de las coordenadas
set ( handles . q1 , String , round ( q1 *180/ pi ) ) ;
set ( handles . q_2 , String , round ( q2 *180/ pi ) ) ;
set ( handles . q_3 , String , round ( q3 *180/ pi ) ) ;
set ( handles . q_4 , String , round ( q4 *180/ pi ) ) ;
set ( handles .X , String , P3 (1) ) ;
set ( handles .Y , String , P3 (2) ) ;
set ( handles .Z , String , P3 (3) ) ;
set ( handles . Theta , String , round ( P3 (4) *180/ pi ) ) ;

mover_lineal ( P3 , P4 , n /2) ;
[ q1 , q2 , q3 , q4 ] = cinematica_inv ( P4 (1) , P4 (2) , P4 (3) , P4 (4) ,13.85 , 10.5 , 10.5 , 9.25) ;
85 %Mostrar los valores de las articulaciones y de las coordenadas
86
87 set ( handles . q1 , String , round ( q1 *180/ pi ) ) ;
88 set ( handles . q_2 , String , round ( q2 *180/ pi ) ) ;
89 set ( handles . q_3 , String , round ( q3 *180/ pi ) ) ;
90 set ( handles . q_4 , String , round ( q4 *180/ pi ) ) ;
91
92 set ( handles .X , String , P4 (1) ) ;
93 set ( handles .Y , String , P4 (2) ) ;
94 set ( handles .Z , String , P4 (3) ) ;
95 set ( handles . Theta , String , round ( P4 (4) *180/ pi ) ) ;
96
97 mover_lineal ( P4 , P3 , n /2) ;
98 [ q1 , q2 , q3 , q4 ] = cinematica_inv ( P3 (1) , P3 (2) , P3 (3) , P3 (4) ,13.85 , 10.5 , 10.5 , 9.25) ;
99
%Mostrar los valores de las articulaciones y de las coordenadas
100 set ( handles . q1 , String , round ( q1 *180/ pi ) ) ;
101 set ( handles . q_2 , String , round ( q2 *180/ pi ) ) ;
102 set ( handles . q_3 , String , round ( q3 *180/ pi ) ) ;
103 set ( handles . q_4 , String , round ( q4 *180/ pi ) ) ;
104
105 set ( handles .X , String , P3 (1) ) ;
40

Informe de proyecto final de Robtica 2015-II

106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126

Proyecto final de robtica

set ( handles .Y , String , P3 (2) ) ;

set ( handles .Z , String , P3 (3) ) ;
set ( handles . Theta , String , round ( P3 (4) *180/ pi ) ) ;
mover_lineal ( P3 , P0 , n /2) ;
[ q1 , q2 , q3 , q4 ] = cinematica_inv ( P0 (1) , P0 (2) , P0 (3) , P0 (4) ,13.85 , 10.5 , 10.5 , 9.25) ;
%Mostrar los valores de las articulaciones y de las coordenadas
set ( handles . q1 , String , round ( q1 *180/ pi ) ) ;
set ( handles . q_2 , String , round ( q2 *180/ pi ) ) ;
set ( handles . q_3 , String , round ( q3 *180/ pi ) ) ;
set ( handles . q_4 , String , round ( q4 *180/ pi ) ) ;
set ( handles .X , String , P0 (1) ) ;
set ( handles .Y , String , P0 (2) ) ;
set ( handles .Z , String , P0 (3) ) ;
set ( handles . Theta , String , round ( P0 (4) *180/ pi ) ) ;
%Mover a home al finalizar .
mover_a_home ;
ready =1;
%
Copyright 2014 The MathWorks , Inc .

Referencias
[1]Gua de proyecto final. Ing. Crdenas, Pedro. 2015
[2]joymex2.
Escabe
Development
Projects.
http://escabe.org/joomla/index.php/7-projects/matlab/1-joymex2

41

2011.