ANEXO IV

Vista previa del material en texto

Universidad de Sevilla 
Escuela Politécnica Superior de Sevilla 
 
 
 
 
 
PROGRAMACIÓN E IMPLEMENTACIÓN DE UN BRAZO 
ROBÓTICO HACIENDO USO DE LAS COMUNICACIONES 
INDUSTRIALES PARA SU CONTROL 
 
 
ANEXO IV. CÓDIGO DE LA GUI 
 
 
 
 
 
 
 
Autor: Adrián Díaz Campano 
Tutor: Luis Muñoz Saavedra 
Departamento de Arquitectura y Tecnología de Computadores 
2 
ANEXO IV. CÓDIGO DE LA GUI 
Código más relevante de la GUI 
 
 
Comunicación serie 
 
%****************************************************** 
%* INICIA COMUNICACIONES. PUERTO SERIE * 
%****************************************************** 
delete(instrfind({'Port'},{'COM5'})); % Límpia el puerto. 
global serial_object; 
serial_object = serial('COM5','BaudRate',57600); 
fopen(serial_object); % Abre el puerto 
 
%****************************************************** 
%* ENVÍO DE MENSAJES. PUERTO SERIE * 
%****************************************************** 
q1 = int16(str2double(handles.Texto_q1.String)); % q1 en int16. 
Terminador= 255; % Terminador del mensaje a enviar por el puerto 
serie. 
fwrite(serial_object,'P'); % Envia posición del servo A. 
fwrite(serial_object,'A'); 
fwrite(serial_object,q1,'int16'); 
fwrite(serial_object,Terminador); 
 
 
 
Simulación 
 
%****************************************************** 
%* SIMULACIÓN * 
%****************************************************** 
 
q1 = str2double(handles.Texto_q1.String)*pi/180; 
q2 = str2double(handles.Texto_q2.String)*pi/180; 
q3 = str2double(handles.Texto_q3.String)*pi/180; 
q4 = str2double(handles.Texto_q4.String)*pi/180; 
q5 = str2double(handles.Texto_q5.String)*pi/180; 
 
% Parámetros DH. 
% Theta d a alfa tipo offset 
L(1)= Link ([ 0 L2 L1 pi/2 0 0 ]); 
L(2)= Link ([ 0 0 L3 0 0 pi/2 ]); 
L(3)= Link ([ 0 0 L4 pi/2 0 0 ]); 
L(4)= Link ([ 0 L5 0 pi/2 0 0 ]); 
L(5)= Link ([ 0 0 L6 0 0 pi/2 ]); 
 
Rob = SerialLink(L); 
Rob.name = 'Robot'; 
Rob.plot([q1,q2,q3,q4,q5]); % Gráfica. 
3 
ANEXO IV. CÓDIGO DE LA GUI 
T = Rob.fkine([q1,q2,q3,q4,q5]); % Cálculo de la matriz T. 
 
% Se escribe la posición del punto final de la simulaicón por 
pantalla. 
handles.Texto_Px.String = num2str(T.t(1),'%1.1f'); 
handles.Texto_Py.String = num2str(T.t(2),'%1.1f'); 
handles.Texto_Pz.String = num2str(T.t(3),'%1.1f'); 
 
 
 
Implementación cinemática directa 
 
%****************************************************** 
%* CÁLCULOS CINEMÁTICA DIRECTA * 
%****************************************************** 
q1 = str2double(handles.Texto_q1.String); 
q2 = str2double(handles.Texto_q2.String); 
q3 = str2double(handles.Texto_q3.String); 
q4 = str2double(handles.Texto_q4.String); 
q5 = str2double(handles.Texto_q5.String); 
 
% Ecuación 5.3.10. 
Px = L1*cosd(q1) + L5*(cosd(q1)*cosd(q2)*cosd(q3) - 
cosd(q1)*sind(q2)*sind(q3)) - L3*cosd(q1)*sind(q2) - 
L6*sind(q5)*(sind(q1)*sind(q4) - cosd(q4)*(cosd(q1)*cosd(q2)*sind(q3) 
+ cosd(q1)*cosd(q3)*sind(q2))) + 
L6*cosd(q5)*(cosd(q1)*cosd(q2)*cosd(q3) - cosd(q1)*sind(q2)*sind(q3)) 
- L4*cosd(q1)*cosd(q2)*sind(q3) - L4*cosd(q1)*cosd(q3)*sind(q2); 
 
% Ecuación 5.3.11. 
Py = L1*sind(q1) - L5*(sind(q1)*sind(q2)*sind(q3) - 
cosd(q2)*cosd(q3)*sind(q1)) - L3*sind(q1)*sind(q2) + 
L6*sind(q5)*(cosd(q1)*sind(q4) + cosd(q4)*(cosd(q2)*sind(q1)*sind(q3) 
+ cosd(q3)*sind(q1)*sind(q2))) - 
L6*cosd(q5)*(sind(q1)*sind(q2)*sind(q3) - cosd(q2)*cosd(q3)*sind(q1)) 
- L4*cosd(q2)*sind(q1)*sind(q3) - L4*cosd(q3)*sind(q1)*sind(q2); 
 
% Ecuación 5.3.12. 
Pz = L2 + L5*(cosd(q2)*sind(q3) + cosd(q3)*sind(q2)) + L3*cosd(q2) + 
L6*cosd(q5)*(cosd(q2)*sind(q3) + cosd(q3)*sind(q2)) + 
L4*cosd(q2)*cosd(q3) - L4*sind(q2)*sind(q3) - 
L6*cosd(q4)*sind(q5)*(cosd(q2)*cosd(q3) - sind(q2)*sind(q3)); 
 
 
% Se escribe la posición del punto final por pantalla. 
handles.Texto_PxC.String = num2str(Px,'%1.1f'); 
handles.Texto_PyC.String = num2str(Py,'%1.1f'); 
handles.Texto_PzC.String = num2str(Pz,'%1.1f'); 
 
% Ecuación 5.3.1. 
nx = cosd(q5)*(cosd(q1)*cosd(q2)*cosd(q3) - 
cosd(q1)*sind(q2)*sind(q3)) - sind(q5)*(sind(q1)*sind(q4) - 
cosd(q4)*(cosd(q1)*cosd(q2)*sind(q3) + cosd(q1)*cosd(q3)*sind(q2))); 
 
% Ecuación 5.3.2. 
4 
ANEXO IV. CÓDIGO DE LA GUI 
ny = sind(q5)*(cosd(q1)*sind(q4) + 
cosd(q4)*(cosd(q2)*sind(q1)*sind(q3) + cosd(q3)*sind(q1)*sind(q2))) - 
cosd(q5)*(sind(q1)*sind(q2)*sind(q3) - cosd(q2)*cosd(q3)*sind(q1)); 
 
 
% Ecuación 5.3.3. 
nz = cosd(q5)*(cosd(q2)*sind(q3) + cosd(q3)*sind(q2)) - 
cosd(q4)*sind(q5)*(cosd(q2)*cosd(q3) - sind(q2)*sind(q3)); 
 
% Ecuación 5.3.4. 
%ox = -cosd(q5)*(sind(q1)*sind(q4) - 
cosd(q4)*(cosd(q1)*cosd(q2)*sind(q3) + cosd(q1)*cosd(q3)*sind(q2))) - 
sind(q5)*(cosd(q1)*cosd(q2)*cosd(q3) - cosd(q1)*sind(q2)*sind(q3)); 
 
% Ecuación 5.3.5. 
%oy = cosd(q5)*(cosd(q1)*sind(q4) + 
cosd(q4)*(cosd(q2)*sind(q1)*sind(q3) + cosd(q3)*sind(q1)*sind(q2))) + 
sind(q5)*(sind(q1)*sind(q2)*sind(q3) - cosd(q2)*cosd(q3)*sind(q1)); 
 
% Ecuación 5.3.6. 
oz = - sind(q5)*(cosd(q2)*sind(q3) + cosd(q3)*sind(q2)) - 
cosd(q4)*cosd(q5)*(cosd(q2)*cosd(q3) - sind(q2)*sind(q3)); 
 
% Ecuación 5.3.7. 
%ax = - cosd(q4)*sind(q1) - sind(q4)*(cosd(q1)*cosd(q2)*sind(q3) + 
cosd(q1)*cosd(q3)*sind(q2)); 
 
% Ecuación 5.3.8. 
%ay = cosd(q1)*cosd(q4) - sind(q4)*(cosd(q2)*sind(q1)*sind(q3) + 
cosd(q3)*sind(q1)*sind(q2)); 
 
% Ecuación 5.3.9. 
az = sind(q4)*(cosd(q2)*cosd(q3) - sind(q2)*sind(q3)); 
 
% Ecuación 5.10. 
beta = atand(-nz/(sqrt(1-(-nz)^2))); 
 
if az < 0 
 alpha = atand(oz/az) - 180; % Debido a que atand solo da valores 
entre [90,-90] hay que tener en cuenta los demás casos restando 180 
grados. Ecuación 5.6. 
else 
 alpha = atand(oz/az); % Ecuación 5.6. 
end 
 
if nx < 0 
 gamma = atand(ny/nx) - 180; % Debido a aque atand solo da valores 
entre [90,-90] hay que tener en cuenta los demás casos restando 180 
grados. Ecuación 5.7. 
else 
 gamma = atand(ny/nx); % Ecuación 5.7. 
end 
 
% Se escribe la orientación del punto final por pantalla. 
handles.Texto_alphaC.String = num2str(alpha,'%1.1f'); 
handles.Texto_betaC.String = num2str(beta,'%1.1f'); 
handles.Texto_gammaC.String = num2str(gamma,'%1.1f'); 
 
 
5 
ANEXO IV. CÓDIGO DE LA GUI 
 
 
Implementación cinemática inversa 
%****************************************************** 
%* CÁLCULOS CINEMÁTICA INVERSA * 
%****************************************************** 
L1 = 11; L2 = 96.3; L3 = 120; L4 = -6; L5 = 120.5; L6 = 123; % Medidas 
del brazo. 
 
Px = str2double(handles.Texto_PxC.String); 
Py = str2double(handles.Texto_PyC.String); 
Pz = str2double(handles.Texto_PzC.String); 
alpha = str2double(handles.Texto_alphaC.String); 
beta = str2double(handles.Texto_betaC.String); 
gamma = str2double(handles.Texto_gammaC.String); 
 
% Transformación de los ángulos alpha, beta y gamma a matriz de 
rotación 
% (Ecuación 5.12) 
nx = cosd(gamma)*cosd(beta); 
ny = sind(gamma)*cosd(beta); 
nz = -sind(beta); 
ox = -sind(gamma)*cosd(alpha) + cosd(gamma)*sind(beta)*sind(alpha); 
oy = cosd(gamma)*cosd(alpha) + sind(gamma)*sind(beta)*sind(alpha); 
oz = cosd(beta)*sind(alpha); 
ax = sind(gamma)*sind(alpha) + cosd(gamma)*sind(beta)*cosd(alpha); 
ay = -cosd(gamma)*sind(alpha) + sind(gamma)*sind(beta)*cosd(alpha); 
az = cosd(beta)*cosd(alpha); 
 
% Es necesario redondear para que MATLAB no ponga ceros negativos. 
nx = round(nx,5); 
ny = round(ny,5); 
nz = round(nz,5); 
ox = round(ox,5); 
oy = round(oy,5); 
oz = round(oz,5); 
ax = round(ax,5); 
ay = round(ay,5); 
az = round(az,5); 
 
Pf = [Px Py Pz]; % Posición del extremo del robot. Ecuación 4.30. 
Pm = Pf - L6*[nx ny nz]; % Posición de la muñeca del robot. Ecuación 
5.29. 
 
% Medidas del robot 
L =sqrt(L4^2 + L5^2); % Ecuación 5.13. 
h = Pm(3) - L2; % Ecuación 5.14. 
psi = atand(L5/-L4); % Ecuación5.15. 
 
 
% Primera articulación. 
q1= atand(abs(Pm(2)/Pm(1))); % Ecuación 5.16. Sólo se moverá entre los 
[0,90]. 
 
q1= round(q1); % Se redondea el ángulo, ya que no se van a usar más 
decimales. Así se evitan mayores errores en el cálculo. 
 
6 
ANEXO IV. CÓDIGO DE LA GUI 
 
% Tercera articulación. 
r_p_xy = sqrt((Pm(1)-L1*cosd(q1))^2 +(Pm(2)-L1*sind(q1))^2); % 
Ecuación 5.20. 
cos_theta3 = (-r_p_xy^2 - h^2 + L3^2 + L^2)/(2*L3*L); % Ecuación 5.23. 
 
if cos_theta3 <= 0 % Desde q3 = [0, 90]. 
 q3 = (atand(sqrt(abs(1-cos_theta3^2))/cos_theta3) - psi) +180; % 
Ecuación 5.25. 
 
else % Desde q3 = [-40, 0). 
 q3 = (atand(sqrt(abs(1-cos_theta3^2))/cos_theta3) - psi) ; % 
Ecuación 5.25. 
end 
q3 = round(q3 
 
 
% Segunda articulación. 
if q1 == 0 || q1 == 180 
 if (Pm(1)-L1*cosd(q1))/(abs(Pm(1)-L1*cosd(q1))) == 
L1*cosd(q1)/abs(L1*cosd(q1)) 
 b = atand(h/r_p_xy); % Ecuación 5.27. 
 else 
 b = -atand(h/r_p_xy) + 180; % Ecuación 5.27. 
 end 
elseif q1 == 90 
 if (Pm(2)-L1*sind(q1))/(abs(Pm(2)-L1*sind(q1))) == 
L1*sind(q1)/abs(L1*sind(q1)) 
 b = atand(h/r_p_xy); % Ecuación 5.27. 
 else 
 b = -atand(h/r_p_xy) + 180; % Ecuación 5.27. 
 end 
else 
 if (Pm(1)-L1*cosd(q1))/(abs(Pm(1)-L1*cosd(q1))) == 
L1*cosd(q1)/abs(L1*cosd(q1)) && (Pm(2)-L1*sind(q1))/(abs(Pm(2)-
L1*sind(q1))) == L1*sind(q1)/abs(L1*sind(q1)) 
 b = atand(h/r_p_xy); % Ecuación 5.27. 
 else 
 b = -atand(h/r_p_xy) + 180; % Ecuación 5.27. 
 end 
end 
 
al = abs(atand(L*sind(q3 + psi)/(L3 - L*cosd(q3 + psi))));% Ecuación 
5.28. 
q2 = (b + al) - 90; % Ecuación 5.26. 
q2 = round(q2); 
 
 
% Cuarta articulación. 
 
cos_theta4 = -ax*sind(q1) + ay*cosd(q1); % Ecuación 5.39. 
cos_theta5 = nx*(cosd(q1)*cosd(q2)*cosd(q3) - 
cosd(q1)*sind(q2)*sind(q3)) - ny*(sind(q1)*sind(q2)*sind(q3) - 
cosd(q2)*cosd(q3)*sind(q1)) + nz*(cosd(q2)*sind(q3) + 
cosd(q3)*sind(q2)); % Ecuación 5.40. 
q4 = atand(sqrt(abs(1-cos_theta4^2))/cos_theta4); % Ecuación 5.42. 
if cos_theta4 < 0 
 q4 = q4 + 180; % Debido a que la función atand() da valores 
entre -90º y 90º y q4 tiene un rango comprendido de 0º a 180º hay que 
hacer este ajuste. 
7 
ANEXO IV. CÓDIGO DE LA GUI 
end 
q4 = round(q4); 
 
q5 = atand(sqrt(abs(1-cos_theta5^2))/cos_theta5); % Ecuación 5.44. 
 
if cos_theta5 < 0 
 q5 = q5 +180; % debido a que la función atand() da valores entre 
-90º y 90º y q5 tiene un rango comprendido de 0º a 120º hay que hacer 
este ajuste. 
end 
q5= round(q5); 
 
 
% Se escribe la posición de los motores por pantalla. 
handles.Texto_q1.String = num2str(q1); 
handles.Texto_q2.String = num2str(q2); 
handles.Texto_q3.String = num2str(q3); 
handles.Texto_q4.String = num2str(q4); 
handles.Texto_q5.String = num2str(q5);