% Velocity and acceleration analysis
% Derivative method

clear; clc; close all
fprintf('Results \n\n')
fprintf('Velocity and acceleration analysis \n') 
fprintf('Derivative method \n\n') 

AB = 0.10 ;  % (m)
AC = 0.05 ;  % (m)
CD = 0.15 ;  % (m)
% position angle of driver 1
phi1 = pi/6; % (rad)
% angular speed of driver 1
n = -50;     % (rpm) 
omega1 = pi*n/30; % (rad/s) angular velocity
alpha1 = 0;       % (rad/s^2) angular speed

% A(xA, yA, 0) origin
xA = 0; yA = 0; 
rA_ = [xA yA 0]; 

% C(xC, yC, 0) position of joint at C
xC = AC ; yC = 0; 
rC_ = [xC yC 0]; 

syms  phi(t)

% phi(t) the angle of the diver link with the horizontal
% phi(t) is a function of time, t
% position of joint B 
xB = AB*cos(phi(t));
yB = AB*sin(phi(t));
% position vector of B in terms of phi(t) - symbolic
rB_ = [xB yB 0];

% subs(S,OLD,NEW) replaces OLD with NEW in the symbolic expression S   
xBn = subs(xB, phi(t), phi1); % xB for phi(t)=phi1=pi/6
yBn = subs(yB, phi(t), phi1); % yB for phi(t)=phi1=pi/6
rBn_ = subs(rB_, phi(t), phi1); % rB_ for phi(t)=phi1=pi/6
fprintf('rB_ =  [ %6.3f, %6.3f, %g ] (m)\n', rBn_)
% rB_ =  [  0.087,  0.050, 0 ] (m)

% vB_=vB1_=vB2_ in terms of phi(t) and diff(phi(t),t)
vB_ = diff(rB_, t); % velocity of B1=B2
% diff(S,t) differentiates S with respect to t

% calculate numerical value of vB for phi(t)=pi/6 and phi'(t)=omega
% list for symbolical variables  phi''(t), phi'(t), phi(t) 
slist = {diff(phi(t),t,2), diff(phi(t),t), phi(t)};
% list for numerical values 
nlist = {alpha1, omega1, phi1}; % numerical values for slist
% diff(phi(t), t, 2) -> aplha1 = 0
% diff(phi(t), t) -> omega1
% phi(t) -> phi1 = pi/6

vBn_ = subs(vB_, slist, nlist); % replaces slist with nlist in vB_
fprintf('vB1_ = vB2_ = [ %6.3f, %6.3f, %g ] (m/s)\n', vBn_) 
% norm(v_) is the magnitude of a vector v_
vBn = norm(vBn_);
fprintf('|vB1| = |vB2| = %6.3f (m/s) \n', vBn) 

% acceleration of B1=B2
aB_  = diff(vB_, t);
% numerical value for aB
aBn_ = subs(aB_,slist,nlist); 
fprintf('aB1_ = aB2_ = [ %6.3f, %6.3f, %g ] (m/s^2)\n', aBn_)  
aBn = norm(aBn_);
fprintf('|aB1| = |aB2| = %6.3f (m/s^2) \n', aBn) 
fprintf('\n') 

% angular velocities and accelerations
% Link 2
phi2 = atan((yB-yC)/(xB-xC));  % phi2 in terms of phi(t)
phi2n = subs(phi2, phi(t), phi1); % phi2 for phi(t)=pi/6
fprintf('phi2 = phi3 = %6.3f (degrees) \n', phi2n*180/pi) 

% omega2 in terms of phi(t) and diff('phi(t)',t)
dphi2 = diff(phi2, t);  
dphi2nn = subs(dphi2, diff(phi(t), t), omega1);
% numerical value for omega2
dphi2n  = subs(dphi2nn, phi(t), phi1);  
fprintf('omega2 = omega3 = %6.3f (rad/s) \n', dphi2n) 

% alpha2 in terms of phi(t), diff('phi(t)',t), and diff('phi(t)',t,2)
ddphi2  = diff(dphi2, t); 
% numerical value for alpha2
ddphi2n = subs(ddphi2, slist, nlist); 
fprintf('alpha2 = alpha3 = %6.3f (rad/s^2) \n', ddphi2n) 
fprintf('\n') 

% position of joint D 
syms xDsol yDsol
eqnD1 = (xDsol - xC)^2 + (yDsol - yC)^2-CD^2 ;
eqnD2 = (yB-yC)/(xB-xC)-(yDsol-yC)/(xDsol-xC);
solD = solve(eqnD1, eqnD2, xDsol, yDsol);
xDpositions = solD.xDsol; 
yDpositions = solD.yDsol;
xD1 = xDpositions(1); xD2 = xDpositions(2);
yD1 = yDpositions(1); yD2 = yDpositions(2);
% select the correct position for D for the given input angle
xD1n = subs(xD1, phi(t), phi1); % xD1 for phi(t)=pi/6
if xD1n > xC
    xD = xD1; yD = yD1;
else
    xD = xD2; yD = yD2;
end
% position vector of D in term of phi(t) - symbolic
rD_ = [ xD yD 0 ]; 
xDn = subs(xD, phi(t), phi1); % xD for phi(t)=pi/6
yDn = subs(yD, phi(t), phi1); % yD for phi(t)=pi/6
rDn_ = [ xDn yDn 0 ]; % rD_ for phi(t)=pi/6
fprintf('rD_ =  [ %6.3f, %6.3f, %g ] (m)\n', rDn_) 

% velocity of D
% vD_ in terms of phi(t) and diff(phi(t),t)
vD_  = diff(rD_,t); 
% numerical value for vD_
vDn = subs(vD_, slist, nlist); 
fprintf('vD_ = [ %6.3f, %6.3f, %g ] (m/s)\n', vDn) 
fprintf('|vD| = %6.3f (m/s) \n', norm(vDn)) 

% acceleration of D_
% aD in terms of phi(t), diff(phi(t),t), and diff(phi(t),t,2)
aD_  = diff(vD_, t); 
% numerical value for aD_
aDn_ = subs(aD_, slist, nlist); 
fprintf('aD_ = [ %6.3f, %6.3f, %g ] (m/s^2)\n', aDn_)  
fprintf('|aD| = %6.3f (m/s^2) \n', norm(aDn_)) 
fprintf('\n') 

plot([xA,xBn],[yA,yBn],'k',[xBn,xDn],[yBn,yDn],'b',...
     [xC,xDn],[yC,yDn],'r') 
text(xA,yA,'  A'), text(xBn,yBn,'  B'), text(xC, yC,'  C'),...
text(xDn,yDn,'  D'), grid

% Results 
% 
% Velocity and acceleration analysis 
% Derivative method 
% 
% rB_ =  [  0.087,  0.050, 0 ] (m)
% vB1_ = vB2_ = [  0.262, -0.453, 0 ] (m/s)
% |vB1| = |vB2| =  0.524 (m/s) 
% aB1_ = aB2_ = [ -2.374, -1.371, 0 ] (m/s^2)
% |aB1| = |aB2| =  2.742 (m/s^2) 
% 
% phi2 = phi3 = 53.794 (degrees) 
% omega2 = omega3 = -7.732 (rad/s) 
% alpha2 = alpha3 = -34.865 (rad/s^2) 
% 
% rD_ =  [  0.139,  0.121, 0 ] (m)
% vD_ = [  0.936, -0.685, 0 ] (m/s)
% |vD| =  1.160 (m/s) 
% aD_ = [ -1.077, -10.324, 0 ] (m/s^2)
% |aD| = 10.380 (m/s^2) 

