% R_RRT_RTR Mechanism
% position analysis

clear; clc; close all

AB = 0.20 ;	% m 	
BC = 0.21 ;	% m 	
CD = 0.39 ;	% m	
CF = 0.45 ; % m
a  = 0.30 ;	% m	
b  = 0.25 ;	% m
c  = 0.05 ;	% m
phi = 45*pi/180 ; % rad
n = -500 ;        % rpm

% position of joint A
xA = 0; yA = 0;
rA_ = [xA yA 0];
fprintf('Results \n\n') 
fprintf('phi = phi1 = %g (degrees) \n',phi*180/pi) 
fprintf('rA_ = [%g, %g, %g] (m)\n', rA_) 

% position of joint E
xE =  a;
yE = -b;
rE_ = [xE yE 0]; % position vector of C
fprintf('rE_ = [%6.3f, %6.3f, %g] (m)\n', rE_) 

% position of joint B 
xB = AB*cos(phi); 
yB = AB*sin(phi); 
rB_ = [xB yB 0]; % position vector of B
fprintf('rB_ = [%6.3f, %6.3f, %g] (m)\n', rB_)

% position of joint D
yD = c;
syms xDs
eqD = (xB - xDs)^2 + (yB - yD)^2 - (BC+CD)^2;
solD = solve(eqD, xDs);
xD1=eval(solD(1)); 
xD2=eval(solD(2)); 

if xD1 > xB xD = xD1; else xD = xD2; end
rD_ = [xD yD 0]; % position vector of D 
fprintf('rD_ = [%6.3f, %g, %g] (m)\n', rD_)

% angle of link 2
phi2 = atan((yB-yD)/(xB-xD));
fprintf...
('phi2 = %6.3f (degrees) \n', phi2*180/pi)

% position of joint C
syms xC yC
eqC1 = (xB-xC)^2 + (yB-yC)^2 - BC^2;
eqC2 = (yB-yD)/(xB-xD)-(yD-yC)/(xD-xC);
solC = solve(eqC1, eqC2);
xCpos = eval(solC.xC);
yCpos = eval(solC.yC);
xC1 = xCpos(1); xC2 = xCpos(2); 
yC1 = yCpos(1); yC2 = yCpos(2); 

if xC1 > xB xC=xC1;yC=yC1; else
xC=xC2; yC=yC2; end
rC_ = [xC yC 0]; % position vector of C 
fprintf('rC_ = [%6.3f, %6.3f, %g] (m)\n', rC_)

% angle of link 4
phi4 = atan2((yC-yE),(xC-xE));
phi5 = phi4;
fprintf...
('phi4 = phi5 = %6.3f (degrees) \n', phi4*180/pi)
xF = xC-CF*cos(phi4);
yF = yC-CF*sin(phi4);
rF_ = [xF yF 0];
fprintf('rF_ = [%6.3f, %6.3f, %g] (m)\n', rF_)

% graphic of the mechanism
axis manual
hold on
grid on
sx = .8;
sy = .8;
axis([-sx sx -sy sy]) 
xlabel('x (m)'),ylabel('y (m)')
pM = plot([xA,xB],[yA,yB],'r-o',...
     [xB,xD],[yB,yD],'b-o',...   
     [xC,xE],[yC,yE],'k-o',...
     [xC,xF],[yC,yF],'k-o');
plot(xC, yC, 'k.','Color', 'red')
xlabel('x (m)'),ylabel('y (m)')
pin0_scale = 0.015;  
slider_scale = 0.05;      
% plotting joint A 
plot_pin0( xA, yA, pin0_scale, 0)
% plotting slider at D
plot_slider( xD, yD, slider_scale, pi)
% plotting slider at E
plot_slider( xE, yE, slider_scale, phi4)
% plotting joint E 
plot_pivot0( xE, yE, pin0_scale, pi/2)

text(xA+0.03,yA,'A'), 
text(xB+0.02,yB+0.03,'B'),
text(xC,yC+0.035,'C'), 
text(xD+0.03,yD,'D'),
text(xE-0.05,yE,'E'),
text(xF-0.04,yF,'F')

% eof

% phi = phi1 = 45 (degrees) 
% rA_ = [0, 0, 0] (m)
% rE_ = [ 0.300, -0.250, 0] (m)
% rB_ = [ 0.141,  0.141, 0] (m)
% rD_ = [ 0.734, 0.05, 0] (m)
% phi2 = -8.764 (degrees) 
% rC_ = [ 0.349,  0.109, 0] (m)
% phi4 = phi5 = 82.242 (degrees) 
% rF_ = [ 0.288, -0.336, 0] (m)
