-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathpickplace_demo.m
executable file
·78 lines (68 loc) · 2.38 KB
/
pickplace_demo.m
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
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
76
77
78
clear
addpath('Cinematica')
addpath('FuncionesGraficas')
addpath('FuncionesUtiles')
run('IRB5500.m')
run('inigraf.m')
run('creascena.m');
%Calculo la matriz homogenea a partir de donde se encuentra el robot
q=[0,-pi/2,0,0,-pi/2,0];
robot=crearob(r1,q');
MH=dircin(r1,q');
Mtray=MH;
tf = 1;
%Espero que la pieza llege al sensor 2
mpieza(pieza,10,2000,2,1);
%Muevo el robot para coger la pieza
p1 = MH(1:3,4);
p2=[-2000;1000;900]; %centro de la pieza
%El ultimo parametro de gentrayc_cnf es la configuracion del robot
%(codo-arriba,etc...)
qtrayectoria=gentrayc_recta(p1,p2,tf,Mtray,r1,8,[0,0,0]);
dibrob(robot,r1,qtrayectoria);
%Muevo el robot y la pieza sobre la otra cinta con un movimiento circular
% Antes salgo da la envolvente de la pared subiendo recto en Z
p1 = p2;
p2 =[p1(1);p1(2);p1(3)+200];
qtrayectoria = gentrayc_recta(p1,p2,tf,Mtray,r1,8,[0,0,0]);
movercubo(robot,r1,qtrayectoria,pieza);
% Me muevo con la pieza sobre la otra cinta fuera la envolvente de la pared
p1 = p2;
p2=[2000;1000;900+200];
qtrayectoria = gentrayc_circular(p1,p2,tf,Mtray,r1,8,[0,0,0],1);
movercubo(robot,r1,qtrayectoria,pieza);
%Bajo en Z sobre la cinta
p1 = p2;
p2=[2000;1000;900];
qtrayectoria = gentrayc_recta(p1,p2,tf,Mtray,r1,8,[0,0,0]);
movercubo(robot,r1,qtrayectoria,pieza);
%Vuelvo a la posicion HOME
% Es necesario hacer un cambio de configuracion
p1 = p2;
p2 = [1331;0;1738];
qtrayectoria = gentrayc_recta(p1,p2,tf,Mtray,r1,3,[0,0,0]);
dibrob(robot,r1,qtrayectoria);
%Espero que llege la pieza al otro sensor
mpieza(pieza,10,2000,2,0);
%Muevo el robot a recoger la pieza sobre la cinta 2
p1 = p2;
p2=[2000;-1000;900]; %centro de la pieza
%El ultimo parametro de gentrayc_cnf es la configuracion del robot
%(codo-arriba,etc...)
qtrayectoria=gentrayc_recta(p1,p2,tf,Mtray,r1,3,[0,0,0]);
dibrob(robot,r1,qtrayectoria);
%Muevo el robot y la pieza sobre el armario
% Antes salgo da la envolvente de la pared subiendo recto en Z
p1 = p2;
p2 =[p1(1);p1(2);p1(3)+500];
qtrayectoria = gentrayc_recta(p1,p2,tf,Mtray,r1,3,[0,0,0]);
movercubo(robot,r1,qtrayectoria,pieza);
p1 = p2;
p2=[0;-2400;1200];
qtrayectoria = gentrayc_recta(p1,p2,tf,Mtray,r1,3,[0,0,-pi/2]);
movercubo(robot,r1,qtrayectoria,pieza);
%Vuelvo a la posicion HOME
p1 = p2;
p2 = [-1331;0;1738];
qtrayectoria=gentrayc_recta(p1,p2,tf,Mtray,r1,8,[0,0,0]);
dibrob(robot,r1,qtrayectoria);