clear all
clc
close all
AB=0.25;
BC=223.94*0.0025;
phi=130*(pi/180);
xA=0; yA=0;
xB=AB*cos(phi);
yB=AB*sin(phi);
yC=0;
eqnD1='(xB-xCsol)^2+(yB-yC)^2=BC^2';
solC=solve(eqnD1,'xCsol');
xC1=eval(solC(1));
xC2=eval(solC(2));
if xC1 > xB xC = xC1;
else xC = xC2; end
phi2 = atan((yB-yC)/(xB-xC));
rA=[xA yA 0];
rB=[xB yB 0];
rC=[xC yC 0];
AE=0.3;
BD=90.42*0.0025;
ED=149.76*0.0025;
CF=357.73*0.0025;
DF=43.37*0.0025;
phi0=pi;
xE=0;
yE=0.3;
eqnD1='(xB-xDsol)^2+(yB-yDsol)^2=BD^2';
eqnD2=' (yB-yC) / (xB -xC)= (yDsol - yB) / (xDsol -xB)';
solD = solve(eqnD1, eqnD2, 'xDsol, yDsol');
xDpositions = eval(solD.xDsol);
yFpositions = eval(solD.yDsol);
xD1 = xDpositions(1);
xD2 = xDpositions(2);
yD1 = yFpositions(1);
yD2 = yFpositions(2);
if xD1 <= xC
xD = xD1; yD=yD1;
else
xD = xD2; yD=yD2;
end
rD = [xD yD 0];
eqnF1='(xD-xFsol)^2+(yD-yFsol)^2=DF^2';
eqnF2=' (yB-yC) / (xB -xC)= (yFsol - yB) / (xFsol -xB)';
solF = solve(eqnF1, eqnF2, 'xFsol, yFsol');
xFpositions = eval(solF.xFsol);
yFpositions = eval(solF.yFsol);
xF1 = xFpositions(1);
xF2 = xFpositions(2);
yF1 = yFpositions(1);
yF2 = yFpositions(2);
if xF1 <= xC
xF = xF1; yF=yF1;
else
xF = xF2; yF=yF2;
end
rF = [xF yF 0];
plot([xA,xB],[yA,yB],'r-o',[xB,xC],[yB,yC],'b-o',[xB,xD],[yB,yD],'g-o',...
[xE,xD],[yE,yD],'y-o',[xD,xF],[yD,yF],'k-o')
xlabel('x (m)')
ylabel('y (m)')
title('pozitie mecanism pentru \phi = 130 (deg)')
text(xA,yA,' A'),...
text(xB,yB,' B'),...
text(xC,yC,' C'),...
text(xD,yD,' D'),...
text(xE,yE,' E'),...
text(xE,yE,' E'),...
axis([-0.6 0.6 -0.2 0.5]),...
grid
phi4 = phi2;
phi5 = atan((yD-yE)/(xD-xE));
rE = [xE yE 0];
fprintf('xA = %g (m) \n', xA)
fprintf('yA = %g (m) \n', yA)
fprintf('xB = %g (m) \n', xB)
fprintf('yB = %g (m) \n', yB)
fprintf('xC = %g (m) \n', xC)
fprintf('yC = %g (m) \n', yC)
fprintf('xD = %g (m) \n', xD)
fprintf('yD = %g (m) \n', yD)
fprintf('xE = %g (m) \n', xE)
fprintf('yE = %g (m) \n', yE)
fprintf('xF = %g (m) \n', xF)
fprintf('yF = %g (m) \n', yF)
fprintf('phi0 = %g (degrees) \n', phi0*180/pi)
fprintf('phi1 = %g (degrees) \n', phi*180/pi)
fprintf('phi2 = %g (degrees) \n', phi2*180/pi)
fprintf('phi4 = %g (degrees) \n', phi4*180/pi)
fprintf('phi5 = %g (degrees) \n', phi5*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('rD = [ %g, %g, %g ] (m) \n', rD)
fprintf('rE = [ %g, %g, %g ] (m) \n', rE)
fprintf('rF = [ %g, %g, %g ] (m) \n', rF)
omega1=[0 0 20];
vA=[0 0 0];
vB1=vA+cross(omega1,rB);
vB2=vB1;
vB = norm(vB1);
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)
omega2z = sym('omega2z','real');
vCx = sym('vCx','real');
omega2 = [ 0 0 omega2z ];
vC = [ vCx 0 0 ];
eqvC = vC - (vB2 + cross(omega2,rC-rB));
eqvCx = eqvC(1);
eqvCy = eqvC(2);
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);
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('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')
aA = [0 0 0 ];
ep1=[0 0 0];
aB1 = aA + cross(ep1,rB) - dot(omega1,omega1)*rB;
aB2 = aB1;
aBn = - dot(omega1,omega1)*rB;
aBt = cross(ep1,rB);
fprintf('|aB1|=|aB2|= %g (m/s^2)\n', norm(aB1))
fprintf('|aBn|= %g (m/s^2)\n', norm(aBn))
fprintf('|aBt|= %g (m/s^2)\n', norm(aBt))
ep2z=sym('ep2z','real');
aCx=sym('aCx','real');
ep2 = [ 0 0 ep2z ];
aC = [aCx 0 0 ];
eqaC=aC-(aB1+cross(ep2,rC-rB)-...
dot(Omega2,Omega2)*(rC-rB));
eqaCx = eqaC(1);
eqaCy = eqaC(2);
solaC = solve(eqaCx,eqaCy);
ep2zs=eval(solaC.ep2z);
aCxs=eval(solaC.aCx);
Ep2 = [0 0 ep2zs];
aCs = [aCxs 0 0];
aCB=cross(Ep2,rC-rB)-dot(Omega2,Omega2)*(rC-rB);
aCBn=-dot(Omega2,Omega2)*(rC-rB);
aCBt=cross(Ep2,rC-rB);
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('ep2z = %g (rad/sˆ2)\n', ep2zs)
fprintf('aCx = %g (m/sˆ2)\n', aCxs)
fprintf('\n')
fprintf('alpha2 = [%g, %g, %g] (rad/sˆ2)\n', Ep2)
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')
omega5z = sym('omega5z','real');
omega5 = [ 0 0 omega5z ];
vD42 = sym('vD42','real');
vD4D2 = [ vD42*cos(phi4) vD42*sin(phi4) 0 ];
vE = [0 0 0 ];
vD5 = vE + cross(omega5,rD-rE);
vD4 = vD5;
eqvD = vD4 - ( vB2 + vD4D2);
eqvDx = eqvD(1); eqvDy = eqvD(2);
solvD = solve(eqvDx,eqvDy);
omega5zs = eval(solvD.omega5z);
vD42s = eval(solvD.vD42);
Omega5 = [0 0 omega5zs];
VD42 = vD42s*[cos(phi4) sin(phi4) 0];
VD5 = vE + cross(Omega5,rD-rE);
fprintf('vD4 = vD2 + vD4D2 => \n')
qvDx=vpa(eqvDx,6);
fprintf('x-axis:\n')
fprintf('%s=0\n',char(qvDx))
qvDy=vpa(eqvDy,6);
fprintf('y-axis:\n')
fprintf('%s=0\n',char(qvDy))
fprintf('=>\n')
fprintf('omega5z = %g (rad/s)\n', omega5zs)
fprintf('vD42 = %g (m/s)\n\n', vD42s)
fprintf('omega5 = [%g, %g, %g](rad/s)\n', Omega5)
fprintf('vD4D2 = [ %g, %g, %d ] (m/s)\n', VD42)
fprintf('\n')
fprintf('vD5 = [ %g, %g, %d ] (m/s)\n', VD5)
aE=[0 0 0];
ep5z = sym('ep5z','real');
ep5 = [ 0 0 ep5z ];
aD42=sym('aD42','real');
aD4D2 = [ aD42*cos(phi4) aD42*sin(phi4) 0 ];
aD5=aE+cross(ep5,rD-rE)- dot(Omega5,Omega5)*(rD-rE);
aD4 = aD5;
aD4D2cor = 2*cross(omega2,VD42);
eqaD = aD4 - ( aB2 + aD4D2 + aD4D2cor );
eqaDx = eqaD(1); eqaDy = eqaD(2);
solaD = solve(eqaDx,eqaDy);
ep5zs = eval(solaD.ep5z);
aDpds = eval(solaD.aD42);
Ep5 = [0 0 ep5zs];
AD42 = aDpds*[cos(phi4) sin(phi4) 0];
AD5=aE+cross(Ep5,rD-rE)-...
dot(Omega5,Omega5)*(rD-rE);
fprintf...
('aD4D2cor = [%g, %g, %d] (m/sˆ2)\n', aD4D2cor)
fprintf('\n')
fprintf...
('aD4 = aD2 + aD4D2 + aD4D2cor => \n')
qaDx=vpa(eqaDx,6);
fprintf('x-axis:\n')
fprintf('%s=0\n',char(qaDx))
qaDy=vpa(eqaDy,6);
fprintf('y-axis:\n')
fprintf('%s=0\n',char(qaDy))
fprintf('=>\n')
fprintf('ep5z = %g (rad/sˆ2)\n', ep5zs)
fprintf('aD42 = %g (m/sˆ2)\n', aDpds)
fprintf('\n')
fprintf...
('ep5 = [ %g, %g, %g ] (rad/sˆ2)\n', Ep5)
fprintf('aD4D2 = [ %g, %g, %d ] (m/sˆ2)\n', AD42)
fprintf('aD5 = [ %g, %g, %d ] (m/sˆ2)\n', AD5)
fprintf('\n')
omega45 = omega4 - Omega5;
ep45 = ep4 - Ep5;
fprintf...
('omega45 = [ %g, %g, %g ] (rad/s)\n', omega45)
fprintf...
('ep45 = [ %g, %g, %g ] (rad/sˆ2)\n', ep45)