% R-RRR Mechanism
% Dyad method
% 0 < phi < pi/2

clear; clc; close all 

AB=0.15;    % (m) link 1
BC=0.35;    % (m) link 2
CD=0.20;    % (m) link 3
AD=0.30;    % (m) link 0
phi = pi/3; % (rad) angle of link 1 with x-axis
n = 60;     % (rpm) angular speed of link 1 (constant)

omega = pi*n/30; % (rad/s) angular velocity of 1
alpha = 0; % (rad/s^2) angular acceleration of 1

omega1_ = [0 0 omega]; % angular velocity vector of 1
alpha1_ = [0 0 alpha]; % angular acceleration vector of 1

% Joint A (origin)
xA = 0; yA = 0;
rA_ = [xA yA 0]; % position of A
vA_ = [0 0 0];   % velocity of A 
aA_ = [0 0 0];   % acceleration of A 

% Joint D (fixed)
xD = AD; yD = 0; 
rD_ = [xD yD 0]; % position of D
vD_ = [0 0 0];   % velocity of D 
aD_ = [0 0 0];   % acceleration of D 

% Joint B 
% call pvaR function 
% pvaR(rAi_, vAi_, aAi_, li, phi, omega, alpha)
% rAi_ = rA_
% vAi_ = vA_
% aAi_ = aA_
% li = AB
% phi = phi
% omega = omega
% alpha = alpha
pvaB = pvaR(rA_, vA_, aA_, AB, phi, omega, alpha);

xB = pvaB(1);
yB = pvaB(2);
rB_ = [xB yB 0];   % position vector of B

vBx= pvaB(3);
vBy= pvaB(4);
vB_ = [vBx vBy 0]; % velocity vector of B

aBx= pvaB(5);
aBy= pvaB(6); 
aB_ = [aBx aBy 0]; % acceleration vector of B

% position of joint C
% call function pRRR
% posRRR(rAi_, rAj_, li, lj)
% rAi_ = rB_
% rAj_ = rD_
% li = BC
% lj = CD
posC = posRRR(rB_, rD_, BC, CD);

xCI = posC(1); yCI = posC(2);
xCII = posC(3); yCII = posC(4);
% select correct C
if yCI > yB xC = xCI; yC = yCI;
else xC = xCII; yC = yCII; end 
rC_ = [xC yC 0]; % position vector of C 

% angle of link 2 with x-axis
phi2 = atan((yB-yC)/(xB-xC));
% angle of link 3 with x-axis
phi3 = atan((yC-yD)/(xC-xD));

% Graphic of the mechanism
pm=plot([xA,xB],[yA,yB],'r-o',...
[xB,xC],[yB,yC],'k-o',...
[xD,xC],[yD,yC],'k-o','LineWidth',2);
hold on
axis([-0.1 0.5 -0.1 0.5]),...
xlabel('x (m)'),ylabel('y (m)')
grid

pivot0_scale = 0.01;  
plot_pivot0( xA, yA, pivot0_scale, 0)
plot_pivot0( xD, yD, pivot0_scale, 0)

text(xA+0.01,yA,'A','FontSize',14)
text(xB,yB+0.02,'B','FontSize',14)
text(xC+0.01,yC,'C','FontSize',14) 
text(xD+0.02,yD,'D','FontSize',14) 


fprintf('Results \n\n') 
fprintf('phi = phi1 = %g (degrees) \n',phi*180/pi) 
fprintf('rA_ = [%g, %g, %g] (m)\n', rA_) 
fprintf('rD_ = [%g, %g, %g] (m)\n', rD_) 

fprintf('rB_ = [%6.3f, %6.3f, %g] (m)\n', rB_)
fprintf...
('vB_ = [%6.3f, %6.3f, %g] (m/s)\n',vB_)  
fprintf...
('aB_ = [%6.3f, %6.3f, %g] (m/s^2)\n',aB_)  
fprintf('\n')
fprintf('rC_ = [%6.3f, %6.3f, %g] (m)\n', rC_)
fprintf...
('phi2 = %6.3f (degrees) \n', phi2*180/pi)
fprintf...
('phi3 = %6.3f (degrees) \n', phi3*180/pi)
fprintf('\n')

% phi = phi1 = 60 (degrees) 
% rA_ = [0, 0, 0] (m)
% rD_ = [0.3, 0, 0] (m)
% rB_ = [ 0.075,  0.130, 0] (m)
% vB_ = [-0.816,  0.471, 0] (m/s)
% aB_ = [-2.961, -5.128, 0] (m/s^2)
% 
% rC_ = [ 0.424,  0.157, 0] (m)
% phi2 =  4.433 (degrees) 
% phi3 = 51.701 (degrees) 

% velocity and acceleration analysis dyad method

% velocity and acceleration C 
% call velaccRRR function 
% velaccRRR(rAi_,rAj_,rP_,vAi_,vAj_,aAi_,aAj_)
% rAi_ = rB_
% rAj_ = rD_
% rP_  = rC_
% vAi_ = vB_
% vAj_ = vD_
% aAi_ = aB_
% aAj_ = aD_
vaC = velaccRRR(rB_,rD_,rC_,vB_,vD_,aB_,aD_);
vCx = vaC(1);
vCy = vaC(2);
vC_ = [vCx vCy 0]; % velocity vector of C
aCx = vaC(3);
aCy = vaC(4);
aC_ = [aCx aCy 0]; % acceleration vector of C

fprintf('vC_ = [%6.3f, %6.3f, %d] (m/s)\n', vC_)
fprintf('aC_ = [%6.3f, %6.3f, %d] (m/s^2)\n', aC_)
fprintf('\n')

% angular velocity and acceleration of link 2
% call  angvelacc
% angvelacc(rP_, rQ_, vP_, vQ_, aP_, aQ_)
% rP_ = rB_
% rQ_ = rC_
% vP_ = vB_
% vQ_ = vC_
% aP_ = aB_
% aQ_ = aC_
angvelacc2 = angvelacc(rB_,rC_,vB_,vC_,aB_,aC_);
omega2z = angvelacc2(1);
alpha2z = angvelacc2(2);
omega2_ = [0 0 omega2z]; 
alpha2_ = [0 0 alpha2z]; 

% angular velocity and acceleration link 3
% call  angvelacc
% angvelacc(rP_, rQ_, vP_, vQ_, aP_, aQ_)
% rP_ = rD_
% rQ_ = rC_
% vP_ = vD_
% vQ_ = vC_
% aP_ = aD_
% aQ_ = aC_
angvelacc3 = angvelacc(rD_,rC_,vD_,vC_,aD_,aC_);
omega3z = angvelacc3(1);
alpha3z = angvelacc3(2);
omega3_ = [0 0 omega3z]; 
alpha3_ = [0 0 alpha3z]; 

fprintf...
('omega2_ = [%g,%g,%6.3f](rad/s)\n',omega2_) 
fprintf...
('alpha2_ = [%g,%g,%6.3f](rad/s^2)\n',alpha2_) 
fprintf('\n')
fprintf...
('omega3_ = [%g,%g,%6.3f](rad/s)\n',omega3_) 
fprintf...
('alpha3_ = [%g,%g,%6.3f](rad/s^2)\n',alpha3_) 
fprintf('\n')

%%%%%%%%%%%%%%%%%%%
rC1_ = rB_/2;       % position vector of CM of 1
rC2_ = (rB_+rC_)/2; % position vector of CM of 2
rC3_ = (rC_+rD_)/2; % position vector of CM of 3
aC1_ = aB_/2;       % acceleration of C1
aC2_ = (aB_+aC_)/2; % acceleration of C2
aC3_ = (aC_+aD_)/2; % acceleration of C3

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_ = [%g,%g,%6.3f](rad/s^2)\n', alpha1_) 
fprintf...
('alpha2_ = [%g,%g,%6.3f](rad/s^2)\n', alpha2_) 
fprintf...
('alpha3_ = [%g,%g,%6.3f](rad/s^2)\n', alpha3_) 
fprintf('\n')

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

h = 0.01;   % (m) height of the links
d = 0.001;  % (m) depth of the links
rho = 8000; % (kg/m^3) density of the material
g = 9.807;  % (m/s^2) gravitational acceleration

fprintf('Link 1 \n')
m1 = rho*AB*h*d;        % (kg) mass
G1_ = [0,-m1*g,0];      % (N) force of gravity
Fin1_ = -m1*aC1_;       % (N) force of inertia
IC1 = m1*(AB^2+h^2)/12; % (kg m^2) mass moment of inertia about C1
IA = IC1 + m1*(AB/2)^2; % (kg m^2) mass moment of inertia about A
Min1_ = -IC1*alpha1_;   % (N m) moment of inertia
fprintf('m1 = %6.3f (kg)\n', m1)
fprintf('G1_ = - m1 g_ = [%d,%6.3f,%d] (N)\n', G1_)
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('Min1_=-IC1 alpha1_=[%d, %d, %d](N m)\n',Min1_)
fprintf('\n')

fprintf('Link 2 \n')
m2 = rho*BC*h*d;        % (kg) mass
G2_ = [0,-m2*g,0];      % (N) force of gravity
Fin2_ = -m2*aC2_;       % (N) force of inertia
IC2 = m2*(BC^2+h^2)/12; % (kg m^2) mass moment of inertia about C2
Min2_ = -IC2*alpha2_;   % (N m) moment of inertia
fprintf('m2 = %6.3f (kg)\n', m2)
fprintf('G2_ = - m2 g_ = [%d,%6.3f,%d] (N)\n', G2_)
fprintf('Fin2_= - m2 aC2 =[%6.3f,%6.3f,%d] (N)\n', Fin2_)
fprintf('IC2 = %6.3d (kg m^2)\n', IC2)
fprintf('Min2_=-IC2 alpha2=[%d,%d,%6.3d](N m)\n',Min2_)
fprintf('\n')

fprintf('Link 3 \n')
m3 = rho*CD*h*d;        % (kg) mass
G3_ = [0,-m3*g,0];      % (N) force of gravity
Fin3_ = -m3*aC3_;       % (N) force of inertia
IC3 = m3*(CD^2+h^2)/12; % (kg m^2) mass moment of inertia about C3
IC  = IC3+m3*(CD/2)^2;  % (kg m^2) mass moment of inertia about C
Min3_ = -IC3*alpha3_;   % (N m) moment of inertia
fprintf('m3 = %6.3f (kg)\n',m3)
fprintf('G3_ = - m3 g_ = [%g, %6.3f, %g] (N)\n',G3_)
fprintf('Fin3_ = - m3 aC3 =[%6.3f,%6.3f,%d] (N)\n',Fin3_)
fprintf('IC3 = %6.3d (kg m^2)\n', IC3)
fprintf('Min3_=-IC3 alpha3=[%d,%d,%6.3d](N m)\n',Min3_)
fprintf('\n')

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

% total forces
% Dyad RRR (2 & 3)
F2_ = Fin2_ + G2_; % total force on 2 at C1
M2_ = Min2_;       % total moment on 2
F3_ = Fin3_ + G3_; % total force on 3 at C2
M3_ = Min3_+ Me_;  % total moment on 3

% call dforceRRR
% dforceRRR(rAi_,rAj_,rP_,rCi_,rCj_,Fi_,Mi_,Fj_,Mj_)
% out=[FAixs,FAiys,FAjxs,FAjys,Fjix,Fjiy]
% rAi_ = rB_
% rAj_ = rD_
% rP_  = rC_
% rCi_ = rC2_
% rCj_ = rC3_
% Fi_ = F2_
% Mi_ = M2_
% Fj_ = F3_
% Mj_ = M3_
FD23_ = dforceRRR(rB_,rD_,rC_,rC2_,rC3_,F2_,M2_,F3_,M3_);

F12x = FD23_(1);
F12y = FD23_(2);
F03x = FD23_(3);
F03y = FD23_(4);
F32x = FD23_(5);
F32y = FD23_(6);

F12_ = [F12x F12y 0]; % reaction force of 1 on 2 at B
F03_ = [F03x F03y 0]; % reaction force of 0 on 3 at D
F32_ = [F32x F32y 0]; % reaction force of 3 on 2 at C

fprintf('F12_ = [%6.3f,%6.3f,%d] (N)\n',F12_)
fprintf('F03_ = [%6.3f,%6.3f,%d] (N)\n',F03_)
fprintf('F32_ = [%6.3f,%6.3f,%d] (N)\n',F32_)

% forces link 1 
% dforceR(rAi_,rAj_,rCi_,Fi_,Mi_,Fji_)
% rAi_ = rA_
% rAj_ = rB_
% rCi_ = rC1_
% Fi_  = F1
% Mi_  = M1
% Fji_ = F21_=-F12
F1_ = Fin1_ + G1_; % total force on 1
M1_ = Min1_;       % total moment on 1
FD1_ = dforceR(rA_,rB_,rC1_,F1_,M1_,-F12_);
F01x = FD1_(1);
F01y = FD1_(2);
Mm = FD1_(3);
F01_ = [F01x F01y 0]; % reaction force of 1 on 1 at A
Mm_ = [0 0 Mm];  % equilibrium or motor moment on 1

fprintf('F01_ = [%6.3f,%6.3f,%g] (N)\n',F01_)
fprintf('Mm_ = [%g,%g,%6.3f] (N m)\n',Mm_)

% eof

% Results 
% 
% phi = phi1 = 60 (degrees) 
% rA_ = [0, 0, 0] (m)
% rD_ = [0.3, 0, 0] (m)
% rB_ = [ 0.075,  0.130, 0] (m)
% vB_ = vB1_ = vB2_ = [-0.816,  0.471, 0] (m/s)
% aB_ = aB1_ = aB2_ = [-2.961, -5.128, 0] (m/s^2)
% 
% rC_ = [ 0.424,  0.157, 0] (m)
% phi2 =  4.433 (degrees) 
% phi3 = 51.701 (degrees) 
% 
% vC_ = [-0.831,  0.656, 0] (m/s)
% aC_ = [-3.093, -4.693, 0] (m/s^2)
% 
% omega2_ = [0,0, 0.529](rad/s)
% alpha2_ = [0,0, 1.270](rad/s^2)
% 
% omega3_ = [0,0, 5.291](rad/s)
% alpha3_ = [0,0,-2.406](rad/s^2)
% 
% rC1_ = [ 0.038, 0.065,0] (m)
% rC2_ = [ 0.249, 0.143,0] (m)
% rC3_ = [ 0.362, 0.078,0] (m)
% aC1_ = [-1.480,-2.564,0] (m/s^2)
% aC2_ = [-3.027,-4.911,0] (m/s^2)
% aC3_ = [-1.546,-2.346,0] (m/s^2)
% alpha1_ = [0,0, 0.000](rad/s^2)
% alpha2_ = [0,0, 1.270](rad/s^2)
% alpha3_ = [0,0,-2.406](rad/s^2)
% 
% 
% Dynamic force analysis 
% Link 1 
% m1 =  0.012 (kg)
% G1_ = - m1 g_ = [0,-0.118,0] (N)
% Fin1_= - m1 aC1_ = [ 0.018, 0.031,0] (N)
% IC1 = 2.26e-05 (kg m^2)
% IA = 9.01e-05 (kg m^2)
% Min1_=-IC1 alpha1_=[0, 0, 0](N m)
% 
% Link 2 
% m2 =  0.028 (kg)
% G2_ = - m2 g_ = [0,-0.275,0] (N)
% Fin2_= - m2 aC2 =[ 0.085, 0.137,0] (N)
% IC2 = 2.861e-04 (kg m^2)
% Min2_=-IC2 alpha2=[0,0,-3.633e-04](N m)
% 
% Link 3 
% m3 =  0.016 (kg)
% G3_ = - m3 g_ = [0, -0.157, 0] (N)
% Fin3_ = - m3 aC3 =[ 0.025, 0.038,0] (N)
% IC3 = 5.347e-05 (kg m^2)
% Min3_=-IC3 alpha3=[0,0,1.286e-04](N m)
% 
% Me_ = [0, 0, -100.000] (N m)
% 
% F12_ = [-678.875,-52.561,0] (N)
% F03_ = [678.766,52.818,0] (N)
% F32_ = [678.791,52.699,0] (N)
% F01_ = [-678.893,-52.474,0] (N)
% Mm_ = [0,0,84.251] (N m)