clear all

close all

L1=2;

L2=1.5;

L3=2.8;

L4=3;

% thetai0 = valor inicial dado a thetai (i=1,2,3).

% Obsérvese que los valores elegidos para theta30 y theta40 determinan el modo de
ensamblado.

theta20=pi/6;

theta30=2*pi/6;

theta40=3*pi/6;

% omega2 = velocidad de giro de 2.

% tVuelta = tiempo invertido en una vuelta.

omega2=2*pi/3;

tVuelta=2*pi/omega2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% RESOLUCIÓN DE POSICIÓN INICIAL %

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Data = vector de datos.

Data=[theta20 L1 L2 L3 L4];

% X = vector de incógnitas.

X=[theta30 theta40];

fsolveOptions=optimset('Display','off'); % Info off

% IMPORTANTE: La función pasada a fsolve, EEG(), está dada como una "anonymous
function"

% para poder incluir en la ventana argumentos ajenos a X. La ayuda

mostrada

% desde Octave no menciona esta posibilidad.

X=fsolve(@(X)EEG(X,Data),X,fsolveOptions);

%%%%%%%%%%%%%%%%%%
% FIGURA INICIAL %

%%%%%%%%%%%%%%%%%%

hFig=figure;

theta2=theta20;

theta3=X(1);

theta4=X(2);

xPuntos=[0,L2*cos(theta2),L2*cos(theta2)+L3*cos(theta3),L1]';

yPuntos=[0,L2*sin(theta2),L2*sin(theta2)+L3*sin(theta3),0]';

hMecanismo=plot(xPuntos,yPuntos,'b','Marker','o','MarkerEdgeColor','r');

axis equal

axis([-2 4 -2 4]);

figAlto=10;

figAncho=12;

set(hFig,'PaperUnits','inches')

set(hFig,'PaperSize',[figAlto,figAncho])

set(hFig,'PaperPosition',[0,0,figAncho,figAlto])

set(gca,'fontsize',20)

print(hFig,'Mecanismo.png','-dpng')

grid on

%%%%%%%%%%%%%%

% SIMULACIÓN %

%%%%%%%%%%%%%%

pause

vtheta2=[];

vtheta3=[];

vtheta4=[];

for t=0:tVuelta/100:tVuelta

theta2=theta20+omega2*t;

vtheta2=[vtheta2;theta2];

% Actualizamos Data:
 Data(1)=theta2;

% X contiene, en este momento, la solución obtenida para la posición anterior.

% Es, por tanto, un buen valor inicial para la resolución de la posición actual.

X=fsolve(@(X)EEG(X,Data),X,fsolveOptions);

% Actualizamos la figura:

theta3=X(1);

theta4=X(2);

vtheta3=[vtheta3;theta3];

vtheta4=[vtheta4;theta4];

xPuntos=[0,L2*cos(theta2),L2*cos(theta2)+L3*cos(theta3),L1]';

yPuntos=[0,L2*sin(theta2),L2*sin(theta2)+L3*sin(theta3),0]';

set(hMecanismo,'XData',xPuntos,'YData',yPuntos);

pause(0.025);

end

hFig=figure;

plot(vtheta2,vtheta3,'r',vtheta2,vtheta4,'b')

axis([min(vtheta2) max(vtheta2)]);

grid on

xlabel('\theta_2 (rad)','fontsize',18)

ylabel('\theta_3, \theta_4 (rad)','fontsize',18)

legend('{\fontsize{18}\theta_3}','{\fontsize{18}\theta_4}','location','northwest')

figAlto=10;

figAncho=16;

set(hFig,'PaperUnits','inches')

set(hFig,'PaperSize',[figAlto,figAncho])

set(hFig,'PaperPosition',[0,0,figAncho,figAlto])

set(gca,'fontsize',20)

print(hFig,'theta3y4Graf.png','-dpng')