% R-RTR
% complete rotation of driver 1
% path of point E

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/18; 
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]);
ux = (xB - xC)/CB;
uy = (yB - yC)/CB;

xD = xC + CD*ux;
yD = yC + CD*uy;

b = 0.08;
xE = xB - b*ux;
yE = yB - b*uy;

% 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('path of E')

pM=plot(...
[xA,xB],[yA,yB],'k-o',...
[xC,xD],[yC,yD],'b-o',...
[xB,xE],[yB,yE],'r-o');
plot(xE,yE,'.')

pause(1)

delete(pM)

end

pM=plot(...
[xA,xB],[yA,yB],'k-o',...
[xC,xD],[yC,yD],'b-o',...
[xB,xE],[yB,yE],'r-o');


% eof         

