% R_RRT_RTR Mechanism
% position analysis
% classical method

clear; clc; close all

PIV_1; % position analysis PIV_1.m

fprintf('\n')
fprintf('Velocity and acceleration analysis \n\n') 

n = -500; % rpm
omega1_ = [0 0 pi*n/30]; alpha1_ = [0 0 0]; 
fprintf...
('omega1_ = [%g, %g, %6.3f] (rad/s)\n', omega1_) 
fprintf...
('alpha1_ = [%g, %g, %g] (rad/s^2)\n', alpha1_)  
fprintf('\n') 

vA_ = [0 0 0 ]; aA_ = [0 0 0 ];

vB1_ = vA_ + cross(omega1_,rB_); 
vB2_ = vB1_; 
aB1_ = aA_ + cross(alpha1_,rB_) - ...
    dot(omega1_,omega1_)*rB_;
vB2_ = vB1_; vB_ = vB1_; 
aB2_ = aB1_; aB_ = aB1_;
fprintf...
('vB_ = vB1_ = vB2_ = [%6.3f, %6.3f, %g] (m/s)\n',vB1_)  
fprintf...
('aB_ = aB1_ = aB2_ = [%6.3f, %6.3f, %g] (m/s^2)\n',aB1_)  
fprintf('\n')

syms omega2z vD
omega2_ = [ 0 0 omega2z ]; 
% omega2z unknown (to be calculated)
% vD unknown (to be calculated)
% vD2_ = vB_ + omegaB_ x rBD_   
% (D2 & B are points on link 2)
vD2_ = vB1_ + cross(omega2_,rD_-rB_); 
% point D2 is on link 2 and point D3 is on link 3
% vD3_ = vD2_
% vD3 is parallel to the sliding x-direction 
vD3_ = [vD 0 0]; 
eqvD_ = vD3_ - vD2_; 
% vectorial equation
% the component of the vectorial equation on x-axis
eqvDx = eqvD_(1); 
% the component of the vectorial equation on y-axis
eqvDy = eqvD_(2); 
% two equations eqvBx and eqvBy with two unknowns 
% solve for omega2z and vD 
solvD = solve(eqvDx,eqvDy); 
omega2=eval(solvD.omega2z); 
vDs=eval(solvD.vD);

omega2_ = [0 0 omega2]; 
vD_ = [vDs 0 0];
% print the equations for calculating 
% omega2 and vD
fprintf...
('vD2_ = vB_ + omega2_ x rBD_ = vD3_  => \n')
digits(3)
qvDx=vpa(eqvDx);
fprintf('x-axis: %s = 0 \n', char(qvDx)) 
qvDy=vpa(eqvDy);
fprintf('y-axis: %s = 0 \n', char(qvDy)) 
fprintf('=>\n') 
fprintf('omega2z = %6.3f (rad/s)\n', omega2) 
fprintf('vD = %6.3f (m/s)\n', vDs) 
fprintf('\n') 
fprintf...
('omega2_ = [%g,%g,%6.3f](rad/s)\n', omega2_) 
fprintf('vD_ = [%6.3f,%6.3f,%d] (m/s)\n\n', vD_)  

syms alpha2z aD
alpha2_ = [ 0 0 alpha2z ]; % alpha3z unknown
% aD2 acceleration of D
aD2_ = aB_ + cross(alpha2_, rD_-rB_) -...
    dot(omega2_, omega2_)*(rD_-rB_); 
% aD3 parallel to the sliding direcion x
aD3_ = [aD 0 0]; 
% aD3_ = aD2 
eqaD_ = aD3_ - aD2_; 
% vectorial equation
eqaDx = eqaD_(1); % equation component on x-axis
eqaDy = eqaD_(2); % equation component on y-axis
solaD = solve(eqaDx,eqaDy);
alpha2zs=eval(solaD.alpha2z);
aDs=eval(solaD.aD);
alpha2_ = [0 0 alpha2zs];
aD_ = [aDs 0 0];

% print the equations for calculating 
% alpha2 and aD
fprintf('\n') 
fprintf('aD2_=aB_+alpha2_xrBD_-(omega2_.omega2_)rBD_\n') 
fprintf('aD3_=aD2_ =>\n') 
qaDx=vpa(eqaDx);
fprintf('x-axis:\n') 
fprintf('%s = 0\n',char(qaDx)) 
qaDy=vpa(eqaDy);
fprintf('y-axis:\n') 
fprintf('%s = 0\n',char(qaDy))  
fprintf('=>\n');
fprintf('alpha2z = %6.3f (rad/s^2)\n', alpha2zs) 
fprintf('aD = %6.3f (m/s^2)\n', aDs) 
fprintf('\n') 
fprintf...
('alpha2_ = [%g,%g,%6.3f](rad/s^2)\n', alpha2_) 
fprintf('aD_ = [%6.3f,%6.3f,%d] (m/s^2)\n\n', aD_) 

alpha3_ = [0 0 0];
fprintf...
('alpha3_ = [%g,%g,%g](rad/s^2)\n', alpha3_) 

% vC velocity of C
% C & B points on link 2
vC_ = vB_ + cross(omega2_,rC_-rB_);
fprintf('vC_=vC2_=vC4_=[%6.3f,%6.3f,%g] (m/s)\n',vC_) 
% aC acceleration of C
aC_ = aB_ + cross(alpha2_, rC_-rB_) + ...
    cross(omega2_,cross(omega2_, rC_-rB_));
fprintf('aC_=aC2_=aC4_=[%6.3f,%6.3f,%g] (m/s^2)\n',aC_) 
fprintf('\n')

syms omega4z vE45
omega4_ = [ 0 0 omega4z ]; 
% omega4z unknown (to be calculated)
% vE45 unknown (to be calculated)
vE5_ = [0 0 0 ]; % E is fixed
% vE4_ = vC_ + omega4_ x rCE_   
% (E4 & C are points on link 4)
vE4_ = vC_ + cross(omega4_,rE_-rC_); 
% point E4 is on link 4 and point E5 is on link 5
% vE4_ = vE5_ + vE4E5_
% between the links 4 and 5 there is a  
% translational joint E_T
% vE4E5 is the relative velocity of B4 wrt 5
% vE4E5 is parallel to the sliding direcion EC
% vE4E5 is written as a vector
vE4E5_ = vE45*[ cos(phi4) sin(phi4) 0]; 
% vE4_ = vE5_ + aE45_
eqvE_ = vE4_ - vE5_ - vE4E5_; 
% vectorial equation
% the component of the vectorial equation on x-axis
eqvEx = eqvE_(1); 
% the component of the vectorial equation on y-axis
eqvEy = eqvE_(2); 
% two equations eqvBx and eqvEy with two unknowns 
% solve for omega4z and vE45 
solvE = solve(eqvEx,eqvEy); 
omega4zs=eval(solvE.omega4z); 
vE45s=eval(solvE.vE45);

omega4_ = [0 0 omega4zs]; 
omega5_ = omega4_;
vE45_ = vE45s*[cos(phi4) sin(phi4) 0];

% print the equations for calculating 
% omega4 and vE45
fprintf...
('vE4_ = vC_ + omega4_ x rCE_ = vE5_ + vE45_ => \n')
digits(3)
qvEx=vpa(eqvEx);
fprintf('x-axis: %s = 0 \n', char(qvEx)) 
qvEy=vpa(eqvEy);
fprintf('y-axis: %s = 0 \n', char(qvEy)) 
fprintf('=>\n') 
fprintf('omega4z = %6.3f (rad/s)\n', omega4zs) 
fprintf('vE45 = %6.3f (m/s)\n', vE45s) 
fprintf('\n') 
fprintf...
('omega4_=omega5_ = [%g,%g,%6.3f](rad/s)\n', omega4_) 
fprintf('vE45_ = [%6.3f,%6.3f,%d] (m/s)\n\n', vE45_)  

% Coriolis acceleration
aE45cor_ = 2*cross(omega4_,vE45_);
fprintf('aE45cor_ = [%6.3g,%6.3g,%d] (m/s^2)\n\n',aE45cor_)   

syms alpha4z aE45
alpha4_ = [ 0 0 alpha4z ]; 
% alpha4z unknown (to be calculated)
% aE45 unknown (to be calculated)
aE5_ = [0 0 0 ]; % E is fixed
% aE4_=aC_+alpha4_xrCE_+omega4_x(omega4_xrCE_)
% (E4 & C are points on link 4)
aE4_=aC_+cross(alpha4_,rE_-rC_)+...
cross(omega4_,cross(omega4_,rE_-rC_)); 
% point E4 is on link 4 and point E5 is on link 5
% aE4_ = aE5_ + aE45_ + aE45cor_;
% between the links 4 and 5 there is a  
% translational joint E_T
% aE4E5 is the relative velocity of E4 wrt 5
% aE4E5 is parallel to the sliding direcion EC
% aE4E5 is written as a vector
aE4E5_ = aE45*[ cos(phi4) sin(phi4) 0]; 
% aE4_ = aE5_ + aE45_ + aE45cor_
eqaE_ = aE4_ - aE5_ - aE4E5_ - aE45cor_; 
% vectorial equation
% the component of the vectorial equation on x-axis
eqaEx = eqaE_(1); 
% the component of the vectorial equation on y-axis
eqaEy = eqaE_(2); 
% two equations eqvBx and eqvEy with two unknowns 
% solve for omega4z and aE45 
solaE = solve(eqaEx,eqaEy); 
alpha4zs=eval(solaE.alpha4z); 
aE45s=eval(solaE.aE45);

alpha4_ = [0 0 alpha4zs]; 
aE45_ = aE45s*[ cos(phi4) sin(phi4) 0]; 

% print the equations for calculating 
% alpha4 and aE45
fprintf...
('aE4_=vC_+alpha4_xrCE_+omega4_x(omega4_xrCE_)=\n')
fprintf...
('aE4_=aE5_+aE45_+aE45cor_=>\n')
digits(3)
qaEx=vpa(eqaEx);
fprintf('x-axis: %s = 0 \n', char(qaEx)) 
qaEy=vpa(eqaEy);
fprintf('y-axis: %s = 0 \n', char(qaEy)) 
fprintf('=>\n') 
fprintf('alpha4z = %6.3f (rad/s^2)\n', alpha4zs) 
fprintf('aE45 = %6.3f (m/s^2)\n', aE45s) 
fprintf('\n') 

alpha5_=alpha4_;
fprintf...
('alpha4_=alpha5_ = [%g,%g,%6.3f](rad/s^2)\n', alpha4_) 
fprintf('aE45_ = [%6.3f,%6.3f,%d] (m/s^2)\n\n', aE45_) 

vF_ = vC_ + cross(omega4_,rF_-rC_); 
aF_=aC_+cross(alpha4_,rF_-rC_)+...
cross(omega4_,cross(omega4_,rF_-rC_)); 
fprintf('vF_=[%6.3f,%6.3f,%g] (m/s)\n',vF_) 
fprintf('aF_=[%6.3f,%6.3f,%g] (m/s^2)\n',aF_) 
fprintf('\n') 

% eof

% Results 
% 
% Velocity and acceleration analysis 
% 
% omega1_ = [0, 0, -52.360] (rad/s)
% alpha1_ = [0, 0, 0] (rad/s^2)
% 
% vB_ = vB1_ = vB2_ = [ 7.405, -7.405, 0] (m/s)
% aB_ = aB1_ = aB2_ = [-387.715, -387.715, 0] (m/s^2)
% 
% vD2_ = vB_ + omega2_ x rBD_ = vD3_  => 
% x-axis: vD - 0.0914*omega2z - 7.4 = 0 
% y-axis: 7.4 - 0.593*omega2z = 0 
% =>
% omega2z = 12.487 (rad/s)
% vD =  8.546 (m/s)
% 
% omega2_ = [0,0,12.487](rad/s)
% vD_ = [ 8.546, 0.000,0] (m/s)
% 
% 
% aD2_=aB_+alpha2_xrBD_-(omega2_.omega2_)rBD_
% aD3_=aD2_ =>
% x-axis:
% aD - 0.0914*alpha2z + 480.0 = 0
% y-axis:
% 373.0 - 0.593*alpha2z = 0
% =>
% alpha2z = 629.786 (rad/s^2)
% aD = -422.604 (m/s^2)
% 
% alpha2_ = [0,0,629.786](rad/s^2)
% aD_ = [-422.604, 0.000,0] (m/s^2)
% 
% alpha3_ = [0,0,0](rad/s^2)
% vC_=vC2_=vC4_=[ 7.804,-4.813,0] (m/s)
% aC_=aC2_=aC4_=[-399.926,-252.015,0] (m/s^2)
% 
% vE4_ = vC_ + omega4_ x rCE_ = vE5_ + vE45_ => 
% x-axis: 0.359*omega4z - 0.135*vE45 + 7.8 = 0 
% y-axis: - 0.049*omega4z - 0.991*vE45 - 4.81 = 0 
% =>
% omega4z = -23.109 (rad/s)
% vE45 = -3.716 (m/s)
% 
% omega4_=omega5_ = [0,0,-23.109](rad/s)
% vE45_ = [-0.502,-3.681,0] (m/s)
% 
% aE45cor_ = [  -170,  23.2,0] (m/s^2)
% 
% aE4_=vC_+alpha4_xrCE_+omega4_x(omega4_xrCE_)=
% aE4_=aE5_+aE45_+aE45cor_=>
% x-axis: 0.359*alpha4z - 0.135*aE45 - 204.0 = 0 
% y-axis: - 0.991*aE45 - 0.049*alpha4z - 83.3 = 0 
% =>
% alpha4z = 525.219 (rad/s^2)
% aE45 = -109.981 (m/s^2)
% 
% alpha4_=alpha5_ = [0,0,525.219](rad/s^2)
% aE45_ = [-14.847,-108.974,0] (m/s^2)
% 
% vF_=[-2.500,-3.409,0] (m/s)
% aF_=[-133.299,-45.808,0] (m/s^2)
