Unidad #: Tarea 2

Control de Robots

Tarea 2. Escribiendo mi
nombre
Torres Rodriguez Juan Jesus
M-163
 Unidad #: Tarea 2
Control de Robots

Tarea 2. Escribiendo mi nombre

• b) Condiciones iniciales propuestos (puntos de la trayectoria y tiempo

de ejecución de las trayectorias)
Se utilizaron las siguientes condiciones iniciales y el tiempo base:
%%TRAYECTORIA JESUS

%%VARIABLES GLOBALES
global J1 J2 J3 J4 J5 J6 E1 E2 E3 E4 E5 E6 S1 S2 S3 S4 S5 S6 U1 U2 U3
U4 S11 S12 S13 S14 S15 S16 L1 L2 L3 tb

%%TIEMPO BASE
tb= 1;

%%LETRA J
J1= [2;5];
J2= [2;2];
J3= [4;2];
J4= [4;7];
J5= [3;7];
J6= [5;7];

%%LETRA E
E1= [6;7];
E2= [8;7];
E3= [6;5];
E4= [8;5];
E5= [6;2];
E6= [8;2];

%%LETRA S
S1= [9;2];
S2= [12;2];
S3= [12;5];
S4= [9;5];
S5= [9;7];
S6= [12;7];

%%LETRA U
U1= [13;7];
U2= [13;2];
U3= [16;2];
U4= [16;7];
 Unidad #: Tarea 2
Control de Robots

%%LETRA DOS S
S11= [17;7];
S12= [20;7];
S13= [17;5];
S14= [20;5];
S15= [20;2];
S16= [17;2];

%%PARAMETROS ROBOT 3GDL

L1=12;
L2=11;
L3=9;
• c) Código comentado
Se realizo el siguiente codigo el cual muestra Toda la trayectoria de mi
nombre en este caso JESUS
function [P] = ESCRIBIENDO_MI_NOMBRE(t)

global J1 J2 J3 J4 J5 J6 E1 E2 E3 E4 E5 E6 S1 S2 S3 S4 S5 S6 U1 U2 U3
U4 S11 S12 S13 S14 S15 S16 tb

if t<=tb %%PRIMER LINEA DE LETRA J

x=J2(1);
y=(J2(2)-J1(2))*t/tb+J1((2));
% z=0;
else
if t>=tb && t<=2*tb %%SEGUNDA LINEA LETRA J
t2=t-tb;
x=(J3(1)-J2(1))*t2/tb+J2((1));
y=J3(2);
% z=0;
else
if t>=2*tb && t<=3*tb %%TERCER LINEA LETRA J
t3=t-2*tb;
x=J4(1);
y=(J4(2)-J3(2))*t3/tb+J3((2));
% z=0;
else
if t>=3*tb && t<=4*tb %%CUARTA LINEA LETRA J
t4=t-3*tb;
x=J5(1);
y=(J5(2)-J4(2))*t4/tb+J4((2));
% z=0;
else
if t>=4*tb && t<=5*tb %%QUINTA LINEA LETRA J
 Unidad #: Tarea 2
Control de Robots

t5=t-4*tb;
x=J6(1);
y=(J4(2)-J6(2))*t5/tb+J6((2));
% z=0;
else
if t>=5*tb && t<=6*tb %%UNION DE LETRA J-E
t6=t-5*tb;
x=J6(1);
y=(J6(2)-E1(2))*t6/tb+E1(2);
% z=0;
else
if t>=6*tb && t<=7*tb %%PRIMER LINEA LETRA E
t7=t-6*tb;
x=(E1(1)-E2(1))*t7/tb+E2(1);
y=E2(2);
% z=0;
else
if t>=7*tb && t<=8*tb %%SEGUNDA LINEA LETRA E
t8=t-7*tb;
x=E3(1);
y=(E3(2)-E1(2))*t8/tb +E1(2);
% z=0;
else
if t>=8*tb && t<=9*tb %%TERCER LINEA LETRA
E
t9= t-8*tb;
x=(E3(1)-E4(1))*t9/tb+E4(1);
y=E4(2);
% z=0;
else
if t>=9*tb && t<=10*tb %%CUARTA LINEA
LETRA E
t10=t-9*tb;
x=E5(1);
y=(E5(2)-E3(2))*t10/tb +E3(2);
% z=0;
else
if t>=10*tb && t<=11*tb %%QUINTA LINEA
LETRA E
t11=t-10*tb;
x=(E5(1)-E6(1))*t11/tb+E6(1);
y=E6(2);
% z=0;
else
 Unidad #: Tarea 2
Control de Robots

if t>=11*tb && t<=12*tb %%UNION DE

LETRA E-S
t12=t-10*tb;
x=(S1(1)-E6(1))*t12/tb+E6(1);
y=S1(2);
% z=0;
else
if t>=12*tb && t<=13*tb %%PRIMER
LINEA LETRA S
t13=t-12*tb;
x=(S2(1)-S1(1))*t13/tb+S1(1);
y=S2(2);
% z=0;
else
if t>=13*tb && t<=14*tb %%SEGUNDA
LINEA LETRA S
t14=t-13*tb;
x=S3(1);
y=(S3(2)-S2(2))*t14/tb+S2((2));
% z=0;
else
if t>=14*tb && t<=15*tb
%%TERCER LINEA LETRA S
t15=t-14*tb;
x=S4(1);
y=(S4(2)-S3(2))*t15/tb+S3((2));
% z=0;
else
if t>=15*tb && t<=16*tb
%%CUARTA LINEA LETRA S
t16=t-15*tb;
x=S5(1);
y=(S5(2)-
S4(2))*t16/tb+S4((2));
% z=0;
else
if t>=16*tb && t<=17*tb
%%QUINTA LINEA LETRA S
t17=t-16*tb;
x=(S6(1)-
S5(1))*t17/tb+S5(1);
y=S6(2);
% z=0;
else
 Unidad #: Tarea 2
Control de Robots

if t>=17*tb && t<=18*tb

%%UNION LETRA S-U
t18=t-17*tb;
x=(U1(1)-
S6(1))*t18/tb+S6(1);
y=U1(2);
% z=0;
else
if t>=18*tb && t<=19*tb
%%PRIMER LINEA LETRA U
t19=t-18*tb;
x=U1(1);
y=(U2(2)-
U1(2))*t19/tb+U1((2));
% z=0;
else
if t>=19*tb && t<=20*tb
%%SEGUNDA LINEA LETRA U
t20=t-19*tb;
x=(U3(1)-
U2(1))*t20/tb+U2(1);
y=U3(2);
% z=0;
else
if t>=20*tb &&
t<=21*tb %%TERCER LINEA LETRA U
t21=t-20*tb;
x=U3(1);
y=(U4(2)-
U3(2))*t21/tb+U3(2);
% z=0;
else
if t>=21*tb &&
t<=22*tb %%UNION LETRA U-S
t22=t-21*tb;
x=(S11(1)-
U4(1))*t22/tb+U4(1);
y=S11(2);
% z=0;
else
if t>=22*tb &&
t<=23*tb %%PRIMER LINEA S2
t23=t-22*tb;
x=(S11(1)-
S12(1))*t23/tb+S12(1);
 Unidad #: Tarea 2
Control de Robots

y=S11(2);
% z=0;
else
if t>=23*tb
&& t<=24*tb %%SEGUNDA LINEA S2
t24=t-
23*tb;
x=S13(1);
y=(S13(2)-
S11(2))*t24/tb+S11(2);
% z=0;
else
if t>=24*tb
&& t<=25*tb %%TERCER LINEA S2
t25=t-
24*tb;

x=(S14(1)-S13(1))*t25/tb+S13(1);

y=S14(2);
% z=0;
else
if
t>=25*tb && t<=26*tb %%CUARTA LINEA S2

t26=t-25*tb;

x=S15(1);

y=(S15(2)-S14(2))*t26/tb+S14(2);
% z=0;
else
if
t>=26*tb && t<=27*tb %%QUINTA LINEA S2

t27=t-26*tb;

x=(S16(1)-S15(1))*t27/tb+S15(1);

y=S16(2);
% z=0;
end
end
end
end
 Unidad #: Tarea 2
Control de Robots

end
end
end

end
end
end
end

end
end
end
end
end
end
end
end
end

end
end
end

end

end
end
end
P=[x;y];
End
 Unidad #: Tarea 2
Control de Robots

A continuacion se muestra el codigo realizado para la cinematica directa

del manipulador el cual es el siguiente:

Ilustración 1 Manipulador a trabajar

function [P] = Cinematica_Directa(q)

%Esta funcion es para el calculo de la cinematica directa

global L1 L2

q1=q(1);
q2=q(2);
q3=q(3);

x=(L1*sin(q1)+L2*sin(q2+q3))*sin(q1);
y=L1*cos(q2)+L2*cos(q2+q3);
z=(L1*sin(q1)+L2*sin(q2+q3))*cos(q1);

P=[x;y];
End
 Unidad #: Tarea 2
Control de Robots

Se muestra el codigo realizado para el calculo de la cinematica inversa del

manipulador:

function [q] = Cinematica_Inversa(P)

%%ESTA FUNCION ES PARA EL CALCULO DE LA CINEMATICA
INVERSA

global L1 L2

% Px=P(1);
Py=P(2);
Pz=P(3);

R=Px^2+Pz^2;
cosq3=(d^2-L1^2-L2^2)/(2*L1*L2);
d=L1^2+L2^2+2*L1*L2*cosq3;
senq3=sqrt(1-cosq3);

alpha=atan2(R,Py);
beta=atan2(L2*senq3,L1+L2*cosq3);

q1=atan2(Px,Pz);
q2=alpha-beta;
q3=atan2(senq3,cosq3);

q=[q1;q2;q3];
end
 Unidad #: Tarea 2
Control de Robots

• d) Diagrama de bloques
A continuacion se muestran imagnes del codigo de bloques realizados en
simulink:

Ilustración 2 Diagrama de Bloques Simulink

 Unidad #: Tarea 2
Control de Robots

• e) Gráficas en el tiempo
A continuacion se muestran imágenes de las graficas respecto al tiempo
de nuestra simulacion en simulink:

Ilustración 3 Graficas en el tiempo coordenadas cartesiano

Ilustración 4 Grafica en el Tiempo coordenadas articulares

 Unidad #: Tarea 2
Control de Robots

• f) Gráficas en el plano XY
A continuacion se muestran las graficas en el plano XY tanto en la
trayectoria deseada como en la trayectoria real del manipulador:

Ilustración 5 Trayectoria Deseada

Ilustración 6 Trayectoria Real

 Unidad #: Tarea 2
Control de Robots

• g) Conclusiones
Al realizar el anterior trabajo se pudo poner en practica lo visto en clases
que fueron las simulaciones de las trayectorias y a su vez aplicarle algunos
temas ya antes vistos en el curso de dinamica de robots, una vez realizadas
las simulaciones integrando tanto la inversa y la directa se pudo visluazar
que ambas programaciones incluyendo la de la trayectoria se encuentran
bien puesto que en las graficas podemos ver que nuestra trayectoria
desada que programamos y al compararla con la trayectoria real se dio a
conocer que fue bien programada y se logro el objetivo de pasar las
coordenas cartesianas a articulares y viceversa.