% R-RTR Mechanism
% Dyad method

clear; clc; close all

AB = 0.10 ;  % (m) link 1
% link 2 is the slider at B
AC = 0.05 ;  % (m) link 0
CD = 0.15 ;  % (m) link 3

phi = pi/6;  % (rad) angle of link 1 with x-axis
n = -50;     % (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 C (fixed)
xC = AC ; yC = 0; 
rC_ = [xC yC 0]; % position of C
vC_ = [0 0 0];   % velocity of C 
aC_ = [0 0 0];   % acceleration of C 

% call pvaR function 
% point B=B1=B2  
% 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 of B

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

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

% rB_ = [ 0.087, 0.050,0] (m)
% vB_ = [ 0.262, -0.453, 0] (m/s)
% aB_ = [-2.374, -1.371, 0] (m/s^2)

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

% position of joint D
% call pos3P function 
% C, B, and D same direction
% pos3P(rP_, rQ_ , PQ)
% rP_ = rC_
% rQ_ = rB_
% PQ = CD: C->B->D
posD = pos3P(rC_, rB_, CD);
xD = posD(1);
yD = posD(2);
rD_ = [xD yD 0];

% rD_ = [ 0.139, 0.121,0] (m)
 
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_ = rB1_ = rB2_ = [%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;
ax1 = 0.05;
axis([-ax1 ax -ax1 ax])
xlabel('x (m)'), ylabel('y (m)')

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 = 14;
slider_scale_factor = 0.015;
pin_scale_factor = 0.005;        
         
% plotting joint A
plot_pivot0(xA,yA,pin_scale_factor,0)
% labelling joint A
text(textAx, textAy,'A','FontSize',text_size)
% plotting link 1
plot([xA xB], [yA yB], 'r-o','LineWidth',2);
% 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_pivot0(xC,yC,pin_scale_factor,0)
% labelling joint C
text( textCx, textCy,'C','FontSize',text_size)        
% plotting link 3
plot([xC xD], [yC yD], 'k-o','LineWidth',2);
% labelling joint D 
text(textDx, textDy,'D','FontSize',text_size)

% 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)

fprintf('\n')
fprintf('Velocity and acceleration analysis \n\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];  % angular velocity of link 2
alpha2_ = [0 0 alpha2z];  % angular acceleration of link 2
omega3_ = alpha2_; % angular velocity of link 3
alpha3_ = alpha2_; % angular acceleration of link 3 

% angular velocity and acceleration D
% linvelacc(rP_,rQ_,vP_,aP_,omega,alpha)
% rP_ = rC_
% rQ_ = rD_
% vP_ = vC_
% aP_ = aC_
% omega = omega2z
% alpha = alpha2z
vaD = linvelacc(rC_,rD_,vC_,aC_,omega2z,alpha2z);
vDx = vaD(1);
vDy = vaD(2);
aDx = vaD(3);
aDy = vaD(4);
vD_ = [vDx vDy 0]; % velocity of D 
aD_ = [aDx aDy 0]; % acceleration of D  
% vD_ = [ 0.936,-0.685,0] (m/s)
% aD_ = [-1.077,-10.324,0] (m/s^2)


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

%%%%%%%%%%%%%%%%%%%
rC1_ = rB_/2;       % position vector of CM of 1
rC2_ = rB_;         % position vector of CM of 2
rC3_ = (rD_+rC_)/2; % position vector of CM of 3
aC1_ = aB_/2;       % acceleration of C1
aC2_ = aB_;         % acceleration of C2
aC3_ = (aD_+aC_)/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
hSlider = 0.02; % (m) height of the slider 
wSlider = 0.04; % (m)depth of the slider 
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*hSlider*wSlider*d; % (kg) mass
G2_ = [0,-m2*g,0];          % (N) force of gravity
Fin2_ = -m2*aC2_;           % (N) force of inertia
IC2 = m2*(hSlider^2+wSlider^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
ID = IC3+m3*(CD/2)^2;  % (kg m^2) mass moment of inertia about D 
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')

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

% dforceRTR(rAi_,rAj_,rCi_,Fi_,Mi_,Fj_,Mj_)
% i=(3)  j=(2)
% rAi_ = rC_
% rAj_ = rB_
% rCi_ = rC3
% Fi_ = F3_
% Mi_ = M3_
% Fj_ = F2_
% Mj_ = M2_

FD23=dforceRTR(rC_,rB_,rC3_,F3_,M3_,F2_,M2_);
F03x = FD23(1);
F03y = FD23(2);
F12x = FD23(3);
F12y = FD23(4);
F32x = FD23(5);
F32y = FD23(6);
M32z = FD23(7);

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

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


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 0 on 1 at A
Mm_ = [0 0 Mm]; % equilibrium (motor) moment

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

% Results 
% 
% phi = phi1 = 30 (degrees) 
% rA_ = [ 0.000, 0.000,0] (m)
% rC_ = [ 0.050, 0.000,0] (m)
% rB_ = rB1_ = rB2_ = [ 0.087, 0.050,0] (m)
% phi2 = phi3 = 53.794 (degrees) 
% rD_ = [ 0.139, 0.121,0] (m)
% 
% Velocity and acceleration analysis 
% 
% 
% vB_ = [ 0.262, -0.453, 0] (m/s)
% aB_ = [-2.374, -1.371, 0] (m/s^2)
% omega2_ = omega3_ = [0,0,-7.732](rad/s)
% alpha2_ = alpha3_ = [0,0,-34.865](rad/s^2)
% 
% aD_ = [-1.077, -10.324, 0] (m/s^2)
% vD_ = [ 0.936, -0.685, 0] (m/s)
% 
% 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 
% m1 =  0.008 (kg)
% G1_ = - m1 g_ = [0,-0.078,0] (N)
% Fin1_= - m1 aC1_ = [ 0.009, 0.005,0] (N)
% IC1 = 6.73333e-06 (kg m^2)
% IA = 2.67333e-05 (kg m^2)
% Min1_=-IC1 alpha1_=[0, 0, 0](N m)
% 
% Link 2 
% m2 =  0.006 (kg)
% G2_ = - m2 g_ = [0,-0.063,0] (N)
% Fin2_= - m2 aC2 =[ 0.015, 0.009,0] (N)
% IC2 = 1.067e-06 (kg m^2)
% Min2_=-IC2 alpha2=[0,0,3.719e-05](N m)
% 
% Link 3 
% m3 =  0.012 (kg)
% G3_ = - m3 g_ = [0, -0.118, 0] (N)
% Fin3_ = - m3 aC3 =[ 0.006, 0.062,0] (N)
% IC3 = 2.260e-05 (kg m^2)
% 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)
% F32_ = [-1302.143,953.235,0] (N)
% M32_ = [0,0,-0.000037] (N m)
% 
% F01_ = [1302.118,-953.108,0] (N)
% Mm_ = [0,0,-147.651] (N m)
% 
