% Program 3.2
% R-RRR-RRT
% Velocity and acceleration analysis
clear; clc; close;
AB=0.15; BC=0.40; CD=0.37; CE=0.23; EF=CE; La=0.30; Lb=0.45; Lc=CD;	
phi = pi/6 ;
xA = 0; yA = 0; rA = [xA yA 0]; 
xD = La ; yD = Lb ; rD = [xD yD 0]; 
xB = AB*cos(phi); yB = AB*sin(phi); rB = [xB yB 0]; 
% Position of joint C 
eqnC1 = '( xCsol - xB )^2 + ( yCsol - yB )^2 = BC^2 ';
eqnC2 = '( xCsol - xD )^2 + ( yCsol - yD )^2 = CD^2 ';
solC = solve(eqnC1, eqnC2, 'xCsol, yCsol');
xCpositions = eval(solC.xCsol); yCpositions = eval(solC.yCsol);
xC1 = xCpositions(1); xC2 = xCpositions(2); 
yC1 = yCpositions(1); yC2 = yCpositions(2); 
if xC1 < xD xC = xC1; yC=yC1; else xC = xC2; yC=yC2; end
rC = [xC yC 0];  
% Position of joint E 
eqnE1 = '( xEsol - xC )^2 + ( yEsol - yC )^2 = CE^2 ';
eqnE2 = '( yD - yC ) / ( xD - xC ) = ( yEsol - yC ) / ( xEsol - xC )';
solE = solve(eqnE1, eqnE2, 'xEsol, yEsol');
xEpositions = eval(solE.xEsol); yEpositions = eval(solE.yEsol);
xE1 = xEpositions(1); xE2 = xEpositions(2); 
yE1 = yEpositions(1); yE2 = yEpositions(2); 
if xE1 < xC xE = xE1; yE=yE1; else xE = xE2; yE=yE2; end
rE = [xE yE 0]; 
% Position of joint F 
xF = - Lc ;
eqnF = '( xF - xE )^2 + ( yFsol - yE )^2 = EF^2 ';
solF = solve(eqnF,'yFsol');
yFpositions=eval(solF);
yF1 = yFpositions(1); yF2 = yFpositions(2); 
if yF1 < yE yF=yF1; else yF=yF2; end
rF = [xF yF 0]; 
% Angles of the links with the horizontal
phi2 = atan((yB-yC)/(xB-xC)); phi3 = atan((yD-yC)/(xD-xC));
phi4 = atan((yF-yE)/(xF-xE));
fprintf('Results \n'); fprintf('\n');
fprintf('rA =  [ %g, %g, %g ] (m)\n', rA);
fprintf('rD =  [ %g, %g, %g ] (m)\n', rD);
fprintf('rB =  [ %g, %g, %g ] (m)\n', rB);
fprintf('rC =  [ %g, %g, %g ] (m)\n', rC);
fprintf('rE =  [ %g, %g, %g ] (m)\n', rE);
fprintf('rF =  [ %g, %g, %g ] (m)\n', rF);
fprintf('phi2 = %g (degrees) \n', phi2*180/pi);
fprintf('phi3 = %g (degrees) \n', phi3*180/pi);
fprintf('phi4 = %g (degrees) \n', phi4*180/pi);
% Graph of the mechanism
plot([xA,xB],[yA,yB],'r-o','LineWidth',1.5), hold on 
plot([xB,xC],[yB,yC],'b-o','LineWidth',1.5), hold on 
plot([xD,xE],[yD,yE],'g-o','LineWidth',1.5), hold on 
plot([xE,xF],[yE,yF],'b-o','LineWidth',1.5), grid on,... 
xlabel('x (m)'), ylabel('y (m)'),...
title('positions for \phi = 45 (deg)'),...
text(xA,yA,'\leftarrow A = ground','HorizontalAlignment','left'),...
text(xB,yB,'  B'),...
text(xC,yC,'\leftarrow C = ground','HorizontalAlignment','left'),...
text(xD,yD,'\leftarrow D = ground','HorizontalAlignment','left'),...
text(xE,yE,'  E'), text(xF,yF,'  F')

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

n = 60; % (rpm)  driver link
omega1 = [ 0 0 pi*n/30 ]; alpha1 = [0 0 0 ]; 
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 ]; aA = [0 0 0 ]; 
vB1 = vA + cross(omega1,rB); vB2 = vB1; 
aB1 = aA + cross(alpha1,rB) - dot(omega1,omega1)*rB; 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');

vD = [0 0 0 ]; aD = [0 0 0 ]; 

% velocity of C
omega2z = sym('omega2z','real');
omega3z = sym('omega3z','real');
omega2 = [ 0 0 omega2z ];
omega3 = [ 0 0 omega3z ];
% vC = vC2 = vB + omega2 x rBC  (B2 & C points on link 2)
% vC = vC3 = vD + omega3 x rDC  (B3 & D points on link 3)
% vC = vC2 = vC3
eqvC = vB2 + cross(omega2,rC-rB) - ( vD + cross(omega3,rC-rD) );
eqvCx = eqvC(1); % equation component on x-axis
eqvCy = eqvC(2); % equation component on y-axis
solvC = solve(eqvCx,eqvCy);
omega2zs=eval(solvC.omega2z);
omega3zs=eval(solvC.omega3z);
Omega2 = [0 0 omega2zs];
Omega3 = [0 0 omega3zs];

% print the equations for calculating omega2z and omega3z
fprintf('vC = vB + omega2 x rBC = vD + omega3 x rDC  => \n');
qvCx=vpa(eqvCx,6); fprintf('x-axis: %s = 0 \n', char(qvCx));
qvCy=vpa(eqvCy,6); fprintf('y-axis: %s = 0 \n', char(qvCy));
fprintf('=>\n');
fprintf('omega2z =  %g (rad/s)\n', omega2zs);
fprintf('omega3z =  %g (rad/s)\n', omega3zs);
fprintf('\n');
fprintf('omega2 = [ %g, %g, %g ] (rad/s)\n', Omega2 );
fprintf('omega3 = [ %g, %g, %g ] (rad/s)\n', Omega3 ); fprintf('\n');

vC = vB2 + cross(Omega2,rC-rB);
fprintf('vC =  [ %g, %g, %g ] (m/s)\n', vC ); fprintf('\n'); 

vE = vD + cross(Omega3,rE-rD);
fprintf('vE =  [ %g, %g, %g ] (m/s)\n', vE ); fprintf('\n');

% velocity of F
omega4z = sym('omega4z','real');
vFy = sym('vFy','real');
omega4 = [ 0 0 omega4z ];
vF = [ 0 vFy 0 ];
% vF = vE + omega4 x rEF
eqvF = vF - (vE + cross(omega4,rF-rE)); % E & F points on link 4
eqvFx = eqvF(1); % equation component on x-axis
eqvFy = eqvF(2); % equation component on y-axis
solvF = solve(eqvFx,eqvFy);
omega4zs=eval(solvF.omega4z);
vFys=eval(solvF.vFy);
Omega4 = [0 0 omega4zs];
VF = [0 vFys 0]; 

% print the equations for calculating omega4z and vFy
fprintf('vF = vE + omega4 x rEF  => \n');
qvFx=vpa(eqvFx,6); fprintf('x-axis: %s = 0 \n', char(qvFx));
qvFy=vpa(eqvFy,6); fprintf('y-axis: %s = 0 \n', char(qvFy)); 
fprintf('=>\n');
fprintf('omega4z =  %g (rad/s)\n', omega4zs);
fprintf('vFy =  %g (m/s)\n', vFys); fprintf('\n');
fprintf('omega4 = [ %g, %g, %g ] (rad/s)\n', Omega4 );
fprintf('vF =  [ %g, %g, %g ] (m/s)\n', VF ); fprintf('\n');

% acceleration of C
alpha2z = sym('alpha2z','real');
alpha3z = sym('alpha3z','real');
alpha2 = [ 0 0 alpha2z ];
alpha3 = [ 0 0 alpha3z ];
% aC = aC2 = aB + omega2 x rBC - (omega2.omega2)rBC
% aC = aC3 = aD + omega3 x rDC - (omega3.omega3)rDC
eqaC2= aB2 + cross(alpha2,rC-rB) - dot(Omega2,Omega2)*(rC-rB); 
eqaC3=  aD + cross(alpha3,rC-rD) - dot(Omega3,Omega3)*(rC-rD); 
eqaC = eqaC2 - eqaC3;
eqaCx = eqaC(1); % equation component on x-axis
eqaCy = eqaC(2); % equation component on y-axis
solaC = solve(eqaCx,eqaCy);
alpha2zs=eval(solaC.alpha2z);
alpha3zs=eval(solaC.alpha3z);
Alpha2 = [0 0 alpha2zs];
Alpha3 = [0 0 alpha3zs];

% print the equations for calculating omega2z and omega3z
fprintf('aC = aC2 = aB + omega2 x rBC - (omega2.omega2)rBC \n');
fprintf('aC = aC3 = aD + omega3 x rDC - (omega3.omega3)rDC \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('alpha3z =  %g (rad/s^2)\n', alpha3zs);
fprintf('\n');
fprintf('alpha2 = [ %g, %g, %g ] (rad/s^2)\n', Alpha2 );
fprintf('alpha3 = [ %g, %g, %g ] (rad/s^2)\n', Alpha3 ); fprintf('\n');

aC = aB2 + cross(Alpha2,rC-rB) - dot(Omega2,Omega2)*(rC-rB);
fprintf('aC =  [ %g, %g, %g] (m/s^2)\n', aC ); fprintf('\n');

aE = aD + cross(Alpha3,rE-rD) - dot(Omega3,Omega3)*(rE-rD);
fprintf('aE =  [ %g, %g, %g] (m/s^2)\n', aE ); fprintf('\n');

% acceleration  of F
alpha4z = sym('alpha4z','real');
aFy = sym('aFy','real');
alpha4 = [ 0 0 alpha4z ];
aF = [ 0 aFy 0 ];
% aF = aE + alpha4 x rEF - (omega4.omega4)rEF
eqaF = aF - (aE + cross(alpha4,rF-rE) - dot(Omega4,Omega4)*(rF-rE));
eqaFx = eqaF(1); 
eqaFy = eqaF(2); 
solaF = solve(eqaFx,eqaFy);
alpha4zs=eval(solaF.alpha4z);
aFys=eval(solaF.aFy);
Alpha4 = [0 0 alpha4zs];
AF = [0 aFys 0];

% print the equations for calculating omega4z and aFy
fprintf('aF = aE + alpha4 x rEF - (omega4.omega4)rEF \n');
qaFx=vpa(eqaFx,6); fprintf('x-axis: %s = 0 \n', char(qaFx));
qaFy=vpa(eqaFy,6); fprintf('y-axis: %s = 0 \n', char(qaFy));
fprintf('=>\n');
fprintf('alpha4z =  %g (rad/s^2)\n', alpha4zs);
fprintf('aFy =  %g (m/s^2)\n', aFys); fprintf('\n');
fprintf('alpha4 = [ %g, %g, %g ] (rad/s^2)\n', Alpha4 );
fprintf('aF =  [ %g, %g, %g ] (m/s^2)\n', AF ); fprintf('\n');