% R-RRR 
% velocity and acceleration analysis
% classical method

clear; clc; close all
AB=0.15; %(m)
BC=0.35; %(m)
CD=0.20; %(m) 
xD=0.30; yD=0; %(m) 
phi = pi/3; %(rad)

xB=AB*cos(phi); yB=AB*sin(phi); rB=[xB yB 0]; 
syms xCsol yCsol
eqnC1=(xCsol - xB)^2 +(yCsol - yB)^2 -BC^2;
eqnC2=(xCsol - xD)^2 +(yCsol - yD)^2 -CD^2;
solC = solve(eqnC1, eqnC2, xCsol, yCsol);
xCpositions = eval(solC.xCsol);
yCpositions = eval(solC.yCsol);
xC1 = xCpositions(1); xC2 = xCpositions(2); 
yC1 = yCpositions(1); yC2 = yCpositions(2); 
if yC1 > 0 xC = xC1; yC = yC1;
else xC = xC2; yC=yC2; end 
rC_ = [xC yC 0];
phi2=atan2((yB-yC),(xB-xC));
phi3=atan((yD-yC)/(xD-xC))+pi;

xA = 0; yA = 0; 
rA_ = [xA yA 0]; 
rD_ = [xD yD 0]; 
xB = AB*cos(phi); 
yB = AB*sin(phi); 
rB_ = [xB yB 0]; 

fprintf('Results \n\n')
fprintf('phi = %6.3f (degrees) \n',phi*180/pi)
fprintf('rA_ = [%6.3f,%6.3f,%g] (m)\n',rA_)
fprintf('rB_ = [%6.3f,%6.3f,%g] (m)\n',rB_)
fprintf('rC_ = [%6.3f,%6.3f,%g] (m)\n',rC_)
fprintf('rD_ = [%6.3f,%6.3f,%g] (m)\n',rD_)
fprintf('phi2 = %6.3f (degrees) \n',phi2*180/pi)
fprintf('phi3 = %6.3f (degrees) \n',phi3*180/pi)

axis manual
axis equal
hold on
grid on
axis([-0.1 0.5 -0.1 0.5])
xlabel('x (m)'),ylabel('y (m)')

% Graphic of the mechanism 
plot([xA,xB],[yA,yB],'r-o',...
[xB,xC],[yB,yC],'k-o',...
[xD,xC],[yD,yC],'k-o')
title('Four-bar (R-RRR) mechanism')
text(xA,yA,'  A'), text(xB,yB,'  B')
text(xC,yC,'  C'), text(xD,yD,'  D')

fprintf('\n') 
fprintf('velocity and acceleration analysis \n')  
fprintf('classical method \n\n')  
n = 60; % (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]; 

vB1_ = vA_ + cross(omega1_,rB_); 
vB2_ = vB1_; 
vB_  = vB1_; 

aB1_ = aA_ + cross(alpha1_,rB_) - dot(omega1_,omega1_)*rB_; 
aB2_ = aB1_;
aB_  = aB1_;

fprintf('vB_=vB1_=vB2_= [%6.3f,%6.3f,%g ] (m/s)\n',vB1_) 
fprintf('aB_=aB1_=aB2_= [%6.3f,%6.3f,%g ] (m/s^2)\n',aB1_)  
fprintf('\n') 

vD_ = [0 0 0]; aD_ = [0 0 0]; 

% velocity of C
omega2z = sym('omega2z','real');
omega3z = sym('omega3z','real');
omega2_ = [ 0 0 omega2z ];
omega3_ = [ 0 0 omega3z ];
% B2 & C points on link 2
% vC_ = vC2_ = vB_ + omega2_ x rBC_ 
% B3 & D points on link 3
% vC_ = vC3_ = vD_ + omega3_ x rDC_ 
% vC_ = vC2_ = vC3_
eqvC_=vB2_ + cross(omega2_,rC_-rB_)-...
     (vD_ + cross(omega3_,rC_-rD_));
eqvCx = eqvC_(1); % equation component on x-axis
eqvCy = eqvC_(2); % equation component on y-axis
solvC = solve(eqvCx,eqvCy);
omega2zs=eval(solvC.omega2z);
omega3zs=eval(solvC.omega3z);
omega2_ = [0 0 omega2zs];
omega3_ = [0 0 omega3zs];

digits(3)
% print the equations for calculating omega2z and omega3z
fprintf('vC_=vB_ + omega2_ x rBC_ = vD_ + omega3_ x rDC_ => \n') 
qvCx=vpa(eqvCx); 
fprintf('x-axis: %s = 0 \n', char(qvCx)) 
qvCy=vpa(eqvCy); 
fprintf('y-axis: %s = 0 \n', char(qvCy));
fprintf('=>\n') 
fprintf('omega2z = %6.3f (rad/s)\n',omega2zs) 
fprintf('omega3z = %6.3f (rad/s)\n',omega3zs) 
fprintf('\n') 
fprintf('omega2_ = [%g,%g,%6.3f ] (rad/s)\n',omega2_) 
fprintf('omega3_ = [%g,%g,%6.3f ] (rad/s)\n',omega3_)  
fprintf('\n') 

vC_ = vB2_ + cross(omega2_,rC_-rB_);
fprintf('vC_ =  [%6.3f,%6.3f,%6.3f] (m/s)\n',vC_) 
fprintf('\n')  

% acceleration of C
alpha2z = sym('alpha2z','real');
alpha3z = sym('alpha3z','real');
alpha2_ = [ 0 0 alpha2z ];
alpha3_ = [ 0 0 alpha3z ];
% aC_=aC2_=aB_ + alpha2_ x rBC_ - (omega2_.omega2_)rBC_
% aC_=aC3_=aD_ + alpha3_ x rDC_ - (omega3_.omega3_)rDC_
eqaC2_ = aB2_ + cross(alpha2_,rC_-rB_) - ...
        dot(omega2_,omega2_)*(rC_-rB_); 
eqaC3_ = aD_ + cross(alpha3_,rC_-rD_) -...
        dot(omega3_,omega3_)*(rC_-rD_); 
eqaC_ = eqaC2_ - eqaC3_;
eqaCx = eqaC_(1); % equation component on x-axis
eqaCy = eqaC_(2); % equation component on y-axis
solaC = solve(eqaCx,eqaCy);
alpha2zs=eval(solaC.alpha2z);
alpha3zs=eval(solaC.alpha3z);
alpha2_ = [0 0 alpha2zs];
alpha3_ = [0 0 alpha3zs];

% print the equations for calculating alpha2z and alpha3z
fprintf('aC2_=aB_ + alpha2_ x rBC_ - (omega2_.omega2_)rBC_ \n') 
fprintf('aC3_=aD_ + alpha3_ x rDC_ - (omega3_.omega3_)rDC_ \n') 
fprintf('aC_ = aC2_ = aC3_ \n') 
qaCx=vpa(eqaCx); 
fprintf('x-axis: %s = 0 \n',char(qaCx)) 
qaCy=vpa(eqaCy); 
fprintf('y-axis: %s = 0 \n',char(qaCy)) 
fprintf('=>\n') 
fprintf('alpha2z = %6.3f (rad/s^2)\n',alpha2zs) 
fprintf('alpha3z = %6.3f (rad/s^2)\n',alpha3zs) 
fprintf('\n') 
fprintf('alpha2_ = [%g,%g,%6.3f ] (rad/s^2)\n',alpha2_) 
fprintf('alpha3_ = [%g,%g,%6.3f ] (rad/s^2)\n',alpha3_) 
fprintf('\n') 

aC_ = aB2_ + cross(alpha2_,rC_-rB_) -...
     dot(omega2_,omega2_)*(rC_-rB_);
fprintf('aC_ =  [%6.3f,%6.3f,%g] (m/s^2)\n',aC_)  
fprintf('\n') 

%%%%%%%%%%%%%%%%%%%
rC1_ = rB_/2; 
rC2_ = (rB_+rC_)/2;
rC3_ = (rC_+rD_)/2;
aC1_ = aB1_/2; 
aC2_ = (aB_+aC_)/2; 
aC3_ = (aC_+aD_)/2; 

fprintf('rC1_ = [%6.3f,%6.3f,%d] (m)\n',rC1_)
fprintf('rC2_ = [%6.3f,%6.3f,%d] (m)\n',rC2_)
fprintf('rC3_ = [%6.3f,%6.3f,%d] (m)\n',rC3_)
fprintf('aC1_ = [%6.3f,%6.3f,%d] (m/s^2)\n', aC1_)
fprintf('aC2_ = [%6.3f,%6.3f,%d] (m/s^2)\n', aC2_)
fprintf('aC3_ = [%6.3f,%6.3f,%d] (m/s^2)\n', aC3_)
fprintf...
('alpha1_ = [%g,%g,%6.3f](rad/s^2)\n', alpha1_) 
fprintf...
('alpha2_ = [%g,%g,%6.3f](rad/s^2)\n', alpha2_) 
fprintf...
('alpha3_ = [%g,%g,%6.3f](rad/s^2)\n', alpha3_) 
fprintf('\n')


fprintf('\n')
fprintf('Dynamic force analysis \n')

h = 0.01; % height of the links
d = 0.001; % depth of the links
hSlider = 0.02; % height of the slider 
wSlider = 0.04; % depth of the slider 
rho = 8000; % density of the material
g = 9.807; % gravitational acceleration

fprintf('Link 1 \n')
m1 = rho*AB*h*d; % mass
G1_ = [0,-m1*g,0]; % force of gravity
Fin1_ = -m1*aC1_;  % force of inertia
IC1 = m1*(AB^2+h^2)/12; % mass moment of inertia
IA = IC1 + m1*(AB/2)^2; % A fixed point
alpha1_ = [0 0 0]; % angular acceleration
Min1_ = -IC1*alpha1_; % moment of inertia
fprintf('m1 = %6.3f (kg)\n', m1)
fprintf('G1_ = - m1 g_ = [%d,%6.3f,%d] (N)\n', G1_)
fprintf('m1 aC1_ = [%6.3f,%6.3f,%d] (N)\n', m1*aC1_)
fprintf('Fin1_= - m1 aC1_ = [%6.3f,%6.3f,%d] (N)\n', Fin1_)
fprintf('IC1 = %g (kg m^2)\n', IC1)
fprintf('IA = %g (kg m^2)\n', IA)
fprintf('IC1 alpha1_=[%6.3f,%6.3f,%d](N m)\n',IC1*alpha1_)
fprintf('Min1_=-IC1 alpha1_=[%d, %d, %d](N m)\n',Min1_)
fprintf('\n')

fprintf('Link 2 \n')
m2 = rho*BC*h*d; % mass
G2_ = [0,-m2*g,0]; % force of gravity
Fin2_ = -m2*aC2_;  % force of inertia
IC2 = m2*(BC^2+h^2)/12;  % mass moment of inertia
Min2_ = -IC2*alpha2_;  % moment of inertia
fprintf('m2 = %6.3f (kg)\n', m2)
fprintf('G2_ = - m2 g_ = [%d,%6.3f,%d] (N)\n', G2_)
fprintf('m2 aC2_ = [%6.3f,%6.3f,%d] (N)\n', m2*aC2_)
fprintf('Fin2_= - m2 aC2 =[%6.3f,%6.3f,%d] (N)\n', Fin2_)
fprintf('IC2 = %6.3d (kg m^2)\n', IC2)
fprintf('IC2 alpha2_=[%g,%g,%6.3d](N m)\n',IC2*alpha2_)
fprintf('Min2_=-IC2 alpha2=[%d,%d,%6.3d](N m)\n',Min2_)
fprintf('\n')

fprintf('Link 3 \n')
m3 = rho*CD*h*d; % mass
G3_ = [0,-m3*g,0]; % force of gravity
Fin3_ = -m3*aC3_;  % force of inertia
IC3 = m3*(CD^2+h^2)/12; % mass moment of inertia
IC  = IC3+m3*(CD/2)^2; % C fixed point
Min3_ = -IC3*alpha3_; % moment of inertia
fprintf('m3 = %6.3f (kg)\n', m3)
fprintf('G3_ = - m3 g_ = [%g, %6.3f, %g] (N)\n', G3_)
fprintf('m3 aC3_ = [%6.3f,%6.3f,%g] (N)\n', m3*aC3_)
fprintf('Fin3_ = - m3 aC3 =[%6.3f,%6.3f,%d] (N)\n',Fin3_)
fprintf('IC3 = %6.3d (kg m^2)\n', IC3)
fprintf('IC3 alpha3_=[%d,%d,%6.3d](N m)\n',IC3*alpha3_)
fprintf('Min3_=-IC3 alpha3=[%d,%d,%6.3d](N m)\n',Min3_)
fprintf('\n')

% external moment on 3
Me = 100; % (N m)
Me_ = -sign(omega3_(3))*[0,0,Me];
fprintf('Me_ = [%d, %d, %6.3f] (N m)\n',Me_)
fprintf('\n')

% Newton-Euler eom for link 3
F23x=sym('F23x','real'); % force of 2 on 3 at C x-comp
F23y=sym('F23y','real'); % force of 2 on 3 at C y-comp
F23_=[F23x, F23y, 0]; % unknown joint force 
F03x=sym('F03x','real'); % force of 0 on 3 at D x-comp
F03y=sym('F03y','real'); % force of 0 on 3 at D y-comp
F03_=[F03x, F03y, 0]; % unknown joint force 

% sum F of 3 
% F23_+G3_+F03_ = m3*aC3_
eqF3_ = F23_+G3_+F03_-m3*aC3_;
eqF3x = eqF3_(1); % (1)
eqF3y = eqF3_(2); % (2)
% sum M of 3 wrt C3 
% C3C_xF23_+C3DxF03_+Me_=IC*alpha3_ 
eqM3C3_ = ...
  cross(rC_-rC3_,F23_)+cross(rD_-rC3_,F03_)...
  +Me_-IC3*alpha3_;
eqM3z = eqM3C3_(3); % (3)
  
% Newton-Euler eom for link 2
F12x=sym('F12x','real'); % force of 1 on 2 at B x-comp
F12y=sym('F12y','real'); % force of 1 on 2 at B y-comp
F12_=[F12x, F12y, 0]; % unknown joint force 

% sum F of 2 
% F12_+G2_+(-F23_) = m2*aC2_
eqF2_ = F12_+G2_+(-F23_)-m2*aC2_;
eqF2x = eqF2_(1); % (4)
eqF2y = eqF2_(2); % (5)
% sum M of 2 wrt C2 
% C2BxF12_+C2Cx(-F23_)=IC2*alpha2_ 
eqM2C2_=cross(rB_-rC2_,F12_)+cross(rC_-rC2_,-F23_)- IC2*alpha2_;
eqM2z = eqM2C2_(3); % (6)

% Eqs.(1)-(6) =>
sol23=solve(eqF3x,eqF3y,eqM3z,eqF2x,eqF2y,eqM2z);
F03xs=eval(sol23.F03x);
F03ys=eval(sol23.F03y);
F23xs=eval(sol23.F23x);
F23ys=eval(sol23.F23y);
F12xs=eval(sol23.F12x);
F12ys=eval(sol23.F12y);

F03s_=[F03xs, F03ys, 0];
F23s_=[F23xs, F23ys, 0];
F12s_=[F12xs, F12ys, 0];

fprintf('F03_ = [%6.3f,%6.3f,%g] (N)\n',F03s_)
fprintf('F23_ = [%6.3f,%6.3f,%g] (N)\n',F23s_)
fprintf('F12_ = [%6.3f,%6.3f,%g] (N)\n',F12s_)

% Newton-Euler eom for link 1
% sum M of 1 wrt A (A 'fixed' point)
% IA*alpha1_=rC1_xG1_)+rB_x(-F12s_)+Mm_ =>
Mm_=IA*alpha1_-(cross(rC1_,G1_)+cross(rB_,-F12s_));
fprintf('Mm_ = [%g,%g,%6.3f] (N m)\n',Mm_)

F21_=-F12s_;
% F01_+G1_+F21_ = m1*aC1_ =>
F01_ = m1*aC1_-(G1_+F21_);
fprintf('F01_ = [%6.3f,%6.3f,%g] (N)\n',F01_)

% eof

% Results 
% 
% phi = 60.000 (degrees) 
% rA_ = [ 0.000, 0.000,0] (m)
% rB_ = [ 0.075, 0.130,0] (m)
% rC_ = [ 0.424, 0.157,0] (m)
% rD_ = [ 0.300, 0.000,0] (m)
% phi2 = -175.567 (degrees) 
% phi3 = 231.701 (degrees) 
% 
% velocity and acceleration analysis 
% classical method 
% 
% omega1_ = [0,0, 6.283] (rad/s)
% alpha1_ = [0,0, 0.000] (rad/s^2)
% 
% vB_=vB1_=vB2_= [-0.816, 0.471,0 ] (m/s)
% aB_=aB1_=aB2_= [-2.961,-5.128,0 ] (m/s^2)
% 
% vC_=vB_ + omega2_ x rBC_ = vD_ + omega3_ x rDC_ => 
% x-axis: 0.157*omega3z - 0.0271*omega2z - 0.816 = 0 
% y-axis: 0.349*omega2z - 0.124*omega3z + 0.471 = 0 
% =>
% omega2z =  0.529 (rad/s)
% omega3z =  5.291 (rad/s)
% 
% omega2_ = [0,0, 0.529 ] (rad/s)
% omega3_ = [0,0, 5.291 ] (rad/s)
% 
% vC_ =  [-0.831, 0.656, 0.000] (m/s)
% 
% aC2_=aB_ + alpha2_ x rBC_ - (omega2_.omega2_)rBC_ 
% aC3_=aD_ + alpha3_ x rDC_ - (omega3_.omega3_)rDC_ 
% aC_ = aC2_ = aC3_ 
% x-axis: 0.157*alpha3z - 0.0271*alpha2z + 0.412 = 0 
% y-axis: 0.349*alpha2z - 0.124*alpha3z - 0.741 = 0 
% =>
% alpha2z =  1.270 (rad/s^2)
% alpha3z = -2.406 (rad/s^2)
% 
% alpha2_ = [0,0, 1.270 ] (rad/s^2)
% alpha3_ = [0,0,-2.406 ] (rad/s^2)
% 
% aC_ =  [-3.093,-4.693,0] (m/s^2)
% 
% rC1_ = [ 0.038, 0.065,0] (m)
% rC2_ = [ 0.249, 0.143,0] (m)
% rC3_ = [ 0.362, 0.078,0] (m)
% aC1_ = [-1.480,-2.564,0] (m/s^2)
% aC2_ = [-3.027,-4.911,0] (m/s^2)
% aC3_ = [-1.546,-2.346,0] (m/s^2)
% alpha1_ = [0,0, 0.000](rad/s^2)
% alpha2_ = [0,0, 1.270](rad/s^2)
% alpha3_ = [0,0,-2.406](rad/s^2)
% 
% 
% Dynamic force analysis 
% Link 1 
% m1 =  0.012 (kg)
% G1_ = - m1 g_ = [0,-0.118,0] (N)
% m1 aC1_ = [-0.018,-0.031,0] (N)
% Fin1_= - m1 aC1_ = [ 0.018, 0.031,0] (N)
% IC1 = 2.26e-05 (kg m^2)
% IA = 9.01e-05 (kg m^2)
% IC1 alpha1_=[ 0.000, 0.000,0](N m)
% Min1_=-IC1 alpha1_=[0, 0, 0](N m)
% 
% Link 2 
% m2 =  0.028 (kg)
% G2_ = - m2 g_ = [0,-0.275,0] (N)
% m2 aC2_ = [-0.085,-0.137,0] (N)
% Fin2_= - m2 aC2 =[ 0.085, 0.137,0] (N)
% IC2 = 2.861e-04 (kg m^2)
% IC2 alpha2_=[0,0,3.633e-04](N m)
% Min2_=-IC2 alpha2=[0,0,-3.633e-04](N m)
% 
% Link 3 
% m3 =  0.016 (kg)
% G3_ = - m3 g_ = [0, -0.157, 0] (N)
% m3 aC3_ = [-0.025,-0.038,0] (N)
% Fin3_ = - m3 aC3 =[ 0.025, 0.038,0] (N)
% IC3 = 5.347e-05 (kg m^2)
% IC3 alpha3_=[0,0,-1.286e-04](N m)
% Min3_=-IC3 alpha3=[0,0,1.286e-04](N m)
% 
% Me_ = [0, 0, -100.000] (N m)
% 
% F03_ = [678.766,52.818,0] (N)
% F23_ = [-678.791,-52.699,0] (N)
% F12_ = [-678.875,-52.561,0] (N)
% Mm_ = [0,0,84.251] (N m)
% F01_ = [-678.893,-52.474,0] (N)