% cam and follower
% velocity and acceleration analysis
% equivalent mechanism R RRT
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

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

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

% radius OC
R  = Rc + Rf; 

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

% position of C
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',...
[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=Rm*cos(ang);
% ym=Rm*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 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 ]; 

vO_ = vA_ + cross(omega1_,rO_); 
aO_ = aA_ + cross(alpha1_,rO_) - dot(omega1_,omega1_)*rO_;
% or
aO_ = aA_ + cross(alpha1_,rO_) +...
    cross(omega1_,cross(omega1_,rO_));

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

vC  = sym('vC','real');  % vC2  unknown
omega3 = sym('omega3','real'); % omega3 unknown

% vC2 || yy
vC2_ = [0,vC,0];
omega3_  = [0,0,omega3];

% vC3_ = vO_ + omega3_ x OC_
vC3_ = vO_+cross(omega3_,rC_-rO_);

% vC_=vC2_=vC3_
eqvC = vC2_ - vC3_;

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

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);
vCs = eval(solvC.vC);
omega3s = eval(solvC.omega3);

vC_ = [0,vCs,0];
omega3_  = [0,0,omega3s];

fprintf('vC_ = [%g,%6.3f,%g] (m/s)\n', vC_)
fprintf('omega3_ = [%d,%d,%6.3f] (rad/s)\n', omega3_)
fprintf('\n') 

fprintf('Acceleration analysis \n\n')  

aC  = sym('aC','real');  % aC2  unknown
alpha3 = sym('alpha3','real'); % alpha3 unknown

% vC2 || yy
aC2_ = [0,aC,0];
alpha3_  = [0,0,alpha3];

% aC3_ = aO_ + alpha3_ x OC_ - omega3^2 OC_
aC3_ = aO_+cross(alpha3_,rC_-rO_)-dot(omega3_,omega3_)*(rC_-rO_);

eqaC = aC2_ - aC3_;

eqaCx = eqaC(1); % equation component on x-axis
eqaCy = eqaC(2); % equation component on y-axis

digits(3)
qaCx=vpa(eqaCx); 
fprintf('x-axis: %s = 0 \n', char(qaCx)) 
qaCy=vpa(eqaCy); 
fprintf('y-axis: %s = 0 \n', char(qaCy))  
fprintf('=>\n') 

solaC = solve(eqaCx,eqaCy);
aCs = eval(solaC.aC);
alpha3s = eval(solaC.alpha3);

aC_ = [0,aCs,0];
alpha3_  = [0,0,alpha3s];

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

