% cam and follower

clear; clc; close all

% revolute joint at A = O1 
Rc = 0.1;     % (m) radius of the circle of the cam
e  = 0.04;    % (m) distance from follower to A
RO = 0.5*Rc;  % AO = RO
Rf = 0.0025;  % (m)
Lf = 0.15;    % (m) length of the follower
w  = 2*Rf;    % width of the follower
d  = 0.01;    % (m) depth of the cam and follower
rho= 8000;    % (kg/m^3) density of the material
g  = 9.807;   % (m/s^2) gravitational acceleration
Fe = 100;     % (N) external force on follower

% radius OC
R  = Rc + Rf; 

% angle between AO and x-axis
phi = pi/3; 
fprintf('phi = %6.3f (deg)\n', phi*180/pi)  

% position of O = Oc (rO_ = AO_)
xO = RO*cos(phi); 
yO = RO*sin(phi); 
rO_ = [xO, yO , 0];
fprintf('rO_ = [%6.3f,%6.3f,%g] (m)\n', rO_)

% angle between OC and x-axis
psi = acos((e-xO)/R); 
fprintf('psi = %6.3f (deg)\n', psi*180/pi)  

% position of C (rC_ = AC_)
xC = e; 
yC = yO+R*sin(psi) ; 
rC_ = [xC, yC, 0];
fprintf('rC_ = [%6.3f,%6.3f,%g] (m)\n', rC_)

% 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)')
plot([0,xO],[0,yO],'k-o',[xO,xC],[yO,yC],'b-o',...
[0,xC],[0,yC],'g-o',[xC,xC],[yC,yC+Lf],'r-')
hold on
ang=0:0.01:2*pi; 
xp=R*cos(ang);
yp=R*sin(ang);
ci = plot(xO+xp,yO+yp,'k--');
xm=Rc*cos(ang);
ym=Rc*sin(ang);
cm = plot(xO+xm,yO+ym,'k-');
pivot0_scale = 0.005; 
plot_pivot0(0,0,pivot0_scale,0);
plot_wall(xC-.005,.2,.05,0);
plot_wall(xC+.005,.2,.05,pi);
text(0,0,'  A')
text(xO,yO,'  O')
text(xC,yC+.005,'  C')

fprintf('\n') 
fprintf('Velocity and acceleration analysis \n\n')  

n = 30; % (rpm)  driver link
omega1_ = [0 0 pi*n/30]; 
alpha1_ = [0 0 0]; 
fprintf('omega1_ = [%g,%g,%6.3f] (rad/s)\n', omega1_) 
fprintf('alpha1_ = [%g,%g,%6.3f] (rad/s^2)\n', alpha1_) 
fprintf('\n') 

vA_ = [0 0 0]; 
aA_ = [0 0 0]; 

vC1_ = vA_ + cross(omega1_,rC_); 
aC1_ = aA_ + cross(alpha1_,rC_) - dot(omega1_,omega1_)*rC_;
%aC1_ = aO1_ + cross(alpha1_,rC_) +...
%    cross(omega1_,cross(omega1_,rC_));

fprintf('vC1_ = [%6.3f,%6.3f,%g] (m/s)\n', vC1_) 
fprintf('aC1_ = [%6.3f,%6.3f,%g] (m/s^2)\n', aC1_) 
fprintf('\n') 

n_ = [-cos(psi), -sin(psi), 0];
t_ = [-sin(psi),  cos(psi), 0];

vC2  = sym('vC2','real');  % vC2  unknown
vC21 = sym('vC21','real'); % vC21 unknown

% vC2 parallel to the vertical sliding direction
vC2_ = [0, vC2, 0];
% vC21_ perpendicular to the radius of curvature OC
% vC21_ parallel to the common tangent
% vC21_ relative velocity of C on link 2 about link 1
vC21_ = vC21*t_;

% vC2_ = vC1_ + vC21_ 
eqvC = vC2_ - (vC1_ + vC21_);
eqvCx = eqvC(1); % equation component on x-axis
eqvCy = eqvC(2); % equation component on y-axis

fprintf('vC2_ = vC1_ + vC21_ => \n') 
digits(3)
qvCx=vpa(eqvCx); 
fprintf('x-axis: %s = 0 \n', char(qvCx)) 
qvCy=vpa(eqvCy); 
fprintf('y-axis: %s = 0 \n', char(qvCy))  
fprintf('=>\n') 

solvC = solve(eqvCx,eqvCy);
vC2s = eval(solvC.vC2);
vC21s = eval(solvC.vC21);

vC2_ = [0, vC2s, 0];
vC21_ = vC21s*t_;

fprintf('vC2_ = [%6.3f,%6.3f,%g] (m/s)\n', vC2_)
fprintf('vC21_ = [%6.3f,%6.3f,%d] (m/s)\n', vC21_)
fprintf('\n') 

% aC21cor_ = 2 omega1_ x vC21_ 
aC21cor_ = 2*cross(omega1_, vC21_); 
fprintf('aC21cor_ = [%6.3f,%6.3f,%d] (m/s^2)\n', aC21cor_)

% aC21_ = aC21n_+aC21t_ 

% aC21n_ = vC21^2/R 
% R radius of curvature
% aC21n_ parallel to the common normal
aC21n_ = (dot(vC21_, vC21_)/R)*n_;
fprintf('aC21n_ = [%6.3f,%6.3f,%d] (m/s^2)\n', aC21n_)
fprintf('\n') 

aC2  = sym('aC2','real');  % aC2  unknown
aC21t = sym('aC21t','real'); % aC21t unknown

% aC2 parallel to the sliding direction y axis
aC2_ = [0, aC2, 0];
% aC21t_ parallel to the common tangent
aC21t_ = aC21t*t_;

% aC2_ = aC1_ + aC21n_ + aC21t_ + aC21cor_
eqaC = aC2_ - (aC1_ + aC21n_ + aC21t_ + aC21cor_);
eqaCx = eqaC(1); % equation component on x-axis
eqaCy = eqaC(2); % equation component on y-axis

solaC = solve(eqaCx,eqaCy);
aC2s = eval(solaC.aC2);
aC21ts = eval(solaC.aC21t);

aC2_ = [0, aC2s, 0];
aC21t_ = aC21ts*t_;

fprintf('aC2_ = [%6.3f,%6.3f,%g] (m/s^2)\n', aC2_)
fprintf('aC21t_ = [%6.3f,%6.3f,%d] (m/s^2)\n', aC21t_) 
fprintf('\n') 

% force analysis

% Link 2 follower
Fe_ = [0, -Fe, 0];
fprintf('Fe_ = [%d, %6.3f, %d] (N)\n', Fe_)

m2 = rho*Lf*w*d + rho*(pi*Rf^2/2)*d;
fprintf('m2 = %6.4f (kg)\n', m2)
G2_ = [0, -m2*g, 0];

F02=sym('F02','real');
F12=sym('F12','real');
F02_ = [F02, 0, 0]; 
F12_ = [F12*cos(psi), F12*sin(psi),0];

% m2*aC2_ = F02_ + F12_ + G2_ + Fe_
eqF2 = F02_ + F12_ + G2_ + Fe_ - m2*aC2_;
solF2=solve(eqF2(1),eqF2(2));
F02s=eval(solF2.F02);
F12s=eval(solF2.F12);

F12_ = [F12s*cos(psi), F12s*sin(psi),0];
fprintf('F02 = %6.3f (N)\n', F02s)
fprintf('F12 = %6.3f (N)\n', F12s)
fprintf('F12_ = [%6.3f, %6.3f, %d] (N)\n', F12_)
fprintf('\n') 

% Link 1 cam
m1 = rho*pi*Rc^2*d;
fprintf('m1 = %6.4f (kg)\n', m1)
fprintf('\n')
G1_ = [0, -m1*g, 0];

% IA*alpha1_ = Mm_+rO_xG1_+rP_xF21_
% P contact point between the cam and follower
xP = e - Rf*cos(psi); 
yP = yO + Rc*sin(psi); 
rP_ = [xP, yP, 0];
Mm_ = -cross(rO_, G1_)-cross(rP_, -F12_);
fprintf('Mm = %6.4f (N m)\n', Mm_(3))
fprintf('\n')

% eof

% phi = 60.000 (deg)
% rO_ = [ 0.025, 0.043,0] (m)
% psi = 81.585 (deg)
% rC_ = [ 0.040, 0.145,0] (m)
% 
% Velocity and acceleration analysis 
% 
% omega1_ = [0,0, 3.142] (rad/s)
% alpha1_ = [0,0, 0.000] (rad/s^2)
% 
% vC1_ = [-0.455, 0.126,0] (m/s)
% aC1_ = [-0.395,-1.428,0] (m/s^2)
% 
% vC2_ = vC1_ + vC21_ => 
% x-axis: 0.989*vC21 + 0.455 = 0 
% y-axis: vC2 - 0.146*vC21 - 0.126 = 0 
% =>
% vC2_ = [ 0.000, 0.058,0] (m/s)
% vC21_ = [ 0.455,-0.067,0] (m/s)
% 
% aC21cor_ = [ 0.423, 2.856,0] (m/s^2)
% aC21n_ = [-0.301,-2.038,0] (m/s^2)
% 
% aC2_ = [ 0.000,-0.650,0] (m/s^2)
% aC21t_ = [ 0.274,-0.040,0] (m/s^2)
% 
% Fe_ = [0, -100.000, 0] (N)
% m2 = 0.0608 (kg)
% F02 = -14.876 (N)
% F12 = 101.651 (N)
% F12_ = [14.876, 100.557, 0] (N)
% 
% m1 = 2.5133 (kg)
% 
% Mm = 2.4860 (N m)