% Program 2.7
% R-RRR-RRT
% Position analysis
% Path of C2 (mass center of link BC)

clear; clc; close all 
AB=0.15; BC=0.40; CD=0.37; CE=0.23; EF=CE;
La=0.30; Lb=0.45; Lc=CD;	
xA=0; yA=0; xD=La ; yD=Lb ; xF=-Lc ;

incr = 0 ;

for phi=0:pi/10:2*pi,

xB = AB*cos(phi); yB = AB*sin(phi);
eqnC1 = '( xCsol - xB )^2 + ( yCsol - yB )^2 = BC^2 ';
eqnC2 = '( xCsol - xD )^2 + ( yCsol - yD )^2 = CD^2 ';
solC = solve(eqnC1, eqnC2, 'xCsol, yCsol');
xCpositions = eval(solC.xCsol);
yCpositions = eval(solC.yCsol);
xC1 = xCpositions(1); xC2 = xCpositions(2); 
yC1 = yCpositions(1); yC2 = yCpositions(2); 
if xC1 < xD xC = xC1; yC = yC1; else xC = xC2; yC=yC2; end 

eqnE1 = '( xEsol - xC )^2 + ( yEsol - yC )^2 = CE^2 ';
eqnE2 = '( yD - yC ) / ( xD - xC ) = ( yEsol - yC ) / ( xEsol - xC )';
solE = solve(eqnE1, eqnE2, 'xEsol, yEsol');
xEpositions = eval(solE.xEsol); yEpositions = eval(solE.yEsol);
xE1 = xEpositions(1); xE2 = xEpositions(2); 
yE1 = yEpositions(1); yE2 = yEpositions(2); 
if xE1 < xC xE = xE1; yE=yE1; else xE = xE2; yE=yE2; end

eqnF = '( xF - xE )^2 + ( yFsol - yE )^2 = EF^2 ';
solF = solve(eqnF,'yFsol');
yFpositions=eval(solF);
yF1 = yFpositions(1); yF2 = yFpositions(2); 
if yF1 < yE yF=yF1; else yF=yF2; end

incr = incr + 1;

XC2(incr) = ( xB + xC )/2 ; YC2(incr) = ( yB + yC )/2 ;

% Graphic of the mechanism 
subplot(2,1,1),...  
plot([xA,xB],[yA,yB],'r-',[xB,xC],[yB,yC],'b-',...
[xD,xE],[yD,yE],'g-',[xE,xF],[yE,yF],'k-'),...
hold on,... 
xlabel('x (m)'), ylabel('y (m)'), grid,...
axis([-0.5 0.4 -0.25 0.7]),...
title('Positions of the mechanism'),... 
text(XC2,YC2,'C2'),... 
    
end

% Path of C2 (mass center of link 2)
subplot(2,1,2),...  
plot(XC2, YC2, '-ko'),... 
xlabel('x (m)'), ylabel('y (m)'),... 
title('Path described by C2'), grid,...