% Program 4.1
% R-RRT
% Dynamic force analysis 
% Newton-Euer method

clear; clc; close;
AB = 1; BC = 1; phi = 45*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));
fprintf('Results \n'); fprintf(' \n');
fprintf('phi = phi1 = %g (degrees) \n', phi*180/pi);
fprintf('rA =  [ %g, %g, %g ] (m)\n', rA);
fprintf('rB =  [ %g, %g, %g ] (m)\n', rB);
fprintf('rC =  [ %g, %g, %g ] (m)\n', rC);
n = 30/pi; % (rpm)  driver link
omega1 = [ 0 0 pi*n/30 ]; alpha1 = [0 0 0 ];
fprintf('alpha1 = [ %g, %g, %g ] (rad/s^2)\n', alpha1 ); 
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];
fprintf('alpha2 = [ %g, %g, %g ] (rad/s^2)\n', alpha20 );
alpha30 = [0 0 0]; 
fprintf('alpha3 = [ %g, %g, %g ] (rad/s^2)\n', alpha30 );
fprintf('\n');

fprintf('Position, velocity and accelerations for mass centers \n');
fprintf('\n');
rC1 = (rA+rB)/2; fprintf('rC1 = [ %g, %g, %g ] (m)\n', rC1);
rC2 = (rB+rC)/2; fprintf('rC2 = [ %g, %g, %g ] (m)\n', rC2);
rC3 = rC; fprintf('rC3 = [ %g, %g, %g ] (m)\n', rC3);

% 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; fprintf('aC1 = [ %g, %g, %g ] (m/s^2)\n', aC1 ) ;
aC2 = (aB1+aCs)/2; fprintf('aC2 = [ %g, %g, %g ] (m/s^2)\n', aC2 ) ;
aC3 = aCs; fprintf('aC3 = [ %g, %g, %g ] (m/s^2)\n', aC3 ) ;

fprintf('\n');
% external force
fe = 100;
Fe = -sign(vCs(1))*[fe 0 0];
fprintf('external force Fe = [ %d, %d, %g ] (N)\n', Fe); 

h = 0.01; % height of the bar (m)
d = 0.001; % depth of the bar (m)
hSlider = 0.01; % height of the slider (m)
wSlider = 0.01; % depth of the slider (m)
g = 10.; % gravitational acceleration (m/s^2)

fprintf('\n');
fprintf('Inertia forces and moments \n');
fprintf('\n');

fprintf(' link 1 \n');
m1 = 1 ;
IC1 = m1*(AB^2+h^2)/12 ;
G1 = [ 0  -m1*g  0 ] ;
Fin1 = - m1*aC1 ;
Min1 = - IC1*alpha1 ;
fprintf('m1 = %g (kg)\n', m1) ;
fprintf('IC1 = %g (kg m^2)\n', IC1) ;
fprintf('G1 = [ %d, %g, %d ] (N)\n', G1) ;
fprintf('Fin1 = [ %g, %g, %d ] (N)\n', Fin1) ;
fprintf('Min1 = [ %d, %d, %d ] (N m)\n', Min1) ;

fprintf(' link 2 \n');
m2 = 1 ;
IC2 = m2*(BC^2+h^2)/12 ;
G2 = [ 0  -m2*g  0 ] ;
Fin2 = - m2*aC2 ;
Min2 = - IC2*alpha20 ;
fprintf('m2 = %g (kg)\n', m2) ;
fprintf('IC2 = %g (kg m^2)\n', IC2) ;
fprintf('G2 = [ %d, %g, %d ] (N)\n', G2) ;
fprintf('Fin2 = [ %g, %g, %d ] (N)\n', Fin2) ;
fprintf('Min2 = [ %d, %d, %d ] (N m)\n', Min2) ;

fprintf(' link 3 \n');
m3 = 1 ;
IC3 = m3*(hSlider^2+wSlider^2)/12 ;
G3 = [ 0  -m3*g  0 ] ;
Fin3 = - m3*aC3 ;
Min3 = - IC3*alpha30 ;
fprintf('m3 = %g (kg)\n', m3) ;
fprintf('IC3 = %g (kg m^2)\n', IC3) ;
fprintf('G3 = [ %d, %g, %d ] (N)\n', G3) ;
fprintf('Fin3 = [ %g, %g, %d ] (N)\n', Fin3) ;
fprintf('Min3 = [ %d, %d, %d ] (N m)\n', Min3) ;

fprintf('\n');
fprintf('Dynamic force analysis  \n');
fprintf('Newton-Euler method  \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-m3*aC3 ;
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-m2*aC2 ;
eqF2x = eqF2(1) ;
eqF2y = eqF2(2) ;
% sum of the moments for link 2 wrt C2 
eqM2 = cross(rB-rC2,F12)+cross(rC-rC2,F32)-IC2*alpha20 ;
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))) ;
fprintf('\n');
fprintf('Eqs(1)-(5) => F03y, F23x, F23y, F12x, F12y \n');
sol32=solve(eqF3x,eqF3y,eqF2x,eqF2y,eqM2z);
F03ys=eval(sol32.F03y); 
F23xs=eval(sol32.F23x);
F23ys=eval(sol32.F23y);
F12xs=eval(sol32.F12x);
F12ys=eval(sol32.F12y);
F03s = [ 0, F03ys, 0 ];
F23s = [ F23xs, F23ys, 0 ];
F12s = [ F12xs, F12ys, 0 ];

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('\n');

% link 1
% sum of the forces for 1 
F01=m1*aC1-G1+F12s;
% sum of the moments for 1 wrt A 
Mm=-cross(rB,-F12s)-cross(rC1,G1-m1*aC1)-IC1*alpha1;
fprintf('F01 = [ %g, %g, %g ] (N)\n', F01 );
fprintf('Mm = [ %d, %d, %g ] (N m)\n', Mm );