Stephanny Cedeño Vázquez. Matricula: 120035997. Ingeniería Mecatrónica. Grupo: M178.

Correo: 120035997@upq.edu.mx
Diana Laura Zúñiga Mendoza. Matricula: 120037233
Correo: 120037233@upq.edu.mx

“Trayectoria Mecanismo de 5 barras”

clear al1= atan(Py/Px);

clear all AB=0.65; bet1= acos(((AC^2)+(AB^2)-

(BC^2))/(2*AB*AC));
clc BC=0.90;
th1=al1+bet1;
close all CD=0.90;
ang1=th1/pi;
DE=0.65;

al2= atan(Py/(Ex-Px));
clear a b s s1; Ex=.402;
bet2= acos(((DE^2)+(CE^2)-
a = arduino('COM5', 'UNO', Ey=0;
(CD^2))/(2*DE*CE));
"Libraries", {'Ultrasonic','Servo'});
th2=pi-(al2+bet2);

ang2=th2/pi;
sensor=ultrasonic(a,'D4', 'D5');
v=0;

elseif Px<0
sL = servo(a, 'D2',
'MinPulseDuration', 700*10^-6, al1= atan(Py/-Px);
'MaxPulseDuration', 701*10^-6); disp(k);
bet1= acos(((AC^2)+(AB^2)-
sR = servo(a, 'D3', (BC^2))/(2*AB*AC));
'MinPulseDuration', 700*10^-6,
if k<0.6 th1=pi-al1+bet1;
'MaxPulseDuration', 701*10^-6);
ang1=th1/pi;

b=0.4;

Py=.83;
al2= atan(Py/(Ex-Px));
%Home
for i=0:0.05:b bet2= acos(((DE^2)+(CE^2)-
writePosition(sL, 0.16);
(CD^2))/(2*DE*CE));
writePosition(sR, 0.8); v=v+1;
th2=pi-al2-bet2;
Ex=0.402;
ang2=th2/pi;
pause(2) Ey=0;

elseif Px>Ex
%Objeto %trayectoria lineal horizontal
al1= atan(Py/Px);
writePosition(sL, 0.6); Px=i;
bet1= acos(((AC^2)+(AB^2)-
writePosition(sR, 0.65); (BC^2))/(2*AB*AC));

th1=al1+bet1;

pause(2) AC=sqrt((Px^2)+(Py^2)); ang1=th1/pi;

CE=sqrt((Ex-Px)^2+Py^2);

k=readDistance(sensor)*10; al2= atan(Py/(Px-Ex));

bet2= acos(((DE^2)+(CE^2)-
if Px==0 (CD^2))/(2*DE*CE));
 Stephanny Cedeño Vázquez. Matricula: 120035997. Ingeniería Mecatrónica. Grupo: M178.
Correo: 120035997@upq.edu.mx
Diana Laura Zúñiga Mendoza. Matricula: 120037233
Correo: 120037233@upq.edu.mx

th2=al2-bet2; Ax=0; ValCx=eval(SolC.SolCx);

ang2=th2/pi; Ay=0; ValCy=eval(SolC.SolCy);

Bx=AB*cos(th1); Cx1=ValCx(1);

elseif Px<Ex By=AB*sin(th1); Cy1=ValCy(1);

al1= atan(Py/Px); Dx=(DE*cos(th2))+Ex; Cx2=ValCx(2);

bet1= acos(((AC^2)+(AB^2)- Dy=DE*sin(th2); Cy2=ValCy(2);

(BC^2))/(2*AB*AC));
Cx=Cx2;
th1=al1+bet1;
%Calculo simbolico Cy=Cy2;
ang1=th1/pi;
syms SolCx SolCy

%ecua1
al2= atan(Py/(Ex-Px));
ec1=BC^2==(SolCx-Bx)^2+(SolCy- writePosition(sL, ang2);
bet2= acos(((DE^2)+(CE^2)- By)^2;
writePosition(sR, ang1);
(CD^2))/(2*DE*CE));
ec2=CD^2==(Dx-SolCx)^2+(Dy-
th2=pi-(al2+bet2); SolCy)^2;
end
ang2=th2/pi; %Sistemas de ecuaciones
end
SE=[ec1,ec2];

end %resolver sistemas de ecuaciones

SolC=solve(SE, [SolCx, SolCy]);

%MCD
Stephanny Cedeño Vázquez. Matricula: 120035997. Ingeniería Mecatrónica. Grupo: M178.
Correo: 120035997@upq.edu.mx
Diana Laura Zúñiga Mendoza. Matricula: 120037233
Correo: 120037233@upq.edu.mx
Stephanny Cedeño Vázquez. Matricula: 120035997. Ingeniería Mecatrónica. Grupo: M178.
Correo: 120035997@upq.edu.mx
Diana Laura Zúñiga Mendoza. Matricula: 120037233
Correo: 120037233@upq.edu.mx