% Program 3.5
% R-RTR-RTR
% Velocity and acceleration analysis
% Derivative method
clear; clc; close;
fprintf('Results \n') ; fprintf('\n');
fprintf('Velocity and acceleration analysis \n');
fprintf('Derivative method \n'); fprintf('\n');
AB = 0.14 ; AC = 0.06 ; AE = 0.25 ; CD = 0.15 ; % (m)
xA = 0; yA = 0; rA = [xA yA 0]; 
xC = 0 ; yC = AC ; rC = [xC yC 0]; 
xE = 0 ; yE = -AE ; rE = [xE yE 0];  

n = 50 ; % rpm of the driver link (constant)
omega = n*pi/30;  % rad/s (omega=constant)

% sym  constructs symbolic numbers, variables and objects
t = sym('t','real');
% 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(sym('phi(t)'));
yB = AB*sin(sym('phi(t)'));
rB = [xB yB 0]; % position vector of B in terms of phi(t) - symbolic

% subs(S,OLD,NEW) replaces OLD with NEW in the symbolic expression S   
xBn = subs(xB,'phi(t)',pi/6); % xB for phi(t)=pi/6
yBn = subs(yB,'phi(t)',pi/6); % yB for phi(t)=pi/6
rBn = subs(rB,'phi(t)',pi/6); % rB for phi(t)=pi/6
fprintf('rB =  [ %g, %g, %g ] (m)\n', rBn);

% velocity of B1=B2
% diff(S,t) differentiates S with respect to t
vB = diff(rB,t); % vB1=vB2 in terms of phi(t) and diff(phi(t),t)

% calculates numerical value of vB for phi(t)=pi/6 and phi'(t)=omega
% creates a list ,slist, for the symbolical variables 
% phi''(t), phi'(t), phi(t) 
slist = {diff('phi(t)',t,2), diff('phi(t)',t), 'phi(t)'};
% creates a list, nlist, with the numerical values for slist
nlist = {0, omega, pi/6}; % numerical values for slist
% diff('phi(t)',t,2) -> 0
% diff('phi(t)',t) -> omega
% 'phi(t)' -> pi/6

vBn = subs(vB,slist,nlist); % replaces slist with nlist in vB
% double(S) converts the symbolic object S to a numeric object
VB  = double(vBn); % converts the symbolic object vBn  to a numeric object
fprintf('vB1 = vB2 = [ %g, %g, %g ] (m/s)\n', VB);

% acceleration of B1=B2
aB  = diff(vB,t);
aBn = double(subs(aB,slist,nlist)); % numerical value for aB
fprintf('aB1 = aB2 = [ %g, %g, %g ] (m/s^2)\n', aBn); fprintf('\n');

% position of joint D 
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 = eval(solD.xDsol); yDpositions = eval(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)',pi/6); % xD1 for phi(t)=pi/6
if xD1n < xC
    xD = xD1; yD = yD1;
else
    xD = xD2; yD = yD2;
end
rD = [ xD yD 0 ]; % position vector of D in term of phi(t) - symbolic
xDn = subs(xD,'phi(t)',pi/6); % xD for phi(t)=pi/6
yDn = subs(yD,'phi(t)',pi/6); % yD for phi(t)=pi/6
rDn = [ xDn yDn 0 ]; % rD for phi(t)=pi/6
fprintf('rD =  [ %g, %g, %g ] (m)\n', rDn);

% velocity of D3=D4
vD  = diff(rD,t); % vD in terms of phi(t) and diff('phi(t)',t)
vDn = double(subs(vD,slist,nlist)); % numerical value for vD
fprintf('vD3 = vD4 = [ %g, %g, %g ] (m/s)\n', vDn);

% acceleration of D3=D4
% aD in terms of phi(t), diff('phi(t)',t), and diff('phi(t)',t,2)
aD  = diff(vD,t); 
aDn = double(subs(aD,slist,nlist)); % numerical value for aD
fprintf('aD3 = aD4 = [ %g, %g, %g ] (m/s^2)\n', aDn); 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)',pi/6); % phi2 for phi(t)=pi/6
fprintf('phi2 = phi3 = %g (degrees) \n', phi2n*180/pi);

dphi2 = diff(phi2,t);  % omega2 in terms of phi(t) and diff('phi(t)',t)
dphi2nn = subs(dphi2,diff('phi(t)',t),omega);
dphi2n  = subs(dphi2nn,'phi(t)',pi/6);  % numerical value for omega2
fprintf('omega2 = omega3 = %g (rad/s) \n', dphi2n);

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

% Link 4
phi4  = atan((yD-yE)/(xD-xE))+pi; % phi4 in terms of phi(t)
phi4n = subs(phi4,'phi(t)',pi/6); % numerical value for phi4
fprintf('phi4 = phi5 = %g (degrees) \n', phi4n*180/pi);

dphi4  = diff(phi4,t);
dphi4n = double(subs(dphi4,slist,nlist)); % numerical value for omega4
fprintf('omega4 = omega5 = %g (rad/s) \n', dphi4n);

ddphi4  = diff(dphi4,t);
ddphi4n = double(subs(ddphi4,slist,nlist)); % numerical value for alpha4
fprintf('alpha4 = alpha5 = %g (rad/s^2) \n', ddphi4n);

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