% R_RTR_1.m
% positions for \Phi = 30 (deg)

clear ; clc; close all

AB = 0.10 ;  % (m)
AC = 0.05 ;  % (m)
CD = 0.15 ;  % (m)
phi = pi/6;  % (rad)
 
xA = 0; yA = 0; 
rA_ = [xA yA 0]; 
xC = AC ; yC = 0; 
rC_ = [xC yC 0]; 
xB = AB*cos(phi); 
yB = AB*sin(phi); 
rB_ = [xB yB 0];  

phi2 = atan((yB-yC)/(xB-xC)); 
phi3 = phi2;

CB = norm([xB-xC, yB-yC]);
ux = (xB - xC)/CB;
uy = (yB - yC)/CB;

xD = xC + CD*ux;
yD = yC + CD*uy;
rD_ = [xD yD 0];

fprintf('Results \n\n') 
fprintf('phi = phi1 = %g (degrees) \n',phi*180/pi) 
fprintf('rA_ = [%6.3f,%6.3f,%g] (m)\n',rA_) 
fprintf('rC_ = [%6.3f,%6.3f,%g] (m)\n',rC_) 
fprintf('rB_ = [%6.3f,%6.3f,%g] (m)\n',rB_) 
fprintf('phi2 = phi3 = %6.3f (degrees) \n',phi2*180/pi) 
fprintf('rD_ = [%6.3f,%6.3f,%g] (m)\n',rD_) 

% Graphic of the mechanism
axis manual
axis equal
hold on
grid on
ax = 0.2;
axis([-ax ax -ax ax])

xlabel('x (m)'), ylabel('y (m)')
title('positions for phi = 30 (deg)')

textAx = xA+0.01;
textAy = yA;

textBx = xB+0.01;
textBy = yB-0.01;

textCx = xC+0.01;
textCy = yC;

textDx = xD+0.007;
textDy = yD+0.007;
 
% plotting paramters
text_size = 10;
slider_scale_factor = 0.015;
pin_scale_factor = 0.005;        
         
% plotting joint A
plot_pin0(xA,yA,pin_scale_factor,0)
% labelling joint A
text(textAx, textAy,'A','FontSize',text_size)
% plotting link 1
plot([xA xB], [yA yB], '-o')
% labelling joint B 
text(textBx, textBy,'B','FontSize',text_size)
% plotting link 2 
plot_slider( xB, yB, slider_scale_factor, phi2)
% plotting joint C 
plot_pin0(xC,yC,pin_scale_factor,0)
% labelling joint C
text( textCx, textCy,'C','FontSize',text_size)        
% plotting link 3
plot([xC xD], [yC yD], '-o')
% labelling joint D 
text(textDx, textDy,'D','FontSize',text_size)

fprintf('\n') 
fprintf('Closed contour equations  \n\n')  
fprintf('Relative velocities \n\n') 

n = -50.; % (rpm) 
omega1_ = [0 0 pi*n/30]; % (rad/s)
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_;


% symbolic unknowns
syms omega21z omega03z vB32

omega10_ = omega1_; 
omega21v_ = [ 0 0 omega21z]; 
omega03v_ = [ 0 0 omega03z]; 
v32v_ = vB32*[ cos(phi2) sin(phi2) 0];

eqIomega_ = omega10_ + omega21v_ + omega03v_; 
eqIvz = eqIomega_(3);
eqIv = cross(rB_,omega21v_) + cross(rC_,omega03v_) + v32v_;
eqIvx=eqIv(1);
eqIvy=eqIv(2);

Ivz=vpa(eqIvz);
fprintf('%s = 0 \n', char(Ivz)) 
Ivx=vpa(eqIvx);
fprintf('%s = 0 \n', char(Ivx)) 
Ivy=vpa(eqIvy);
fprintf('%s = 0 \n', char(Ivy)) 

solIv=solve(eqIvz,eqIvx,eqIvy);
omega21_ = [ 0 0 eval(solIv.omega21z) ]; 
omega03_ = [ 0 0 eval(solIv.omega03z) ]; 
vB3B2_ = eval(solIv.vB32)*[ cos(phi2) sin(phi2) 0]; 

fprintf('omega21_ = [ %g, %g, %6.3f ] (rad/s)\n', omega21_) 
fprintf('omega03_ = [ %g, %g, %6.3f ] (rad/s)\n', omega03_) 
fprintf('vB32 = %6.3f (m/s)\n', eval(solIv.vB32)) 
fprintf('vB3B2_ = [ %6.3f, %6.3f, %d ] (m/s)\n', vB3B2_)  
fprintf('\n') 

fprintf('Absolute velocities \n\n') 
omega30_ = - omega03_;
omega20_ = omega30_;
vC_ = [0 0 0]; % C is fixed
vD_ = vC_ + cross(omega30_, rD_-rC_);
fprintf('omega20_=omega30_= [%d, %d, %6.3f] (rad/s)\n',omega30_) 
fprintf('vD_ = [ %6.3f, %6.3f, %g ] (m/s)\n', vD_) 

% vB32 =  0.211 (m/s)
% vB3B2_ = [  0.125,  0.170, 0 ] (m/s)
% omega20_=omega30_= [0, 0, -7.732] (rad/s)
% vD_ = [  0.936, -0.685, 0 ] (m/s)

fprintf('\n') 
fprintf('Relative accelerations \n\n') 

syms alpha21z alpha03z aB32

alpha10_ = [ 0 0 0 ];  
alpha21v_ = [ 0 0 alpha21z]; 
alpha03v_ = [ 0 0 alpha03z]; 
a32v_ = aB32*[ cos(phi2) sin(phi2) 0]; 

eqIalpha_ = alpha10_ + alpha21v_ + alpha03v_; 
eqIaz = eqIalpha_(3);
eqIa_=cross(rB_,alpha21v_)+cross(rC_,alpha03v_)...
     +a32v_+2*cross(omega20_,vB3B2_)...
-dot(omega1_,omega1_)*rB_-dot(omega20_,omega20_)*(rC_-rB_);
eqIax = eqIa_(1);
eqIay = eqIa_(2);

Iaz=vpa(eqIaz);
fprintf('%s = 0 \n', char(Iaz)) 
Iax=vpa(eqIax);
fprintf('%s = 0 \n', char(Iax)) 
Iay=vpa(eqIay);
fprintf('%s = 0 \n', char(Iay))  

solIa = solve(eqIaz,eqIax,eqIay);
alpha21_ = [ 0 0 eval(solIa.alpha21z) ]; 
alpha03_ = [ 0 0 eval(solIa.alpha03z) ]; 
aB3B2_ = eval(solIa.aB32)*[ cos(phi2) sin(phi2) 0]; 

fprintf('alpha21_ = [ %g, %g, %6.3f ] (rad/s^2)\n', alpha21_) 
fprintf('alpha03_ = [ %g, %g, %6.3f ] (rad/s^2)\n', alpha03_) 
fprintf('aB32 = %6.3f (m/s^2)\n', eval(solIa.aB32)) 
fprintf('aB3B2_ = [ %6.3f, %6.3f, %d ] (m/s^2)\n', aB3B2_)  
fprintf('\n') 

fprintf('Absolute accelerations \n\n') 
alpha30_ = - alpha03_;
alpha20_ = alpha30_;
aC_ = [0 0 0 ]; 
aD_=aC_+cross(alpha30_,rD_-rC_)-dot(omega20_,omega20_)*(rD_-rC_);
fprintf('alpha20_=alpha30_=[%d,%d,%6.3f] (rad/s^2)\n',alpha30_) 
fprintf('aD_ = [ %6.3f, %6.3f, %g ] (m/s^2)\n', aD_) 


omega2_ = omega20_;
omega3_ = omega2_;
alpha2_ = alpha20_;
alpha3_ = alpha2_;

%%%%%%%%%%%%%%%%%%%
rC1_ = rB_/2; 
rC2_ = rB_; 
rC3_ = (rD_+rC_)/2;
aC1_ = aB1_/2; 
aC2_ = aB1_; 
aC3_ = (aD_+aC_)/2; 

fprintf('\n')
fprintf('rC1_ = [%6.3f,%6.3f,%d] (m)\n',rC1_)
fprintf('rC2_ = [%6.3f,%6.3f,%d] (m)\n',rC2_)
fprintf('rC3_ = [%6.3f,%6.3f,%d] (m)\n',rC3_)
fprintf('aC1_ = [%6.3f,%6.3f,%d] (m/s^2)\n', aC1_)
fprintf('aC2_ = [%6.3f,%6.3f,%d] (m/s^2)\n', aC2_)
fprintf('aC3_ = [%6.3f,%6.3f,%d] (m/s^2)\n', aC3_)
fprintf...
('alpha1_ = [%d ,%d,%6.3f](rad/s^2)\n', alpha1_) 
fprintf...
('alpha2_ = [%d,%d,%6.3f](rad/s^2)\n', alpha2_) 
fprintf...
('alpha3_ = [%d,%d,%6.3f](rad/s^2)\n', alpha3_) 
fprintf('\n')

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

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

fprintf('Link 1 \n')
m1 = rho*AB*h*d; % mass
G1_ = [0,-m1*g,0]; % force of gravity
Fin1_ = -m1*aC1_;  % force of inertia
IC1 = m1*(AB^2+h^2)/12; % mass moment of inertia
IA  = m1*(AB^2+h^2)/3 ; % A fixed point
alpha1_ = [0 0 0]; % angular acceleration
Min1_ = -IC1*alpha1_; % moment of inertia
%   fprintf('m1 = %6.3f (kg)\n', m1)
fprintf('G1_ = - m1 g_ = [%d,%6.3f,%d] (N)\n', G1_)
% fprintf('m1 aC1_ = [%6.3f,%6.3f,%d] (N)\n', m1*aC1_)
fprintf('Fin1_= - m1 aC1_ = [%6.3f,%6.3f,%d] (N)\n', Fin1_)
% fprintf('IC1 = %g (kg m^2)\n', IC1)
% fprintf('IA = %g (kg m^2)\n', IA)
% fprintf('IC1 alpha1_=[%6.3f,%6.3f,%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;
G2_ = [0,-m2*g,0];
Fin2_ = -m2*aC2_;
IC2 = m2*(hSlider^2+wSlider^2)/12;
Min2_ = -IC2*alpha2_;
% fprintf('m2 = %6.3f (kg)\n', m2)
fprintf('G2_ = - m2 g_ = [%d,%6.3f,%d] (N)\n', G2_)
% fprintf('m2 aC2_ = [%6.3f,%6.3f,%d] (N)\n', m2*aC2_)
fprintf('Fin2_= - m2 aC2_ =[%6.3f,%6.3f,%d] (N)\n', Fin2_)
% fprintf('IC2 = %6.3d (kg m^2)\n', IC2)
% fprintf('IC2 alpha2_=[%g,%g,%6.3d](N m)\n',IC2*alpha2_)
fprintf('Min2_=-IC2 alpha2_=[%d,%d,%6.3d](N m)\n',Min2_)
fprintf('\n')

fprintf('Link 3 \n')
m3 = rho*CD*h*d;
G3_ = [0,-m3*g,0];
Fin3_ = -m3*aC3_;
IC3 = m3*(CD^2+h^2)/12;
IC  = IC3+m3*(CD/2)^2; % C fixed point
Min3_ = -IC3*alpha3_;
% fprintf('m3 = %6.3f (kg)\n', m3)
fprintf('G3_ = - m3 g_ = [%d, %6.3f, %d] (N)\n', G3_)
% fprintf('m3 aC3_ = [%6.3f,%6.3f,%g] (N)\n', m3*aC3_)
fprintf('Fin3_ = - m3 aC3_ =[%6.3f,%6.3f,%d] (N)\n',Fin3_)
% fprintf('IC3 = %6.3d (kg m^2)\n', IC3)
% fprintf('IC3 alpha3_=[%d,%d,%6.3d](N m)\n',IC3*alpha3_)
fprintf('Min3_=-IC3 alpha3_=[%d,%d,%6.3d](N m)\n',Min3_)
fprintf('\n')

Me = 100; % N m
Me_ = -sign(omega3_(3))*[0,0,Me];
fprintf('Me_ = [%d, %d, %6.3f] (N m)\n',Me_)
fprintf('\n')


%  joint C rotation
syms F03x F03y
% unknown joint force of 0 on 3 at C 
F03_=[F03x, F03y, 0]; 
% for joint B translation:
% (F03_+G3_+Fin3_+F23_) .(rB_-rC_)=0
% F23_ perpendicular to BC: F32_.BC_ = 0 => 
eqF3 = (F03_+G3_+Fin3_)*(rB_-rC_).'; % (F1)
% for joint B rotation
% BC_xF03_+BC3_x(G3_+Fin3_)+Me_+Min2_+Min3_=0_
eqM32B_ = ...
cross(rC_-rB_,F03_)+cross(rC3_-rB_,G3_+Fin3_)...
+Me_+Min2_+Min3_;
eqM32z = eqM32B_(3); % (F2)
% Eq(F1) and Eq(F2) =>
solF03=solve(eqF3,eqM32z);
F03xs=eval(solF03.F03x);
F03ys=eval(solF03.F03y);
F03s_=[F03xs, F03ys, 0]; 
fprintf('F03_ = [%6.3f,%6.3f,%g] (N)\n',F03s_)
% F03_ = [-1302.149,953.291,0] (N)

% joint B rotation
syms F12x F12y 
F12_=[F12x, F12y, 0]; % unknown joint force of 1 on 2 at B 
% for joint B translation
% (F12_+G2_+Fin2_+F32_) .(rB_-rC_)=0
% F32_ = -F23_ perpendicular to BC: F32_.BC_ = 0 => 
eqF2 = (F12_+G2_+Fin2_)*(rB_-rC_).'; % (F3)
% for joint C rotation
% CB_x(F12_+G2_+Fin2_)+CC3_x(G3_+Fin3_)+Me_+Min2_+Min3_=0_
eqM23C_ = ...
cross(rB_-rC_,F12_+G2_+Fin2_)+cross(rC3_-rC_,G3_+Fin3_)...
+Me_+Min2_+Min3_;
eqM23z = eqM23C_(3); % (F4)
% Eq(F3) and Eq(F4) =>
solF12=solve(eqF2,eqM23z);
F12xs=eval(solF12.F12x);
F12ys=eval(solF12.F12y);
F12s_=[F12xs, F12ys, 0]; 
fprintf('F12_ = [%6.3f,%6.3f,%g] (N)\n',F12s_)
% F12_ = [1302.128,-953.181,0] (N)

% joint B translation
% for joint B rotation
% Min2_ - M23_ = 0_
M23_= Min2_; % 
fprintf('M23_ = [%d,%d,%6.6f] (N m)\n',M23_)
% M23_ = [0,0,0.000037] (N m)
syms F23x F23y 
F23_=[F23x, F23y, 0]; % unknown joint force of 2 on 3 at B 
% F23_ perpendicular to BC: F32.BC = 0 => 
eqF23 = (rB_-rC_)*F23_.'; % (F5)
% for joint C rotation
% CB_xF23_+CC3_x(G3_+Fin3_)+Me_+Min3_+Min3_=0_
eqM3C_ = ...
cross(rB_-rC_,F23_)+cross(rC3_-rC_,G3_+Fin3_)...
+Me_+Min3_+M23_;
eqM3Cz = eqM3C_(3); % (F6)
% Eq(F5) and Eq(F6) =>
solF23=solve(eqF23,eqM3Cz);
F23xs=eval(solF23.F23x);
F23ys=eval(solF23.F23y);
F23s_=[F23xs, F23ys, 0];
fprintf('F23_ = [%6.3f,%6.3f,%g] (N)\n',F23s_)
% F23_ = [1302.143,-953.235,0] (N)

% joint A rotation
syms F01x F01y Mm
F01_=[F01x, F01y, 0]; % unknown joint force of 0 on 1 at A 
Mm_= [0, 0, Mm]; % equilibrium moment
% for joint B rotation
% BA_xF01_+BC1_x(G1_+Fin1_)+Mm_+Min1_=0_
eqM1B_ = ...
cross(rA_-rB_,F01_)+cross(rC1_-rB_,G1_+Fin1_)+Mm_+Min1_;
eqM1Bz = eqM1B_(3); % (F7)
% for joint B translation
% (F01+G1_+Fin1_+G2_+Fin2_+F32_) .(rB_-rC_)=0
% F32_ = -F23_ perpendicular to BC: F32_.BC_ = 0 => 
eqF12 = (F01_+G1_+Fin1_+G2_+Fin2_)*(rB_-rC_).'; % (F8)
% for joint C rotation
% CA_xF01_+CC1_x(G1_+Fin1_)+CB_x(G2_+Fin2_)+CC3_x(G3_+Fin3_)
% Mm_+Me_+Min2_+Min3_=0_
eqM123C_ = ...
 cross(rA_-rC_,F01_)+cross(rC1_-rC_,G1_+Fin1_)...
+cross(rB_-rC_,G2_+Fin2_)+cross(rC3_-rC_,G3_+Fin3_)...
+Mm_+Me_+Min2_+Min3_;
eqM123z = eqM123C_(3); % (F9)

% Eq(F7), Eq(F8) and Eq(F9) =>
solF01=solve(eqM1Bz, eqF12, eqM123z);
F01xs=eval(solF01.F01x);
F01ys=eval(solF01.F01y);
Mms=eval(solF01.Mm);
F01s_=[F01xs, F01ys, 0];
Mms_=[0, 0, Mms];
fprintf('F01_ = [%6.3f,%6.3f,%g] (N)\n',F01s_)
fprintf('Mm_ = [%d,%d,%6.3f] (N m)\n',Mms_)






% eof         

% Results 
% 
% phi = phi1 = 30 (degrees) 
% rA_ = [ 0.000, 0.000,0] (m)
% rC_ = [ 0.050, 0.000,0] (m)
% rB_ = [ 0.087, 0.050,0] (m)
% phi2 = phi3 = 53.794 (degrees) 
% rD_ = [ 0.139, 0.121,0] (m)
% 
% Closed contour equations  
% 
% Relative velocities 
% 
% omega03z + omega21z - 5.24 = 0 
% 0.05*omega21z + 0.591*vB32 = 0 
% 0.807*vB32 - 0.0866*omega21z - 0.05*omega03z = 0 
% omega21_ = [ 0, 0, -2.496 ] (rad/s)
% omega03_ = [ 0, 0,  7.732 ] (rad/s)
% vB32 =  0.211 (m/s)
% vB3B2_ = [  0.125,  0.170, 0 ] (m/s)
% 
% Absolute velocities 
% 
% omega20_=omega30_= [0, 0, -7.732] (rad/s)
% vD_ = [  0.936, -0.685, 0 ] (m/s)
% 
% Relative accelerations 
% 
% alpha03z + alpha21z = 0 
% 0.591*aB32 + 0.05*alpha21z + 2.45 = 0 
% 0.807*aB32 - 0.05*alpha03z - 0.0866*alpha21z - 0.311 = 0 
% alpha21_ = [ 0, 0, -34.865 ] (rad/s^2)
% alpha03_ = [ 0, 0, 34.865 ] (rad/s^2)
% aB32 = -1.196 (m/s^2)
% aB3B2_ = [ -0.706, -0.965, 0 ] (m/s^2)
% 
% Absolute accelerations 
% 
% alpha20_=alpha30_=[0,0,-34.865] (rad/s^2)
% aD_ = [ -1.077, -10.324, 0 ] (m/s^2)
% 
% rC1_ = [ 0.043, 0.025,0] (m)
% rC2_ = [ 0.087, 0.050,0] (m)
% rC3_ = [ 0.094, 0.061,0] (m)
% aC1_ = [-1.187,-0.685,0] (m/s^2)
% aC2_ = [-2.374,-1.371,0] (m/s^2)
% aC3_ = [-0.538,-5.162,0] (m/s^2)
% alpha1_ = [0 ,0, 0.000](rad/s^2)
% alpha2_ = [0,0,-34.865](rad/s^2)
% alpha3_ = [0,0,-34.865](rad/s^2)
% 
% 
% Dynamic force analysis 
% Link 1 
% G1_ = - m1 g_ = [0,-0.078,0] (N)
% Fin1_= - m1 aC1_ = [ 0.009, 0.005,0] (N)
% Min1_=-IC1 alpha1_=[0, 0, 0](N m)
% 
% Link 2 
% G2_ = - m2 g_ = [0,-0.063,0] (N)
% Fin2_= - m2 aC2_ =[ 0.015, 0.009,0] (N)
% Min2_=-IC2 alpha2_=[0,0,3.719e-05](N m)
% 
% Link 3 
% G3_ = - m3 g_ = [0, -0.118, 0] (N)
% Fin3_ = - m3 aC3_ =[ 0.006, 0.062,0] (N)
% Min3_=-IC3 alpha3_=[0,0,7.880e-04](N m)
% 
% Me_ = [0, 0, 100.000] (N m)
% 
% F03_ = [-1302.149,953.291,0] (N)
% F12_ = [1302.128,-953.181,0] (N)
% M23_ = [0,0,0.000037] (N m)
% F23_ = [1302.143,-953.235,0] (N)
% F01_ = [1302.118,-953.108,0] (N)
% Mm_ = [0,0,-147.651] (N m)