% Program 4.7
% R-RTR-RTR
% Dynamic force analysis 
% Contour method

fprintf('R-RTR-RTR  \n');
fprintf('Dynamic force analysis \n');
fprintf('Contour method \n');
fprintf('Results  \n');
fprintf('  \n'); 

clear 
clc
close

AB = 0.14; AC = 0.06; AE = 0.25; CD = 0.15; DF=0.4; EG=0.5;
phi = 30*(pi/180); 
fprintf('phi = phi1 = %g (degrees) \n', phi*180/pi); fprintf('\n');
fprintf('Position analysis \n');
fprintf('\n');
xA = 0; yA = 0; rA = [xA yA 0]; fprintf('rA = [ %g, %g, %g ] (m)\n', rA);
xC = 0; yC = AC; rC = [xC yC 0]; fprintf('rC = [ %g, %g, %g ] (m)\n', rC);
xE = 0; yE = -AE; rE = [xE yE 0]; fprintf('rE = [ %g, %g, %g ] (m)\n', rE);
xB = AB*cos(phi); yB = AB*sin(phi); rB = [xB yB 0];  
fprintf('rB = [ %g, %g, %g] (m)\n', rB);
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]; fprintf('rD = [ %g, %g, %g ] (m)\n', rD);
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]; 
fprintf('rF = [ %g, %g, %g ] (m)\n', rF);
xG = xE + EG*cos(phi5); yG = yE + EG*sin(phi5); rG = [xG yG 0]; 
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);
fprintf('\n');
xC1 = xB/2; yC1 = yB/2; rC1 = [xC1 yC1 0]; 
fprintf('rC1 = [ %g, %g, %g ] (m)\n', rC1);
rC2 = rB;  fprintf('rC2 = rB = [ %g, %g, %g ] (m)\n', rC2);
xC3 = (xD+xF)/2; yC3 = (yD+yF)/2; rC3 = [xC3 yC3 0]; 
fprintf('rC3 = [ %g, %g, %g ] (m)\n', rC3);
rC4 = rD; fprintf('rC4 = rD = [ %g, %g, %g ] (m)\n', rC4);
xC5 = (xE+xG)/2; yC5 = (yE+yG)/2; rC5 = [xC5 yC5 0]; 
fprintf('rC5 = [ %g, %g, %g ] (m)\n', rC5);

% Graphic of the mechanism
plot([0,xB],[0,yB],'r-o',[xD,xC],[yD,yC],'b-o',[xC,xF],[yC,yF],'b-o'),...
hold on
plot([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=C2'), text(xC,yC,'  C'),
text(xD,yD,'  D=C4'), text(xE,yE,'  E'), text(xF,yF,'  F'), 
text(xG,yG,'  G'), text(xC1,yC1,'  C1'), text(xC3,yC3,'  C3'),
text(xC5,yC5,'  C5'), axis([-0.3 0.3 -0.3 0.3]), grid on,

fprintf('\n'); 
fprintf('Velocity and acceleration analysis \n'); fprintf('\n');
n = 50.; 
omega1 = [ 0 0 pi*n/30 ]; alpha1 = [0 0 0 ]; 
vA = [0 0 0 ]; aA = [0 0 0 ]; 
vB1 = vA + cross(omega1,rB); vB2 = vB1; 
aB1 = aA + cross(alpha1,rB) - dot(omega1,omega1)*rB; aB2 = aB1;
fprintf('aB1 = aB2 = [ %g, %g, %g ] (m/s^2)\n', aB1); 
omega3z=sym('omega3z','real'); alpha3z=sym('alpha3z','real');
vB32=sym('vB32','real'); aB32=sym('aB32','real');
omega3 = [ 0 0 omega3z ]; 
vC = [0 0 0 ]; 
vB3 = vC + cross(omega3,rB-rC); 
vB3B2 = vB32*[ cos(phi2) sin(phi2) 0]; 
eqvB = vB3 - vB2 - vB3B2; eqvBx = eqvB(1); eqvBy = eqvB(2); 
solvB = solve(eqvBx,eqvBy);
omega3zs=eval(solvB.omega3z); vB32s=eval(solvB.vB32);
Omega3 = [0 0 omega3zs]; Omega2 = Omega3;
v32 = vB32s*[cos(phi2) sin(phi2) 0];
vD3 = vC + cross(Omega3,rD-rC); vD4 = vD3;
aB3B2cor = 2*cross(Omega3,v32); 
alpha3 = [ 0 0 alpha3z ]; 
aC = [0 0 0 ]; 
aB3 = aC + cross(alpha3,rB-rC) - dot(Omega3,Omega3)*(rB-rC); 
aB3B2 = aB32*[ cos(phi2) sin(phi2) 0]; 
eqaB = aB3 - aB2 - aB3B2 - aB3B2cor; eqaBx = eqaB(1); eqaBy = eqaB(2); 
solaB = solve(eqaBx,eqaBy);
alpha3zs=eval(solaB.alpha3z); aB32s=eval(solaB.aB32);
Alpha3 = [0 0 alpha3zs]; Alpha2 = Alpha3;
aD3 = aC + cross(Alpha3,rD-rC) - dot(Omega3,Omega3)*(rD-rC); aD4 = aD3;
fprintf('aD3 = aD4 = [ %g, %g, %g ] (m/s^2)\n', aD3 );
omega5z=sym('omega5z','real'); alpha5z=sym('alpha5z','real');
vD54=sym('vD54','real'); aD54=sym('aD54','real');
omega5 = [ 0 0 omega5z ];
vE = [0 0 0 ]; 
vD5 = vE + cross(omega5,rD-rE); 
vD5D4 = vD54*[ cos(phi5) sin(phi5) 0]; 
eqvD = vD5 - vD4 - vD5D4; eqvDx = eqvD(1);  eqvDy = eqvD(2);
solvD = solve(eqvDx,eqvDy);
omega5zs=eval(solvD.omega5z); vD54s=eval(solvD.vD54);
Omega5 = [0 0 omega5zs];
v54 = vD54s*[cos(phi5) sin(phi5) 0];
Omega4 = Omega5;
aD5D4cor = 2*cross(Omega5,v54); 
alpha5 = [ 0 0 alpha5z ]; 
aE = [0 0 0 ]; 
aD5 = aE + cross(alpha5,rD-rE) - dot(Omega5,Omega5)*(rD-rE); 
aD5D4 = aD54*[ cos(phi5) sin(phi5) 0];
eqaD = aD5 - aD4 - aD5D4 - aD5D4cor; eqaDx = eqaD(1); eqaDy = eqaD(2); 
solaD = solve(eqaDx,eqaDy);
alpha5zs=eval(solaD.alpha5z); aD54s=eval(solaD.aD54);
Alpha5 = [0 0 alpha5zs]; Alpha4 = Alpha5;
aF = aC + cross(Alpha3,rF-rC) - dot(Omega3,Omega3)*(rF-rC); 
aG = aE + cross(Alpha5,rG-rE) - dot(Omega5,Omega5)*(rG-rE); 
fprintf('aF = [ %g, %g, %g ] (m/s^2)\n', aF );
fprintf('aG = [ %g, %g, %g ] (m/s^2)\n', aG );

fprintf('omega4 = omega5 = [ %g, %g, %g ] (rad/s)\n',Omega5);fprintf('\n');
fprintf('alpha1 = [ %g, %g, %g ] (rad/s^2)\n', alpha1); 
fprintf('alpha2 = alpha3 = [ %g, %g, %g ] (rad/s^2)\n', Alpha3);
fprintf('alpha4 = alpha5 = [ %g, %g, %g ] (rad/s^2)\n', Alpha5 );
fprintf('\n');
aC1 = aB1/2; fprintf('aC1 = [ %g, %g, %g ] (m/s^2)\n', aC1 );
aC2 = aB2; fprintf('aC2 = aB2 = [ %g, %g, %g ] (m/s^2)\n', aC2 );
aC3 = (aD3+aF)/2; fprintf('aC3 = [ %g, %g, %g ] (m/s^2)\n', aC3 );
aC4 = aD3; fprintf('aC4 = aD4 = [ %g, %g, %g ] (m/s^2)\n', aC4 );
aC5 = (aE+aG)/2; fprintf('aC5 = [ %g, %g, %g ] (m/s^2)\n', aC5 );

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

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

Me = -sign(Omega5(3))*[0,0,100];
fprintf('Me = [ %d, %d, %g] (N m)\n', Me);
fprintf('\n');
fprintf('Inertia forces and inertia moments  \n');
fprintf('\n');

fprintf('Link 1  \n');
m1 = rho*AB*h*d;
Fin1 = -m1*aC1;
G1 = [0,-m1*g,0];
IC1 = m1*(AB^2+h^2)/12;
Min1 = -IC1*alpha1;
fprintf('m1 =  %g (kg)\n', m1);
fprintf('m1 aC1 = [ %g, %g, %g] (N)\n', m1*aC1 );
fprintf('Fin1 = - m1 aC1 = [ %g, %g, %d] (N)\n', Fin1 );
fprintf('G1 = - m1 g = [ %g, %g, %g] (N)\n', G1 );
fprintf('IC1 =  %g (kg m^2)\n', IC1);
fprintf('IC1 alpha1 = [ %g, %g, %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*hSlider*wSlider*d;
Fin2 = -m2*aC2;
G2 = [0,-m2*g,0];
IC2 = m2*(hSlider^2+wSlider^2)/12;
Min2 = -IC2*Alpha2;
fprintf('m2 =  %g (kg)\n', m2);
fprintf('m2 aC2 = [ %g, %g, %g] (N)\n', m2*aC2 );
fprintf('Fin2 = - m2 aC2 = [ %g, %g, %d] (N)\n', Fin2 );
fprintf('G2 = - m2 g = [ %g, %g, %g] (N)\n', G2 );
fprintf('IC2 =  %g (kg m^2)\n', IC2);
fprintf('IC2 alpha2 = [ %g, %g, %g] (N m)\n', IC2*Alpha2 );
fprintf('Min2 = - IC2 alpha2 = [ %d, %d, %g] (N m)\n', Min2 );
fprintf('\n');

fprintf('Link 3  \n');
m3 = rho*DF*h*d;
Fin3 = -m3*aC3;
G3 = [0,-m3*g,0];
IC3 = m3*(DF^2+h^2)/12;
Min3 = -IC3*Alpha3;
fprintf('m3 =  %g (kg)\n', m3);
fprintf('m3 aC3 = [ %g, %g, %g] (N)\n', m3*aC3 );
fprintf('Fin3 = - m3 aC3 = [ %g, %g, %d] (N)\n', Fin3 );
fprintf('G3 = - m3 g = [ %g, %g, %g] (N)\n', G3 );
fprintf('IC3 =  %g (kg m^2)\n', IC3);
fprintf('IC3 alpha3 = [ %g, %g, %g] (N m)\n', IC3*Alpha3 );
fprintf('Min3 = - IC3 alpha3 = [ %d, %d, %g] (N m)\n', Min3 );
fprintf('\n');

fprintf('Link 4  \n');
m4 = rho*hSlider*wSlider*d;
Fin4 = -m4*aC4;
G4 = [0,-m4*g,0];
IC4 = m4*(hSlider^2+wSlider^2)/12;
Min4 = -IC4*Alpha4;
fprintf('m4 =  %g (kg)\n', m4);
fprintf('m4 aC4 = [ %g, %g, %g] (N)\n', m4*aC4 );
fprintf('Fin4 = - m4 aC4 = [ %g, %g, %d] (N)\n', Fin4 );
fprintf('G4 = - m4 g = [ %g, %g, %g] (N)\n', G4 );
fprintf('IC4 =  %g (kg m^2)\n', IC4);
fprintf('IC4 alpha4 = [ %g, %g, %g] (N m)\n', IC4*Alpha4 );
fprintf('Min4 = - IC4 alpha4 = [ %d, %d, %g] (N m)\n', Min4 );
fprintf('\n');

fprintf('Link 5  \n');
m5 = rho*EG*h*d;
Fin5 = -m5*aC5;
G5 = [0,-m5*g,0];
IC5 = m5*(EG^2+h^2)/12;
Min5 = -IC5*Alpha5;
fprintf('m5 =  %g (kg)\n', m5);
fprintf('m5 aC5 = [ %g, %g, %g] (N)\n', m5*aC5 );
fprintf('Fin5 = - m5 aC5 = [ %g, %g, %d] (N)\n', Fin5 );
fprintf('G5 = - m5 g = [ %g, %g, %g] (N)\n', G5 );
fprintf('IC5 =  %g (kg m^2)\n', IC5);
fprintf('IC5 alpha5 = [ %g, %g, %g] (N m)\n', IC5*Alpha5 );
fprintf('Min5 = - IC5 alpha5 = [ %d, %d, %g] (N m)\n', Min5 );

fprintf('\n');
fprintf('Joint reactions \n');
fprintf('Contour method \n');
fprintf('\n');

fprintf('Joint E_rotation \n');
fprintf('\n');
F05=[ sym('F05x','real'), sym('F05y','real'), 0 ]; 
% Sum of the forces for 5 projected on DE
eqER1=dot(F05+G5+Fin5,rE-rD); 
ER1=vpa(eqER1,6); 
fprintf('%s = 0 \n', char(ER1));
% Sum of the moments for 5 & 4 wrt D  
eqER2=cross(rE-rD,F05)+cross(rC5-rD,G5+Fin5)+Me+Min4+Min5;
eqER2z=eqER2(3);
ER2=vpa(eqER2z,6);
fprintf('%s = 0 \n', char(ER2));
solF05=solve(eqER1,eqER2z);
F05s=[ eval(solF05.F05x), eval(solF05.F05y), 0 ];
fprintf('F05 = [ %g, %g, %g] (N)\n', F05s );
fprintf('\n');

fprintf('Joint D_translation \n');
fprintf('\n');
F45=[ sym('F45x','real'), sym('F45y','real'), 0 ]; 
F54=-F45;
rP=[ sym('xP','real'), sym('yP','real'), 0 ]; 
% Sum of the moments for 5 wrt E  
eqDT1=cross(rP-rE,F45)+cross(rC5-rE,G5+Fin5)+Me+Min5;
eqDT1z=eqDT1(3);
DT1=vpa(eqDT1z,6);
fprintf('%s = 0 \n', char(DT1));
% Sum of the moments for 4 wrt D  
eqDT2=cross(rP-rD,F54)+Min4;
eqDT2z=eqDT2(3);
DT2=vpa(eqDT2z,6);
fprintf('%s = 0 \n', char(DT2));
% F45 perpendicular to DE
eqF45DE=dot(F45,rD-rE); % 
F45DE=vpa(eqF45DE,6);
fprintf('%s = 0 \n', char(F45DE));
% point P is on the line ED
eqP=cross(rD-rE,rP-rE); 
eqPz=eqP(3); % eq(4)
Pz=vpa(eqPz,6);
fprintf('%s = 0 \n', char(Pz));
solF45=solve(eqDT1z,eqDT2z,F45DE,eqPz);
F45s=[ eval(solF45.F45x), eval(solF45.F45y), 0 ];
rPs=[ eval(solF45.xP), eval(solF45.yP), 0 ];
fprintf('F45 = [ %g, %g, %g] (N)\n', F45s );
fprintf('rP = [ %g, %g, %g] (m)\n', rPs );
fprintf('\n');

fprintf('Joint D_rotation \n');
fprintf('\n');
F34=[ sym('F34x','real'), sym('F34y','real'), 0 ]; 
F43=-F34;
% Sum of the forces for 4 projected on ED
eqDR1=dot(F34+G4+Fin4,rD-rE); 
DR1=vpa(eqDR1,6); 
fprintf('%s = 0 \n', char(DR1));
% Sum of the moments for 4 & 5 wrt E  
eqDR24=cross(rC4-rE,G4+Fin4)+cross(rD-rE,F34)+Min4;
eqDR25=cross(rC5-rE,G5+Fin5)+Me+Min5;
eqDR2=eqDR24+eqDR25;
eqDR2z=eqDR2(3);
DR2=vpa(eqDR2z,6);
fprintf('%s = 0 \n', char(DR2));
solF34=solve(eqDR1,eqDR2z);
F34s=[ eval(solF34.F34x), eval(solF34.F34y), 0 ];
fprintf('F34 = [ %g, %g, %g] (N)\n', F34s );
fprintf('\n');

fprintf('Joint C_rotation \n');
fprintf('\n');
F03=[ sym('F03x','real'), sym('F03y','real'), 0 ]; 
% Sum of the forces for 3 projected on CD
eqCR1=dot(F03-F34s+G3+Fin3,rD-rC); 
CR1=vpa(eqCR1,6); 
fprintf('%s = 0 \n', char(CR1));
% Sum of the moments for 3 & 2 wrt B
eqCR2=cross(rC3-rB,G3+Fin3)+cross(rC-rB,F03)+cross(rD-rB,-F34s)+Min2+Min3;
eqCR2z=eqCR2(3);
CR2=vpa(eqCR2z,6);
fprintf('%s = 0 \n', char(CR2));
solF03=solve(eqCR1,eqCR2z);
F03s=[ eval(solF03.F03x), eval(solF03.F03y), 0 ];
fprintf('F03 = [ %g, %g, %g] (N)\n', F03s );
fprintf('\n');

fprintf('Joint B_translation \n');
fprintf('\n');
F23=[ sym('F23x','real'), sym('F23y','real'), 0 ]; 
F32=-F23;
rQ=[ sym('xQ','real'), sym('yQ','real'), 0 ]; 
% Sum of the moments for 3 wrt C  
eqBT1=cross(rQ-rC,F23)+cross(rC3-rC,G3+Fin3)+cross(rD-rC,-F34s)+Min3;
eqBT1z=eqBT1(3);
BT1=vpa(eqBT1z,6);
fprintf('%s = 0 \n', char(BT1));
% Sum of the moments for 2 wrt B 
eqBT2=cross(rQ-rB,F32)+Min2;
eqBT2z=eqBT2(3);
BT2=vpa(eqBT2z,6);
fprintf('%s = 0 \n', char(BT2));
% F23 perpendicular to BC
eqF23BC=dot(F23,rC-rB); % 
F23BC=vpa(eqF23BC,6);
fprintf('%s = 0 \n', char(F23BC));
% point Q is on the line BC
eqQ=cross(rB-rC,rQ-rC); 
eqQz=eqQ(3); % eq(4)
Qz=vpa(eqQz,6);
fprintf('%s = 0 \n', char(Qz));
solF23=solve(eqBT1z,eqBT2z,F23BC,eqQz);
F23s=[ eval(solF23.F23x), eval(solF23.F23y), 0 ];
rQs=[ eval(solF23.xQ), eval(solF23.yQ), 0 ];
fprintf('F23 = [ %g, %g, %g] (N)\n', F23s );
fprintf('rQ = [ %g, %g, %g] (N)\n', rQs );
fprintf('\n');

fprintf('Joint B_rotation \n');
fprintf('\n');
F12=[ sym('F12x','real'), sym('F12y','real'), 0 ]; 
F21=-F12;
% Sum of the forces for 2 projected on BC
eqBR1=dot(F12+G2+Fin2,rC-rB); 
BR1=vpa(eqBR1,6); 
fprintf('%s = 0 \n', char(BR1));
% Sum of the moments for 2 & 3 wrt C
eqBR2=cross(rB-rC,F12)+cross(rC2-rC,G2+Fin2)+Min2...
+cross(rC3-rC,G3+Fin3)+cross(rD-rC,-F34s)+Min3;
eqBR2z=eqBR2(3);
BR2=vpa(eqBR2z,6);
fprintf('%s = 0 \n', char(BR2));
solF12=solve(eqBR1,eqBR2z);
F12s=[ eval(solF12.F12x), eval(solF12.F12y), 0 ];
fprintf('F12 = [ %g, %g, %g] (N)\n', F12s );
fprintf('\n');

% Sum of the moments for 1 wrt A
M1m=-(cross(rB,-F12s)+cross(rC1,G1+Fin1)+Min1);
fprintf('Mm = [ %d, %d, %g] (N m)\n', M1m );
fprintf('\n');

fprintf('Joint A_rotation \n');
fprintf('\n');
F01=[ sym('F01x','real'), sym('F01y','real'), 0 ]; 
% Sum of the moments for 1 wrt B
eqAR1=cross(-rB,F01)+cross(rC1-rB,G1+Fin1)+Min1+M1m;
eqAR1z=eqAR1(3);
AR1=vpa(eqAR1z,6);
fprintf('%s = 0 \n', char(AR1));
% Sum of the forces for 1 & 2 projected on BC
eqAR2=dot(F01+G1+Fin1+G2+Fin2,rC-rB); 
AR2=vpa(eqAR2,6); 
fprintf('%s = 0 \n', char(AR2));
solF01=solve(eqAR1z,eqAR2);
F01s=[ eval(solF01.F01x), eval(solF01.F01y), 0 ];
fprintf('F01 = [ %g, %g, %g] (N)\n', F01s );
fprintf('\n');