% R-RTR
% complete rotation of driver 1

clear; clc; close all

AB = 0.10 ;  % (m)
AC = 0.05 ;  % (m)
CD = 0.15 ;  % (m)
phi = pi/6;  % (rad)
 
xA = 0; yA = 0;  
xC = AC ; yC = 0; 

step=pi/6; 
for phi=pi/6:step:2*pi+pi/6

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]);
cx = (xB - xC)/CB;
cy = (yB - yC)/CB;

xD = xC + CD*cx;
yD = yC + CD*cy;

% Graphic of the mechanism

text_size = 10;
slider_scale_factor = 0.015;
pin_scale_factor = 0.005;        
         
plot_pin0(xA,yA,pin_scale_factor,0)
plot_pin0(xC,yC,pin_scale_factor,0)
plot_slider( xB, yB, slider_scale_factor, phi2);
hold on
plot(...
[xA,xB],[yA,yB],'r-o',...
[xC,xD],[yC,yD],'b-o');
plot_slider( xB, yB, slider_scale_factor, phi2);

hold off

ax = 0.2;
axis([-ax ax -ax ax])
grid on
xlabel('x (m)'), ylabel('y (m)')

pause(0.1)

end

% eof         

