% Program HW force analysis
% R-RTR
% Velocity and acceleration analysis

clear; clc; close;

AB = 0.14 ; AC = 0.06 ; CF = 0.2 ; % (m)
phi_input = 60; phi = phi_input*(pi/180); 
xA = 0; yA = 0; rA = [xA yA 0]; 
xC = 0 ; yC = AC ; rC = [xC yC 0]; 
xB = AB*cos(phi); yB = AB*sin(phi); rB = [xB yB 0]; 
phi2 = atan((yB-yC)/(xB-xC)); phi3 = phi2;
xF = xC + CF*cos(phi3) ; yF = yC + CF*sin(phi3) ; rF = [xF yF 0]; 
fprintf('Results \n') ; fprintf('\n');
fprintf('rA=[%g, %g, %g] (m)\n', rA); 
fprintf('rC=[%g, %g, %g] (m)\n', rC);
fprintf('rB=[%g, %g, %g] (m)\n', rB);
fprintf('rF=[%g, %g, %g] (m)\n', rF);
fprintf('phi2=phi3=%g(degrees)\n',phi2*180/pi);
plot([xA,xB],[yA,yB],'r-o',[xC,xF],[yC,yF],'b-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(xF,yF,' F'),... 
grid, axis([-0.1 0.3 -0.1 0.3]);

fprintf('\n'); 
fprintf('Velocity and acceleration analysis \n'); fprintf('\n');
n = 30*pi; 
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];

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;
aF = aC + cross(Alpha3,rF-rC) - dot(Omega3,Omega3)*(rF-rC); 
fprintf('aF = [ %g, %g, %g ] (m/s^2)\n', aF );

fprintf('alpha1 = [ %g, %g, %g ] (rad/s^2)\n', alpha1); 
fprintf('alpha2 = alpha3 = [ %g, %g, %g ] (rad/s^2)\n', Alpha3);
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 = (aC+aF)/2; fprintf('aC3 = [ %g, %g, %g ] (m/s^2)\n', aC3 );

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('\n');
fprintf('Dynamic force analysis  \n');
fprintf('Newton-Euler method  \n');
fprintf('\n');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

h = 0.01; % height of the bar
d = 0.01; % 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(Omega3(3))*[0,0,1000];
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*CF*h*d;
Fin3 = -m3*aC3;
G3 = [0,-m3*g,0];
IC3 = m3*(CF^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');