- No correr todo el programa al mismo tiempo, hacerlo por partes.
- El tamaño de paso para este modelo es de 22.5°, si se desea otro valor modificar todos los cálculos en donde aparezca este valor.
% MODELADO DE BRAZO ROBOTICO SCARA
close all;
clear all;
clc;
%% Variables y constantes del brazo
syms x y z w
a1=101; % elemento uno
a2=59; % elemento dos
a3=60; % elemento tres
a4=102; % elemento cuatro, con actuador final
d1=15; % offset uno
d2=x; % offset dos variable
o1=y; % angulo uno
o2=z; % angulo dos
o3=w; % angulo tres, se dejo encero ya que no había actuador final, pero si hay debe tomarse en cuenta
%% Matriz de referencia
% S0=[cos(o1) -sin(o1) 0 0; sin(o1) cos(o1) 0 0; 0 0 1 a1; 0 0 0 1];
% S1=[1 0 0 a2; 0 1 0 0; 0 0 1 0; 0 0 0 1];
% S2=[cos(o2) -sin(o2) 0 0; sin(o2) cos(o2) 0 0; 0 0 1 d1; 0 0 0 1];
% S3=[cos(o3) -sin(o3) 0 a3; sin(o3) cos(o3) 0 0; 0 0 1 0; 0 0 0 1];
% S4=[0 1 0 0; 1 0 0 0; 0 0 -1 -d2; 0 0 0 1];
% S5=S0*S1*S2*S3*S4
%% Modelo del brazo en pose cero
% figure
% s0=[0,0,0];s1=[0,0,a1];s2=[a2,0,a1];s3=[a2,0,a1+d1];s4=[a2+a3,0,a1];s5=[a2+a3,0,a1+d1-(a4/2)];
% x0=[0,0];y0=[0,0];z0=[0,0];
% x1=[0,0];y1=[0,0];z1=[0,a1];
% x2=[0,a2];y2=[0,0];z2=[a1,a1];
% x3=[a2,a2];y3=[0,0];z3=[a1,a1+d1];
% x4=[a2,a2+a3];y4=[0,0];z4=[a1+d1,a1+d1];
% x5=[a2+a3,a2+a3];y5=[0,0];z5=[a1+d1,a1+d1-a4];
% plot3(x0,y0,z0,'b','linewidth',3),grid on,hold on;
% plot3(x1,y1,z1,'b','linewidth',3),grid on,hold on;
% plot3(x2,y2,z2,'b','linewidth',3),grid on,hold on;
% plot3(x3,y3,z3,'b','linewidth',3),grid on,hold on;
% plot3(x4,y4,z4,'b','linewidth',3),grid on,hold on;
% plot3(x5,y5,z5,'b','linewidth',3),grid on,hold on;
% xlabel('X0'),ylabel('Y0'),zlabel('Z0');
% title('BRAZO SCARA POSE CERO');
%% Mapeo de poses Establecidas
% N=1;
% n1=0;n2=0;
% for i=1 : 4
% Op1=i*(pi/4);
% m1=0;m2=0;
% for l=0 : 3
% fac2=m1+m2;
% m1=m1+1;
% if m1==3
% m2=2;
% else m2=0;
% end
% Op2=fac2*(pi/4);
% for m=0 : 2 : 6
% d2=2.5*m;
% Ap1=((180*(Op1))/pi)
% Ap2=((180*(Op2))/pi)
% S1=round(Ap1/22.5)*22.5; S2=round(Ap2/22.5)*22.5;
% o1=S1
% o2=S2
% o3=0;
% S0=[cosd(o1) -sind(o1) 0 0; sind(o1) cosd(o1) 0 0; 0 0 1 a1; 0 0 0 1];
% S1=[1 0 0 a2; 0 1 0 0; 0 0 1 0; 0 0 0 1];
% S2=[cosd(o2) -sind(o2) 0 0; sind(o2) cosd(o2) 0 0; 0 0 1 d1; 0 0 0 1];
% S3=[cosd(o3) -sind(o3) 0 a3; sind(o3) cosd(o3) 0 0; 0 0 1 0; 0 0 0 1];
% S4=[0 1 0 0; 1 0 0 0; 0 0 -1 -d2; 0 0 0 1];
% S5=S0*S1*S2*S3*S4
% x0=[0,0,0];y0=[0,0,0];z0=[0,0,0];
% x1=[0,0];y1=[0,0];z1=[0,a1];
% x2=[0,cosd(o1)*a2];y2=[0,sind(o1)*a2];z2=[a1,a1];
% x3=[cosd(o1)*a2,cosd(o1)*a2];y3=[sind(o1)*a2,sind(o1)*a2];z3=[a1,a1+d1];
% x4=[cosd(o1)*a2,(cosd(o1)*a2)+(cosd(o1+o2)*a3)];y4=[sind(o1)*a2,(sind(o1)*a2)+(sind(o1+o2)*a3)];z4=[a1+d1,a1+d1];
% x5=[(cosd(o1)*a2)+(cosd(o1+o2)*a3),(cosd(o1)*a2)+(cosd(o1+o2)*a3),(cosd(o1)*a2)+(cosd(o1+o2)*a3)];y5=[(sind(o1)*a2)+(sind(o1+o2)*a3),(sind(o1)*a2)+(sind(o1+o2)*a3),(sind(o1)*a2)+(sind(o1+o2)*a3)];z5=[a1+d1,a1+d1-d2,(a1+d1)+(a4-d2)];
% figure
% plot3(x0,y0,z0,'b','linewidth',3),grid on,hold on;
% plot3(x1,y1,z1,'b','linewidth',3),grid on,hold on;
% plot3(x2,y2,z2,'b','linewidth',3),grid on,hold on;
% plot3(x3,y3,z3,'b','linewidth',3),grid on,hold on;
% plot3(x4,y4,z4,'b','linewidth',3),grid on,hold on;
% plot3(x5,y5,z5,'b','linewidth',3),grid on,hold on;
% xlabel('X0'),ylabel('Y0'),zlabel('Z0');
% title(['BRAZO SCARA POSE ',num2str(N)]);
% Dyx=sqrt((((cosd(o1)*a2)+(cosd(o1+o2)*a3))^2)+(((sind(o1)*a2)+(sind(o1+o2)*a3))^2))
% Dxz=sqrt(((Dyx)^2)+((a1+d1-d2)^2))
% N=N+1
% end
% end
% end
%% Calculo de pose con vector final
% Xf=69; Yf=28; Zf=a1+d1-58; % cambiar estos valores para un vector final distinto
% N=0;
% for i=0 : 1 : 15
% Op1=i*22.5;
% for l=-7 : 1 : 7
% Op2=l*22.5;
% for m=a4 : -1 : 0
% d2=m;
% o1=(Op1);o2=(Op2);o3=0;
% S0=[cosd(o1) -sind(o1) 0 0; sind(o1) cosd(o1) 0 0; 0 0 1 a1; 0 0 0 1];
% S1=[1 0 0 a2; 0 1 0 0; 0 0 1 0; 0 0 0 1];
% S2=[cosd(o2) -sind(o2) 0 0; sind(o2) cosd(o2) 0 0; 0 0 1 d1; 0 0 0 1];
% S3=[cosd(o3) -sind(o3) 0 a3; sind(o3) cosd(o3) 0 0; 0 0 1 0; 0 0 0 1];
% S4=[0 1 0 0; 1 0 0 0; 0 0 -1 -d2; 0 0 0 1];
% S5=S0*S1*S2*S3*S4;
% Ex=abs(S5(1,4)-Xf); % Error en x
% Ey=abs(S5(2,4)-Yf); % Error en y
% Ez=abs(S5(3,4)-Zf); % Error en z
% Ep=abs((Ex+Ey+Ez)/3); % Error promedio
% fprintf('Ex= %5.3f, Ey= %5.3f, Ez= %5.3f, Eprom= %5.3f\n',Ex,Ey,Ez,Ep)
% N=N+1;
% if (Ex<3.5) && (Ey<3.5) && (Ez<=0) || (Ep<7) && (Ez<=0)
% Matriz=S5
% Angulo1=Op1
% Angulo2=Op2
% x0=[0,0,0];y0=[0,0,0];z0=[0,0,0];
% x1=[0,0];y1=[0,0];z1=[0,a1];
% x2=[0,cosd(o1)*a2];y2=[0,sind(o1)*a2];z2=[a1,a1];
% x3=[cosd(o1)*a2,cosd(o1)*a2];y3=[sind(o1)*a2,sind(o1)*a2];z3=[a1,a1+d1];
% x4=[cosd(o1)*a2,(cosd(o1)*a2)+(cosd(o1+o2)*a3)];y4=[sind(o1)*a2,(sind(o1)*a2)+(sind(o1+o2)*a3)];z4=[a1+d1,a1+d1];
% x5=[(cosd(o1)*a2)+(cosd(o1+o2)*a3),(cosd(o1)*a2)+(cosd(o1+o2)*a3),(cosd(o1)*a2)+(cosd(o1+o2)*a3)];y5=[(sind(o1)*a2)+(sind(o1+o2)*a3),(sind(o1)*a2)+(sind(o1+o2)*a3),(sind(o1)*a2)+(sind(o1+o2)*a3)];z5=[a1+d1,a1+d1-d2,(a1+d1)+(a4-d2)];
% figure
% plot3(x0,y0,z0,'b','linewidth',3),grid on,hold on;
% plot3(x1,y1,z1,'b','linewidth',3),grid on,hold on;
% plot3(x2,y2,z2,'b','linewidth',3),grid on,hold on;
% plot3(x3,y3,z3,'b','linewidth',3),grid on,hold on;
% plot3(x4,y4,z4,'b','linewidth',3),grid on,hold on;
% plot3(x5,y5,z5,'b','linewidth',3),grid on,hold on;
% xlabel('X0'),ylabel('Y0'),zlabel('Z0');
% title(['BRAZO SCARA POSE ',num2str(N)]);
% Dyx=sqrt((((cosd(o1)*a2)+(cosd(o1+o2)*a3))^2)+(((sind(o1)*a2)+(sind(o1+o2)*a3))^2))
% Dxz=sqrt(((Dyx)^2)+((a1+d1-d2)^2))
% q=1;
% break;
% end
% q=0;
% end
% if q==1
% break;
% end
% end
% if q==1
% break;
% end
% end