% Program 4.3
% R-RRT
% Dynamic force analysis 
% Dyad method

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;

% Inertia forces and moments

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('\n');
fprintf('Dynamic force analysis  \n');
fprintf('Dyad method \n');
fprintf('\n');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Dyad: B(R), C(R), C(T)  - links 3&2
F03 = [ 0 sym('F03y','real') 0 ] ;
F12 = [ sym('F12x','real') sym('F12y','real') 0 ] ;
% sum of the forces for links 3 and 2
eqF32 = F03+Fe+G3+Fin3+Fin2+G2+F12 ;
eqF32x = eqF32(1) ;
eqF32y = eqF32(2) ;
% sum of the moments for link 2 wrt C 
eqM2C = cross(rB-rC,F12)+cross(rC2-rC,Fin2+G2)+Min2 ;
eqM2Cz = eqM2C(3) ;

fprintf('%s = 0 (1)\n', char(vpa(eqF32x,6))) ;
fprintf('%s = 0 (2)\n', char(vpa(eqF32y,6))) ;
fprintf('%s = 0 (3)\n', char(vpa(eqM2Cz,6))) ;
fprintf('Eqs(1)-(3) => F03y, F12x, F12y \n');

sol32=solve(eqF32x,eqF32y,eqM2Cz);
F03ys=eval(sol32.F03y); 
F12xs=eval(sol32.F12x);
F12ys=eval(sol32.F12y);
F03s = [ 0, F03ys, 0 ];
F12s = [ F12xs, F12ys, 0 ];
fprintf('\n');
fprintf('F03 = [ %g, %g, %g ] (N)\n', F03s );
fprintf('F12 = [ %g, %g, %g ] (N)\n', F12s ); 
fprintf('\n');

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