% Program 3.1
% R-RRT
% Velocity and acceleration analysis
clear; clc; close;
AB=1; BC=1;
phi_input = 45; % input angle in degrees
phi = phi_input*(pi/180); % convert to radians
xA = 0; yA = 0; rA = [xA yA 0];                       % position of A 
xB = AB*cos(phi); yB = AB*sin(phi); rB = [xB yB 0];   % position of B 
yC = 0; xC = xB+sqrt(BC^2-(yC-yB)^2); rC = [xC yC 0]; % position of C 
phi2 = atan((yB-yC)/(xB-xC));

fprintf('Results \n'); fprintf(' \n');
fprintf('phi = phi1 = %g (degrees) \n', phi_input);
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 = 45 (deg)'),...  
text(xA,yA,'  A'),text(xB,yB,'  B'),text(xC,yC,'  C'),...  

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

n = 30/pi; % (rpm)  driver link
omega1 = [ 0 0 pi*n/30 ]; % (rad/s) angular velocity of the driver link 1
alpha1 = [0 0 0 ]; % (rad/s^2) angular acceleration of the driver link 1
fprintf('omega1 =  [ %g, %g, %g ] (rad/s)\n', omega1);
fprintf('alpha1 =  [ %g, %g, %g ] (rad/s^2)\n', alpha1 ); 
fprintf('\n');

vA = [0 0 0 ]; % (m/s) velocity of A (fixed)
aA = [0 0 0 ]; % (m/s^2) acceleration of A
% A and B=B1 are two points on the rigid link 1
vB1 = vA + cross(omega1,rB); % velocity of B
vB2 = vB1; % between 1 & 2 there is a rotational joint B_R
aB1 = aA + cross(alpha1,rB) - dot(omega1,omega1)*rB; % acceleration of B
aB2 = aB1;
fprintf('vB = vB1 = vB2 =  [ %g, %g, %g ] (m/s)\n', vB1);
fprintf('aB = aB1 = aB2 =  [ %g, %g, %g ] (m/s^2)\n', aB1);
fprintf('\n');

% velocity of C
% sym  constructs symbolic numbers, variables and objects
% 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
eqvC = vC - (vB2 + cross(omega2,rC-rB)); % B2 & C points on link 2
% 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];

% 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('\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];

% 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('\n');