% Program 3.4
% R-RTR-RTR
% Velocity and acceleration analysis

clear; clc; close;

AB = 0.14 ; AC = 0.06 ; AE = 0.25 ; CD = 0.15 ; DF=0.4; EG=0.5; % (m)
phi_input = 30; phi = phi_input*(pi/180); 
xA = 0; yA = 0; rA = [xA yA 0]; 
xC = 0 ; yC = AC ; rC = [xC yC 0]; 
xE = 0 ; yE = -AE ; rE = [xE yE 0];  

xB = AB*cos(phi); yB = AB*sin(phi); rB = [xB yB 0]; 

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);
if (phi>=0 && phi<=pi/2)||(phi >= 3*pi/2 && phi<=2*pi)
   if xD1 <= xC xD = xD1; yD=yD1; else xD = xD2; yD=yD2; end
   else
   if xD1 >= xC xD = xD1; yD=yD1; else xD = xD2; yD=yD2; end
end 
rD = [xD yD 0]; 

phi2 = atan((yB-yC)/(xB-xC)); phi3 = phi2;
phi4 = atan((yD-yE)/(xD-xE))+pi; phi5 = phi4;
xF = xD + DF*cos(phi3) ; yF = yD + DF*sin(phi3) ; rF = [xF yF 0]; 
xG = xE + EG*cos(phi5) ; yG = yE + EG*sin(phi5) ; rG = [xG yG 0]; 

fprintf('Results \n') ; fprintf('\n');

fprintf('rA=[%g, %g, %g] (m)\n', rA); fprintf('rC=[%g, %g, %g] (m)\n', rC);
fprintf('rE=[%g, %g, %g] (m)\n', rE); fprintf('rB=[%g, %g, %g] (m)\n', rB);
fprintf('rD=[%g, %g, %g] (m)\n', rD); fprintf('rF=[%g, %g, %g] (m)\n', rF);
fprintf('rG=[%g, %g, %g] (m)\n', rG); 
fprintf('phi2=phi3=%g(degrees)\n',phi2*180/pi);
fprintf('phi4=phi5=%g(degrees)\n',phi4*180/pi);
plot([xA,xB],[yA,yB],'r-o',[xD,xF],[yD,yF],'b-o',[xE,xG],[yE,yG],'g-o') 
xlabel('x (m)'),ylabel('y (m)'), title('positions for \phi = 30 (deg)'),...
text(xA,yA,' A'), text(xB,yB,' B'), text(xC,yC,' C'), text(xD,yD,'  D'),...
text(xE,yE,' E'), text(xF,yF,' F'), text(xG,yG,' G'), grid

fprintf('\n');
fprintf('Velocity and acceleration analysis \n'); fprintf('\n');

n = 50.; omega1 = [ 0 0 pi*n/30 ]; alpha1 = [0 0 0 ]; 
fprintf('omega1 =  [ %g, %g, %g ] (rad/s)\n', omega1);
fprintf('alpha1 =  [ %g, %g, %g ] (rad/s^2)\n', alpha1); fprintf('\n');

vA = [0 0 0 ]; aA = [0 0 0 ];

vB1 = vA + cross(omega1,rB); vB2 = vB1; 
aB1 = aA + cross(alpha1,rB) - dot(omega1,omega1)*rB;

fprintf('vB = vB1 = vB2 = [ %g, %g, %g ] (m/s)\n', vB1); aB2 = aB1;
fprintf('aB = aB1 = aB2 = [ %g, %g, %g ] (m/s^2)\n', aB1); fprintf('\n');

omega3z=sym('omega3z','real'); vB32=sym('vB32','real');

omega3 = [ 0 0 omega3z ]; % omega3z unknown (to be calculated)

vC = [0 0 0 ]; % C is fixed
% vB3 = vC + omega3 x rCB   (B3 & C are points on link 3)
vB3 = vC + cross(omega3,rB-rC); 
% point B2 is on link 2 and point B3 is on link 3
% vB3 = vB2 + vB3B2
% between the links 2 and 3 there is a translational joint B_T
% vB3B2 is the relative velocity of B3 wrt 2
% vB3B2 is parallel to the sliding direcion BC
% vB3B2 is written as a vector
vB3B2 = vB32*[ cos(phi2) sin(phi2) 0]; 
% vB32 unknown scalar (to be calculated)
eqvB = vB3 - vB2 - vB3B2; % vB3 = vB2 + vB3B2
% vectorial equation
eqvBx = eqvB(1); % the component of the vectorial equation on x-axis
eqvBy = eqvB(2); % the component of the vectorial equation on y-axis
% two equations eqvBx and eqvBy with two unknowns omega3z and vB32
solvB = solve(eqvBx,eqvBy); % solve for omega3z and vB32 
omega3zs=eval(solvB.omega3z); vB32s=eval(solvB.vB32);

Omega3 = [0 0 omega3zs]; Omega2 = Omega3;
VB32 = vB32s*[cos(phi2) sin(phi2) 0];

% print the equations for calculating omega3 and vB32
fprintf('vB3 = vC + omega3 x rCB = vB2 + vB3B2 => \n');
qvBx=vpa(eqvBx,6);
fprintf('x-axis: %s = 0 \n', char(qvBx));
qvBy=vpa(eqvBy,6);
fprintf('y-axis: %s = 0 \n', char(qvBy));
fprintf('=>\n');
fprintf('omega3z =  %g (rad/s)\n', omega3zs);
fprintf('vB32 =  %g (m/s)\n', vB32s);
fprintf('\n');
fprintf('omega2 = omega3 = [ %g, %g, %g ] (rad/s)\n', Omega3 );
fprintf('vB3B2 =  [ %g, %g, %g ] (m/s)\n', VB32 ); fprintf('\n');

aB3B2cor = 2*cross(Omega3,VB32); % Coriolis acceleration

alpha3z=sym('alpha3z','real'); aB32=sym('aB32','real');

alpha3 = [ 0 0 alpha3z ]; % alpha3z unknown
aC = [0 0 0 ]; % C is fixed
% aB3 acceleration of B3
aB3 = aC + cross(alpha3,rB-rC) - dot(Omega3,Omega3)*(rB-rC); 
% aB3B2 relative velocity of B3 wrt 2
% aB3B2 parallel to the sliding direcion BC
aB3B2 = aB32*[ cos(phi2) sin(phi2) 0]; % aB32 unknown
eqaB = aB3 - aB2 - aB3B2 - aB3B2cor; % aB3 = aB2 + aB3B2 + aB3B2cor
% vectorial equation
eqaBx = eqaB(1); % equation component on x-axis
eqaBy = eqaB(2); % equation component on y-axis
solaB = solve(eqaBx,eqaBy);
alpha3zs=eval(solaB.alpha3z);
aB32s=eval(solaB.aB32);
Alpha3 = [0 0 alpha3zs];
Alpha2 = Alpha3;
AB32 = aB32s*[cos(phi2) sin(phi2) 0];

% print the equations for calculating alpha3 and aB32
fprintf('aB32cor =  [ %g, %g, %g ] (m/s^2)\n', aB3B2cor ); fprintf('\n');
fprintf('aB3=aC+omega3xrCB-(omega3.omega3)rCB=aB2+aB3B2+aB3B2cor =>\n');
qaBx=vpa(eqaBx,6);
fprintf('x-axis: %s = 0 \n', char(qaBx));
qaBy=vpa(eqaBy,6);
fprintf('y-axis: %s = 0 \n', char(qaBy));
fprintf('=>\n');
fprintf('alpha3z =  %g (rad/s)\n', alpha3zs);
fprintf('aB32  =  %g (m/s)\n', aB32s);
fprintf('\n');
fprintf('alpha2 = alpha3 = [ %g, %g, %g ] (rad/s^2)\n', Alpha3 );
fprintf('aB3B2 =  [ %g, %g, %g ] (m/s^2)\n', AB32 );
fprintf('\n');

% vD3 velocity of D3
vD3 = vC + cross(Omega3,rD-rC); % D3 & C points on link 3
vD4 = vD3;
fprintf('vD3 = vD4 = [ %g, %g, %g ] (m/s)\n', vD3 );
% aD3 acceleration of D3
aD3 = aC + cross(Alpha3,rD-rC) - dot(Omega3,Omega3)*(rD-rC); 
aD4 = aD3;
fprintf('aB3 = aD4 = [ %g, %g, %g ] (m/s^2)\n', aD3 );
fprintf('\n');

omega5z=sym('omega5z','real');
vD54=sym('vD54','real');
omega5 = [ 0 0 omega5z ]; % omega5z unknown
vE = [0 0 0 ]; 
% vD5 velocity of D5
vD5 = vE + cross(omega5,rD-rE); % D5 & E points on link 5
% vD5D4 relative velocity of D5 wrt 4
% vD5D4 parallel to the sliding direcion DE
vD5D4 = vD54*[ cos(phi5) sin(phi5) 0]; % vD54 unknown
eqvD = vD5 - vD4 - vD5D4; % vD5 = vD4 + vD5D4
% vectorial equation
eqvDx = eqvD(1); % component on x-axis
eqvDy = eqvD(2); % component on y-axis
solvD = solve(eqvDx,eqvDy);
omega5zs=eval(solvD.omega5z);
vD54s=eval(solvD.vD54);
Omega5 = [0 0 omega5zs];
Omega4 = Omega5;
VD54 = vD54s*[cos(phi5) sin(phi5) 0];

% print the equations for calculating omega5 and vD54
fprintf('vD5 = vE + omega5 x rED = vD4 + vD5D4 => \n');
qvDx=vpa(eqvDx,6);
fprintf('x-axis: %s = 0 \n', char(qvDx));
qvDy=vpa(eqvDy,6);
fprintf('y-axis: %s = 0 \n', char(qvDy));
fprintf('=>\n');
fprintf('omega5z =  %g (rad/s)\n', omega5zs);
fprintf('vD54 =  %g (m/s)\n', vD54s);
fprintf('\n');
fprintf('omega4 = omega5 = [ %g, %g, %g ] (rad/s)\n', Omega5 );
fprintf('vD5D4 =  [ %g, %g, %g] (m/s)\n', VD54 ); 
fprintf('\n');

aD5D4cor = 2*cross(Omega5,VD54); % Coriolis acceleration

alpha5z=sym('alpha5z','real');
aD54=sym('aD54','real');
alpha5 = [ 0 0 alpha5z ]; % alpha5z unknown
aE = [0 0 0 ]; 
% aD5 acceleration of D5
aD5 = aE + cross(alpha5,rD-rE) - dot(Omega5,Omega5)*(rD-rE); 
% aD5D4 relative velocity of B5 wrt 4
% aD5D4 parallel to the sliding direcion DE
aD5D4 = aD54*[ cos(phi5) sin(phi5) 0]; % aD54 unknown
eqaD = aD5 - aD4 - aD5D4 - aD5D4cor; % aB5 = aB4 + aD5D4+ aD5D4cor
% vectorial equation
eqaDx = eqaD(1); % component on x-axis
eqaDy = eqaD(2); % component on y-axis
solaD = solve(eqaDx,eqaDy);
alpha5zs=eval(solaD.alpha5z);
aD54s=eval(solaD.aD54);
Alpha5 = [0 0 alpha5zs];
Alpha4 = Alpha5;
AD54 = aD54s*[cos(phi5) sin(phi5) 0];

% print the equations for calculating alpha5 and aD54
fprintf('aD54cor =  [ %g, %g, %g ] (m/s^2)\n', aD5D4cor ); fprintf('\n');
fprintf('aD5=aE+omega5xrED-(omega5.omega5)rED=aD4+aD5D4+aD5D4cor =>\n');
qaDx=vpa(eqaDx,6);
fprintf('x-axis: %s = 0 \n', char(qaDx));
qaDy=vpa(eqaDy,6);
fprintf('y-axis: %s = 0 \n', char(qaDy));
fprintf('=>\n');
fprintf('alpha5z =  %g (rad/s^2)\n', alpha5zs);
fprintf('aD54  =  %g (m/s^2)\n', aD54s);
fprintf('\n');
fprintf('alpha4 = alpha5 = [ %g, %g, %g ] (rad/s^2)\n', Alpha5 );
fprintf('aD54 =  [ %g, %g, %g ] (m/s^2)\n', AD54 );
fprintf('\n');