% cam and follower
% simulation

clear; clc; close all

% revolute joint at A 
Rm = 0.1;     % m radius of the circle
Rf = 0.0025;  % (m)
R  = Rm + Rf; % R = Rc
e = 0.04; % m distance from follower to O1

% center of the circle at O
% position of O
RO = 0.5*R;

phii = pi/3;
phif = 2*pi+pi/3;

for phi=phii:pi/10:phif

% position of O    
xO = RO*cos(phi); 
yO = RO*sin(phi); 
rO_ = [xO,yO,0];

% position of C
d = e-xO;
psi = acos(d/R); 
xC = e; 
yC = yO+R*sin(psi) ; 
rC_ = [xC,yC,0];

% Graphic of the mechanism
axis manual
axis equal
hold on
grid on
ds = .25;
axis([-ds ds -ds ds])
xlabel('x (m)'),ylabel('y (m)')
gr = plot([0,xO],[0,yO],'k-o',[xO,xC],[yO,yC],'b-o',...
[xC,xC],[yC,yC+0.15],'r-');

ang=0:0.01:2*pi; 
xp=R*cos(ang);
yp=R*sin(ang);
ci = plot(xO+xp,yO+yp,'k--');
pivot0_scale = 0.005; 
plot_pivot0(0,0,pivot0_scale,0);
plot_wall(xC-.005,.178,.05,0);
plot_wall(xC+.005,.178,.05,pi);
pause(.5);
delete(gr);
delete(ci);
end

gr = plot([0,xO],[0,yO],'k-o',[xO,xC],[yO,yC],'b-o',...
[xC,xC],[yC,yC+1.5*R],'r-');
ci = plot(xO+xp,yO+yp,'k--');
plot_pivot0(0,0, pivot0_scale, 0)
text(0, 0+0.005,'  A')
text(xO+0.005,yO+0.005,'O')
text(xC+0.005,yC+0.005,'C')

% eof