% Program 3.7
% R-RTR-RRT
% 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.1 ; CD = 0.075 ; DE = 0.2 ; AC = 0.15 ; % (m)
xC = 0 ; yC = 0 ; xA = 0 ; yA = AC ; 
phi1 = pi/4 ; omega = 4.712 ; alpha = 0;

t = sym('t','real');
xB1 = xA + AB*cos(sym('phi(t)'));
yB1 = yA + AB*sin(sym('phi(t)'));
rB = [ xB1 yB1 0 ]; % position vector of B function of phi(t) - symbolic
 
xBn = subs(xB1,'phi(t)',pi/4); % xB for phi(t)=pi/4
yBn = subs(yB1,'phi(t)',pi/4); % yB for phi(t)=pi/4
rBn = subs(rB,'phi(t)',pi/4);  % rB for phi(t)=pi/4
fprintf('rB =  [ %g, %g, %g ] (m)\n', rBn);

% velocity of B1=B2
vB = diff(rB,t); % differentiates rB with respect to t

% list for the symbolical variables phi''(t), phi'(t), phi(t) 
slist = {diff('phi(t)',t,2), diff('phi(t)',t), 'phi(t)'};
% list with the numerical values for phi''(t), phi'(t), phi(t) 
nlist = {alpha,  omega, phi1}; % numerical values for slist
vBn = double(subs(vB,slist,nlist)); 
fprintf('vB1 = vB2 = [ %g, %g, %g ] (m/s)\n', vBn);
% acceleration of B1=B2
aB  = diff(vB,t); % differentiates vB with respect to t
aBn = double(subs(aB,slist,nlist));
fprintf('aB1 = aB2 = [ %g, %g, %g ] (m/s^2)\n', aBn); fprintf('\n');

% joint D
xB = sym('xB(t)'); % xB(t) symbolic
yB = sym('yB(t)'); % yB(t) symbolic

% list for the symbolical variables of B
% xB''(t), yB''(t), xB'(t), yB'(t), xB(t), yB(t)
sB={diff('xB(t)',t,2),diff('yB(t)',t,2),...
    diff('xB(t)',t),diff('yB(t)',t),'xB(t)','yB(t)'};
% three dots (...) are used whenever a line break is needed
% list with the numerical values for the sB list
nB={aBn(1),aBn(2),vBn(1),vBn(2),xBn,yBn};

xD = sym('xD(t)'); % xB(t) symbolic
yD = sym('yD(t)'); % xB(t) symbolic

% equations for xD(t) and yD(t) function of xB(t) and yB(t)
eqD1 = (xD - xC)^2 + (yD - yC)^2 - CD^2;
eqD2 = (yD - yC)/(xD - xC) - (yB - yC)/(xB - xC);

sDp = {'xD(t)','yD(t)'};
nDp = {'xDn','yDn'};

% replaces {'xD(t)','yD(t)'} with {'xDn','yDn'}  
eqD1n = subs(eqD1,sDp,nDp);  
eqD2n = subs(eqD2,sDp,nDp);

eqD1p = subs(eqD1n,sB,nB); % equation is function of only 'xDn' and 'yDn'
eqD2p = subs(eqD2n,sB,nB); % equation is function of only 'xDn' and 'yDn'

solDp = solve(eqD1p, eqD2p);
xDpositions = eval(solDp.xDn); 
yDpositions = eval(solDp.yDn);
xD1 = xDpositions(1); xD2 = xDpositions(2);
yD1 = yDpositions(1); yD2 = yDpositions(2);

if yD1 < 0
    xDp = xD1; yDp = yD1;
else
    xDp = xD2; yDp = yD2;
end
rD = [xDp yDp 0];
fprintf('rD =  [ %g, %g, %g ] (m)\n', rD);

% velocity of D
deqD1 = diff(eqD1,t);
deqD2 = diff(eqD2,t);

sDv = {diff('xD(t)',t),diff('yD(t)',t),'xD(t)','yD(t)'};
nDv = {'vxD','vyD',xDp,yDp};

deqD1v = subs(subs(deqD1,sDv,nDv),sB,nB);
deqD2v = subs(subs(deqD2,sDv,nDv),sB,nB);

solvD = solve(deqD1v, deqD2v);
vDx = eval(solvD.vxD);
vDy = eval(solvD.vyD);
fprintf('vD = [ %g, %g, %g ] (m/s)\n', [vDx vDy 0]);

fDv = {vDx, vDy, xDp, yDp};

% acceleration of D
ddeqD1 = diff(deqD1,t);
ddeqD2 = diff(deqD2,t);

sDa = {diff('xD(t)',t,2),diff('yD(t)',t,2)};
nDa = {'axD','ayD'};

ddeqD1n = subs(ddeqD1,sDa,nDa);
ddeqD2n = subs(ddeqD2,sDa,nDa);

ddeqD1a = subs(subs(ddeqD1n,sDv,fDv),sB,nB);
ddeqD2a = subs(subs(ddeqD2n,sDv,fDv),sB,nB);

solaD = solve(ddeqD1a, ddeqD2a);
aDx = eval(solaD.axD);
aDy = eval(solaD.ayD);
fprintf('aD = [ %g, %g, %g ] (m/s^2)\n', [aDx aDy 0]); fprintf('\n');

sD={diff('xD(t)',t,2),diff('yD(t)',t,2),...
    diff('xD(t)',t),diff('yD(t)',t),'xD(t)','yD(t)'};
nD={aDx,aDy,vDx,vDy,xDp,yDp};

% joint E
xE = sym('xE(t)'); 
yE = 0;
% equations for xE(t) function of xD(t) and yD(t)
eqE = (xE - xD)^2 + (yE - yD)^2 - DE^2;

eqEp = subs(subs(eqE,'xE(t)','xEn'),sD,nD);
solEp = solve(eqEp);
xE1 = eval(solEp(1)); xE2 = eval(solEp(2));
if xE1 > xDp xEp = xE1; else xEp = xE2; end
rE = [xEp yE 0];
fprintf('rE =  [ %g, %g, %g ] (m)\n', rE);

% velocity of E
deqE = diff(eqE,t);
sEv = {diff('xE(t)',t),'xE(t)'};
nEv = {'vxE',xEp};
deqEv = subs(subs(deqE,sEv,nEv),sD,nD);
solvE = solve(deqEv);
vEx = eval(solvE);
vEy = 0;
fprintf('vE = [ %g, %g, %g ] (m/s)\n', [vEx vEy 0]);
fEv = {vEx,xEp};

% acceleration of E
ddeqE = diff(deqE,t);
ddeqEn = subs(ddeqE,diff('xE(t)',t,2),'axD');
ddeqEa = subs(subs(ddeqEn,sEv,fEv),sD,nD);
solaE = solve(ddeqEa);
aEx = eval(solaE);
aEy = 0;
fprintf('aE = [ %g, %g, %g ] (m/s^2)\n', [aEx aEy 0]); fprintf('\n');

sE={diff('xE(t)',t,2),diff('xE(t)',t),'xE(t)'};
nE={aEx,vEx,xEp};

% angular velocity and acceleration of links 2 and 3
phi3 = atan((yB-yC)/(xB-xC)); 
phi3n = subs(phi3,sB,nB);
fprintf('phi2 = phi3 = %g (degrees) \n', double(phi3n*180/pi));

dphi3 = diff(phi3,t);  
dphi3n = subs(dphi3,sB,nB);
fprintf('omega2 = omega3 =  %g (rad/s) \n', double(dphi3n));

ddphi3 = diff(dphi3,t); 
ddphi3n = subs(ddphi3,sB,nB) ;
fprintf('alpha2 = alpha3 = %g (rad/s^2) \n', double(ddphi3n)); fprintf('\n');

% angular velocity and acceleration of link 4
phi4 = atan((yE-yD)/(xE-xD)); 
phi4n = subs(subs(phi4,sD,nD),sE,nE);
fprintf('phi4 = %g (degrees) \n', double(phi4n*180/pi));

dphi4 = diff(phi4,t);  
dphi4n = subs(subs(dphi4,sD,nD),sE,nE);
fprintf('omega4 =  %g (rad/s) \n', double(dphi4n));

ddphi4 = diff(dphi4,t); 
ddphi4n = subs(subs(ddphi4,sD,nD),sE,nE);
fprintf('alpha4 = %g (rad/s^2) \n', double(ddphi4n)); 

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