% R_RTR
% positions for \Phi = 30 (deg)

clear ; clc; close all

AB = 0.10 ;  % (m)
AC = 0.05 ;  % (m)
CD = 0.15 ;  % (m)
phi = pi/6;  % (rad)
 
xA = 0; yA = 0; 
rA_ = [xA yA 0]; 
xC = AC ; yC = 0; 
rC_ = [xC yC 0]; 
xB = AB*cos(phi); 
yB = AB*sin(phi); 
rB_ = [xB yB 0];  

phi2 = atan((yB-yC)/(xB-xC)); 
phi3 = phi2;

CB = norm([xB-xC, yB-yC]);
ux = (xB - xC)/CB;
uy = (yB - yC)/CB;

xD = xC + CD*ux;
yD = yC + CD*uy;
rD_ = [xD yD 0];

fprintf('Results \n\n') 
fprintf('phi = phi1 = %g (degrees) \n',phi*180/pi) 
fprintf('rA_ = [%6.3f,%6.3f,%g] (m)\n',rA_) 
fprintf('rC_ = [%6.3f,%6.3f,%g] (m)\n',rC_) 
fprintf('rB_ = [%6.3f,%6.3f,%g] (m)\n',rB_) 
fprintf('phi2 = phi3 = %6.3f (degrees) \n',phi2*180/pi) 
fprintf('rD_ = [%6.3f,%6.3f,%g] (m)\n',rD_) 

% Graphic of the mechanism
axis manual
axis equal
hold on
grid on
ax = 0.2;
axis([-ax ax -ax ax])

xlabel('x (m)'), ylabel('y (m)')
title('positions for phi = 30 (deg)')

textAx = xA+0.01;
textAy = yA;

textBx = xB+0.01;
textBy = yB-0.01;

textCx = xC+0.01;
textCy = yC;

textDx = xD+0.007;
textDy = yD+0.007;
 
% plotting paramters
text_size = 10;
slider_scale_factor = 0.015;
pin_scale_factor = 0.005;        
         
% plotting joint A
plot_pin0(xA,yA,pin_scale_factor,0)
% labelling joint A
text(textAx, textAy,'A','FontSize',text_size)
% plotting link 1
plot([xA xB], [yA yB], '-o')
% labelling joint B 
text(textBx, textBy,'B','FontSize',text_size)
% plotting link 2 
plot_slider( xB, yB, slider_scale_factor, phi2)
% plotting joint C 
plot_pin0(xC,yC,pin_scale_factor,0)
% labelling joint C
text( textCx, textCy,'C','FontSize',text_size)        
% plotting link 3
plot([xC xD], [yC yD], '-o')
% labelling joint D 
text(textDx, textDy,'D','FontSize',text_size)
        
% eof         

