L1=200;
L2=62.5;
L3=175;
L4=112.5;
P=[0,0];
S=[L1,0];
axis(gca,'equal');
axis([-100,400,-100,150]);
w=-1;
maxt = 500;
all_theta2 = zeros(maxt, 1);
all_theta3 = zeros(maxt, 1);
all_Q = zeros(maxt, 2);
all_R = zeros(maxt, 2);
for t=1:maxt %One tenth of time
theta2=w*(t/10); %theta2 = w * t, the value of 10 is reduced to accelerate the mechanism.
%1) Angle values obtained by position analysis are entered.
QS=sqrt(L1.^2+L2.^2-2*L1*L2*cos(theta2));
ba=acos((L3.^2+QS.^2-L2.^2)/(2*L1*QS));
fi=acos((L3.^2+QS.^2-L4.^2)/(2*L3*QS));
ga=acos((L4.^2+QS.^2-L3.^2)/(2*L4*QS));
theta3=-ba+fi;
theta4=180-(ba+ga);
%2) The positions of the joint points are entered.
Q=[(L2*cos(theta2)),(L2*sin(theta2))];
R=[(L2*cos(theta2)+L3*cos(theta3)),(L2*sin(theta2)+L3*sin(theta3))];
%3) Links are created.
l2_line=line([P(1),Q(1)],[P(2),Q(2)],'LineWidth',4,'Color','k');
l3_line=line([Q(1),R(1)],[Q(2),R(2)],'LineWidth',4,'Color','k');
l4_line=line([R(1),S(1)],[R(2),S(2)],'LineWidth',4,'Color','k');
%4) Joints are created.
p_joint=viscircles(P,0.1,'Color','r','LineWidth',6);
q_joint=viscircles(Q,0.1,'Color','r','LineWidth',6);
r_joint=viscircles(R,0.1,'Color','r','LineWidth',6);
s_joint=viscircles(S,0.1,'Color','r','LineWidth',6);
%5 Trajectory is created (if any).
R_traj=viscircles(R,0.001,'Color','k');
pause(0.001);
delete(p_joint);
delete(q_joint);
delete(r_joint);
delete(s_joint);
delete(l2_line);
delete(l3_line);
delete(l4_line);
all_theta2(t,:) = theta2;
all_theta3(t,:) = theta3;
all_Q(t,:) = Q;
all_R(t,:) = R;
end
