% Velocity and acceleration analysis

% R-RRT
clear all; clc; close all
AB=1; BC=1;
phi = pi/6; % input angle
xA=0;yA=0;rA=[xA yA 0];
xB=AB*cos(phi); yB = AB*sin(phi);
rB = [xB yB 0];
yC = 0; xC = xB+sqrt(BC^2-(yC-yB)^2);
rC = [xC yC 0];
phi2 = atan((yB-yC)/(xB-xC));
fprintf('Results \n\n')
fprintf('phi = phi1 = %g (degrees) \n', phi*180/pi)
fprintf('rA = [ %g, %g, %g ] (m)\n', rA)
fprintf('rB = [ %g, %g, %g ] (m)\n', rB)
fprintf('rC = [ %g, %g, %g ] (m)\n', rC)
fprintf('phi2 = %g (degrees) \n', phi2*180/pi)
% Graphic of the mechanism
plot([0,xB],[0,yB],'r-o',[xB,xC],[yB,yC],'b-o'),...
xlabel('x (m)'), ylabel('y (m)'),...
title('positions for \phi = \pi/6 (rad)'),...
text(xA,yA,' A'),text(xB,yB,' B'),...
text(xC,yC,' C')
fprintf('\n')
fprintf('Velocity and acceleration analysis \n\n')
% angular velocity of the driver link 1
omega1 = [0 0 1 ]; % (rad/s)
% velocity of A (fixed)
vA=[0 0 0];%(m/s)
% A and B=B1 are two points on the rigid link 1
vB1 = vA + cross(omega1,rB); % velocity of B1
% between1&2there is a rotational joint B_R
vB2 = vB1;
vB = norm(vB1);
% norm() is the vector norm
fprintf('omega1 = [ %g, %g, %g ] (rad/s)\n', omega1)
fprintf('vB=vB1=vB2 = [ %g, %g, %g ] (m/s)\n', vB1)
fprintf('|vB|= %g (m/s)\n', vB)
% velocity of C
% sym constructs symbolic numbers and variables
% sym('x','real') also assumes that x is real
omega2z=sym('omega2z','real');
vCx=sym('vCx','real');
omega2=[0 0 omega2z];
vC=[vCx 0 0];
% vC = vB + omega2 x rBC (B2 & C points on link 2)
eqvC = vC - (vB2 + cross(omega2,rC-rB));
% vectorial equation
eqvCx = eqvC(1); % equation component on x-axis
eqvCy = eqvC(2); % equation component on y-axis
solvC = solve(eqvCx,eqvCy);
omega2zs=eval(solvC.omega2z);
vCxs=eval(solvC.vCx);
Omega2 = [0 0 omega2zs];
VC = [vCxs 0 0];
vCB = cross(Omega2,rC-rB);
% print the equations for calculating
% omega2z and vCx
fprintf('vC = vB + omega2 x rBC => \n')
qvCx=vpa(eqvCx,6);
% vpa(S,D) uses variable-precision arithmetic (vpa)
% to compute each element of S to D decimal digits
% of accuracy
fprintf('x-axis: %s = 0 \n', char(qvCx))
% char() creates character array (string)
qvCy=vpa(eqvCy,6);
fprintf('y-axis: %s = 0 \n', char(qvCy))
fprintf('=>\n')
fprintf('omega2z = %g (rad/s)\n', omega2zs)
fprintf('vCx = %g (m/s)\n', vCxs)
fprintf('\n')
fprintf('omega2 = [ %g, %g, %g ] (rad/s)\n', Omega2)
fprintf('vC = [ %g, %g, %g ] (m/s)\n', VC)
fprintf('vCB = [ %g, %g, %d ] (m/s)\n', vCB)
fprintf('\n')
% acceleration of B
aA=[0 0 0];%(m/s^2)
% angular acceleration aplpha 1
alpha1=[0 0 -1];%(rad/s^2)
% acceleration of B
aB1 = aA + cross(alpha1,rB) - dot(omega1,omega1)*rB;
aB2 = aB1;
aBn = - dot(omega1,omega1)*rB;
aBt = cross(alpha1,rB);
fprintf('aB=aB1=aB2 = [%g, %g, %g] (m/s^2)\n', aB1)
fprintf('aBn = [ %g, %g, %d ] (m/s^2)\n', aBn)
fprintf('aBt = [ %g, %g, %g ] (m/s^2)\n', aBt)
fprintf('\n')
% acceleration of C
alpha2z=sym('alpha2z','real');
aCx=sym('aCx','real');
alpha2=[0 0 alpha2z]; % alpha3z unknown
aC=[aCx 0 0];%aCx unknown
eqaC=aC-(aB1+cross(alpha2,rC-rB)-...
dot(Omega2,Omega2)*(rC-rB));
eqaCx = eqaC(1); % equation component on x-axis
eqaCy = eqaC(2); % equation component on y-axis
solaC = solve(eqaCx,eqaCy);
alpha2zs=eval(solaC.alpha2z);
aCxs=eval(solaC.aCx);
Alpha2 = [0 0 alpha2zs];
aCs = [aCxs 0 0];
aCB=cross(Alpha2,rC-rB)-dot(Omega2,Omega2)*(rC-rB);
aCBn=-dot(Omega2,Omega2)*(rC-rB);
aCBt=cross(Alpha2,rC-rB);
% print the equations for calculating alpha2z and aCx
fprintf...
('aC = aB + omega2 x rBC - (omega2.omega2)rBC =>\n')
qaCx=vpa(eqaCx,6);
fprintf('x-axis: %s = 0 \n', char(qaCx))
qaCy=vpa(eqaCy,6);
fprintf('y-axis: %s = 0 \n', char(qaCy))
fprintf('=>\n')
fprintf('alpha2z = %g (rad/s^2)\n', alpha2zs)
fprintf('aCx = %g (m/s^2)\n', aCxs)
fprintf('\n')
fprintf('alpha2 = [%g, %g, %g] (rad/s^2)\n', Alpha2)
fprintf('aC = [ %g, %g, %g ] (m/s^2)\n', aCs)
fprintf('aCB = [ %g, %g, %d ] (m/s^2)\n', aCB)
fprintf('aCBn = [ %g, %g, %d ] (m/s^2)\n', aCBn)
fprintf('|aCBn| = %g (m/s^2)\n', norm(aCBn))
fprintf('aCBt = [ %g, %g, %d ] (m/s^2)\n', aCBt)
fprintf('|aCBt| = %g (m/s^2)\n', norm(aCBt))
fprintf('\n')
% end of program