% Displacement Analysis of a Shaper Quick Return Machine

close all
clear all


r1=4; %fixed link
r2=2; %crank
r4=8; %link 4
r5=3; %link 5
r7=8;   %link 7; Height of the slider


for i=1:37
    theta2(i)=(i-1)*10*(pi/180);
    r3(i)=sqrt(r1^2+2*r1*r2*sin(theta2(i))+r2^2);
    theta3(i)=atan2((r1+r2*sin(theta2(i))),r2*cos(theta2(i)));
    a1(i)=1;
    b1(i)=-2*r4*cos(theta3(i));
    c1(i)=-r5^2+r4^2+r7^2-2*r4*r7*sin(theta3(i));
    r61(i)=-((-b1(i)+sqrt(b1(i)^2-4*a1(i)*c1(i)))/(2*a1(i)));
    r62(i)=-((-b1(i)-sqrt(b1(i)^2-4*a1(i)*c1(i)))/(2*a1(i)));
    theta51(i)=atan2(r7-r4*sin(theta3(i)),-r61(i)-r4*cos(theta3(i)));
    theta52(i)=atan2(r7-r4*sin(theta3(i)),-r62(i)-r4*cos(theta3(i)));
    
    % Calculate joint locations
    xa(i)=r2*cos(theta2(i));
    ya(i)=r2*sin(theta2(i));
    xb(i)=r4*cos(theta3(i));
    yb(i)=r4*sin(theta3(i))-r1;
    xc1(i)=xb(i)+r5*cos(theta51(i));
    yc1(i)=yb(i)+r5*sin(theta51(i));
    xc2(i)=xb(i)+r5*cos(theta52(i));
    yc2(i)=yb(i)+r5*sin(theta52(i));
    
    % Generate the links at every instant, closure #1
    x1(1,i)=0;
    y1(1,i)=0;
    x1(2,i)=xa(i);
    y1(2,i)=ya(i);
    x1(3,i)=0;
    y1(3,i)=-r1;
    x1(4,i)=xb(i);
    y1(4,i)=yb(i);
    x1(5,i)=xc1(i);
    y1(5,i)=yc1(i);
    
    % Generate the links at every instant, closure #2
    x2(1,i)=0;
    y2(1,i)=0;
    x2(2,i)=xa(i);
    y2(2,i)=ya(i);
    x2(3,i)=0;
    y2(3,i)=-r1;
    x2(4,i)=xb(i);
    y2(4,i)=yb(i);
    x2(5,i)=xc2(i);
    y2(5,i)=yc2(i);
end;

figure
plot(xa,ya,xb,yb,'--',xc1,yc1,'-.')
legend('crank','end of link 4','output, closure#1')
axis([-5 5 -5 5])
axis equal
title('1st closure')

figure
plot(xa,ya,xb,yb,'--',xc2,yc2,'-.')
legend('crank','end of link 4','output, closure#2')
axis([-5 5 -5 5])
axis equal
title('2nd closure')

%plot the links, Closure #1
figure
for i=1:37
    plot(x1(1:5,i),y1(1:5,i))
    hold on
end;
axis([-5 5 -5 5])
title ('motion of the links, Closure #1')
axis equal
grid on

%plot the links, Closure #2
figure
for i=1:37
    plot(x2(1:5,i),y2(1:5,i),'r')
    hold on
end;
axis([-5 5 -5 5])
title ('motion of the links, Closure #2')
axis equal
grid on

