% Program 4.2
% R-RRT
% Dynamic force analysis 
% D'Alembert Principle

clear; clc; close;
AB = 1; BC = 1;
phi_input = 45; phi = phi_input*(pi/180);
xA = 0; yA = 0; rA = [xA yA 0];                       % position of A 
xB = AB*cos(phi); yB = AB*sin(phi); rB = [xB yB 0];   % position of B 
yC = 0; xC = xB+sqrt(BC^2-(yC-yB)^2); rC = [xC yC 0]; % position of C 
phi2 = atan((yB-yC)/(xB-xC));
n = 30/pi; % (rpm)  driver link
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;
omega2z = sym('omega2z','real'); vCx = sym('vCx','real');
omega2 = [ 0 0 omega2z ]; vC = [ vCx 0 0 ];
eqvC = vC - (vB2 + cross(omega2,rC-rB)); 
eqvCx = eqvC(1); eqvCy = eqvC(2); 
solvC = solve(eqvCx,eqvCy); omega2zs=eval(solvC.omega2z);
vCxs=eval(solvC.vCx); Omega2 = [0 0 omega2zs];
vCs = [vCxs 0 0];
alpha2z = sym('alpha2z','real'); aCx = sym('aCx','real');
alpha2 = [ 0 0 alpha2z ]; aC = [aCx 0 0 ]; 
eqaC = aC - (aB1 + cross(alpha2,rC-rB) - dot(Omega2,Omega2)*(rC-rB)); 
eqaCx = eqaC(1); eqaCy = eqaC(2); 
solaC = solve(eqaCx,eqaCy);
alpha2zs=eval(solaC.alpha2z); aCxs=eval(solaC.aCx);
alpha20 = [0 0 alpha2zs]; aCs = [aCxs 0 0];
alpha30 = [0 0 0]; 

rC1 = (rA+rB)/2;
rC2 = (rB+rC)/2;
rC3 = rC;

% Graphic of the mechanism
plot([0,xB],[0,yB],'r-o',[xB,xC],[yB,yC],'b-o'),...
xlabel('x (m)'), ylabel('y (m)'),...  
title('positions for \phi = 45 (deg)'),...  
text(xA,yA,'  A'),text(xB,yB,'  B'),text(xC,yC,'  C=C3');,...
text(rC1(1),rC1(2),' C1'),text(rC2(1),rC2(2),' C2'),...
axis([-0.1,1.6,-0.1,1.6])

aC1 = aB1/2;
aC2 = (aB1+aCs)/2;
aC3 = aCs;

h = 0.01; d = 0.01; hSlider = 0.01; wSlider = 0.01; 
g = 10.; % gravitational acceleration

m1 = 1 ;
IC1 = m1*(AB^2+h^2)/12 ;
G1 = [ 0  -m1*g  0 ] ;
Fin1 = - m1*aC1 ;
Min1 = - IC1*alpha1 ;

m2 = 1 ;
IC2 = m2*(BC^2+h^2)/12 ;
G2 = [ 0  -m2*g  0 ] ;
Fin2 = - m2*aC2 ;
Min2 = - IC2*alpha20 ;

m3 = 1 ;
IC3 = m3*(hSlider^2+wSlider^2)/12 ;
G3 = [ 0  -m3*g  0 ] ;
Fin3 = - m3*aC3 ;
Min3 = - IC3*alpha30 ;

% external force
fe = 100; Fe = -sign(vCs(1))*[fe 0 0];

fprintf('Results \n');
fprintf('\n');
fprintf('Dynamic force analysis  \n');
fprintf('D Alembert Principle  \n');
fprintf('\n');

% link 3
F03 = [ 0 sym('F03y','real') 0 ] ;
F23 = [ sym('F23x','real') sym('F23y','real') 0 ] ;
% sum of the forces for link 3
eqF3 = F03+F23+Fe+G3+Fin3 ;
eqF3x = eqF3(1) ;
eqF3y = eqF3(2) ;
fprintf('%s = 0 (1)\n', char(vpa(eqF3x,6))) ;
fprintf('%s = 0 (2)\n', char(vpa(eqF3y,6))) ;

% link 2
F32 = -F23 ;
F12 = [ sym('F12x','real') sym('F12y','real') 0 ] ;
% sum of the forces for link 2
eqF2 = F32+F12+G2+Fin2 ;
eqF2x = eqF2(1) ;
eqF2y = eqF2(2) ;
% sum of the moments for link 2 wrt C2 
eqM2 = cross(rB-rC2,F12)+cross(rC-rC2,F32)+Min2 ;
eqM2z = eqM2(3) ;
fprintf('%s = 0 (3)\n', char(vpa(eqF2x,6))) ;
fprintf('%s = 0 (4)\n', char(vpa(eqF2y,6))) ;
fprintf('%s = 0 (5)\n', char(vpa(eqM2z,6))) ;

% link 1
F01 = [ sym('F01x','real') sym('F01y','real') 0 ] ;
Mm = [ 0 0 sym('Mmz','real') ] ;
% sum of the forces for 1 
eqF1 = F01+Fin1+G1-F12;
eqF1x = eqF1(1) ;
eqF1y = eqF1(2) ;
% sum of the moments for 1 wrt C1 
eqM1 = cross(rB-rC1,-F12)+cross(rA-rC1,F01)+Min1+Mm;
eqM1z = eqM1(3) ;
fprintf('%s = 0 (6)\n', char(vpa(eqF1x,6))) ;
fprintf('%s = 0 (7)\n', char(vpa(eqF1y,6))) ;
fprintf('%s = 0 (8)\n', char(vpa(eqM1z,6))) ; fprintf('\n');

fprintf('Eqs(1)-(8) => F03y, F23x, F23y, F12x, F12y, F01x, F01y, Mmz \n');

sol321=solve(eqF3x,eqF3y,eqF2x,eqF2y,eqM2z,eqF1x,eqF1y,eqM1z);
F03ys = eval(sol321.F03y); 
F23xs = eval(sol321.F23x);
F23ys = eval(sol321.F23y);
F12xs = eval(sol321.F12x);
F12ys = eval(sol321.F12y);
F01xs = eval(sol321.F01x);
F01ys = eval(sol321.F01y);
Mmzs = eval(sol321.Mmz);

F03s = [ 0, F03ys, 0 ];
F23s = [ F23xs, F23ys, 0 ];
F12s = [ F12xs, F12ys, 0 ];
F01s = [ F01xs, F01ys, 0 ];
Mms = [ 0, 0, Mmzs ]; 

fprintf('\n');
fprintf('F03 = [ %g, %g, %g ] (N)\n', F03s );
fprintf('F23 = [ %g, %g, %g ] (N)\n', F23s );
fprintf('F12 = [ %g, %g, %g ] (N)\n', F12s ); 
fprintf('F01 = [ %g, %g, %g ] (N)\n', F01s ); 
fprintf('Mm = [ %g, %g, %g ] (N m)\n', Mms );