Dan B. Marghitu, Mechanisms and Robots Analysis with MATLAB®
Подождите немного. Документ загружается.
308 A Programs of Chapter 2: Position Analysis
% Graphic of the mechanism
plot([xA,xB],[yA,yB],’k-o’,’LineWidth’,1.5)
hold on % holds the current plot
plot([xD,xC],[yD,yC],’b-o’,’LineWidth’,1.5)
hold on
plot([xC,xB],[yC,yB],’b-o’,’LineWidth’,1.5)
hold on
plot([xB,xF],[yB,yF],’b-o’,’LineWidth’,1.5)
hold on
plot([xA,xD],[yA,yD],’r-o’,’LineWidth’,1.5)
hold on
plot([xD,xG],[yD,yG],’r-o’,’LineWidth’,1.5)
grid on,...
xlabel(’x (m)’), ylabel(’y (m)’),...
title(’positions for \phi = 30 (deg)’),...
text(xA,yA,’\leftarrow A = ground’,...
’HorizontalAlignment’,’left’),...
text(xB,yB,’ B’),...
text(xC,yC,’\leftarrow C = ground’,...
’HorizontalAlignment’,’left’),...
text(xD,yD,’ D’),...
text(xF,yF,’ F’), text(xG,yG,’ G’),...
axis([-0.3 0.3 -0.1 0.3])
% end of program
Results:
rA=[0,0,0](m)
rC=[0,0.1, 0 ] (m)
rB = [ 0.129904, 0.075,0](m)
rD = [ -0.147297, 0.128347,0](m)
phi2 = phi3 = -10.8934 (degrees)
phi4 = phi5 = 138.933 (degrees)
rF = [ 0.245495, 0.0527544, 0] (m)
rG = [ -0.226182, 0.197083, 0] (m)
A.4 R-RTR-RTR Mechanism: Complete Rotation 309
A.4 R-RTR-RTR Mechanism: Complete Rotation
%A4
% Position analysis - complete rotation
% R-RTR-RTR
clear all; clc; close all
% Input data
AB=0.15; AC=0.10; CD=0.15; %(m)
xA=0;yA=0;rA=[xAyA0];
xC = 0; yC = AC; rC = [xC yC 0];
fprintf(’Results \n\n’)
fprintf(’rA = [ %g, %g, %g ] (m)\n’, rA)
fprintf(’rC = [ %g, %g, %g ] (m)\n’, rC)
fprintf(’\n’)
% complete rotation phi=0 to 2
*
pi step pi/3
for phi=0:pi/3:2
*
pi,
% for repeat statements a specific number of times
fprintf(’phi = %g deegres \n’, phi
*
180/pi)
% Position of joint B - position of the driver link
xB=AB
*
cos(phi); yB = AB
*
sin(phi); rB = [xB yB 0];
fprintf(’rB = [ %g, %g, %g ] (m)\n’, rB)
% Position of joint D
eqnD1=’(xDsol - xC)ˆ2 + (yDsol - yC)ˆ2 = CDˆ2 ’;
eqnD2=’(yB-yC)/(xB-xC)=(yDsol-yC)/(xDsol-xC)’;
% Simultaneously solve above equations
solD = solve(eqnD1, eqnD2, ’xDsol, yDsol’);
xDpositions = eval(solD.xDsol);
yDpositions = eval(solD.yDsol);
% Separate the solutions in scalar form
xD1 = xDpositions(1); xD2 = xDpositions(2);
yD1 = yDpositions(1); yD2 = yDpositions(2);
% Select the correct position for D for the angle phi
% see the drawings for each quadrant
310 A Programs of Chapter 2: Position Analysis
if(phi>=0 && phi<=pi/2)||(phi >= 3
*
pi/2 && phi<=2
*
pi)
if xD1 <= xC xD=xD1; yD=yD1; else xD=xD2; yD=yD2;
end
else
if xD1 >= xC xD=xD1; yD=yD1; else xD=xD2; yD=yD2;
end
end
% && short-circuit logical AND
% || short-circuit logical OR
rD = [xD yD 0];
fprintf(’rD = [ %g, %g, %g] (m)\n’, rD)
% Angles of the links with the horizontal
phi2 = atan((yB-yC)/(xB-xC));
phi3 = phi2;
fprintf(’phi2 = phi3 = %g (degrees) \n’, phi2
*
180/pi)
phi4 = atan(yD/xD);
phi5 = phi4;
fprintf(’phi4 = phi5 = %g (degrees) \n’, phi4
*
180/pi)
fprintf(’\n’)
% Graphic of the mechanism
plot([xA,xB],[yA,yB],’k-o’,[xB,xC],[yB,yC],’b-o’,...
[xC,xD],[yC,yD],’b-o’)
hold on
plot([xD,xA],[yD,yA],’r-o’)
xlabel(’x (m)’),...
ylabel(’y (m)’),...
title(’positions for \phi=0 to 360 step 60(deg)’),...
text(xA,yA,’ A’),...
text(xB,yB,’ B’),...
text(xC,yC,’ C’),...
text(xD,yD,’ D’),...
axis([-0.3 0.3 -0.2 0.3])
end % end for
% end of program
Results:
rA=[0,0,0](m)
rC=[0,0.1, 0 ] (m)
A.4 R-RTR-RTR Mechanism: Complete Rotation 311
phi = 0 deegres
rB = [ 0.15, 0,0](m)
rD = [ -0.124808, 0.183205, 0] (m)
phi2 = phi3 = -33.6901 (degrees)
phi4 = phi5 = -55.7355 (degrees)
phi = 60 deegres
rB = [ 0.075, 0.129904,0](m)
rD = [ -0.139333, 0.0444455, 0] (m)
phi2 = phi3 = 21.738 (degrees)
phi4 = phi5 = -17.692 (degrees)
phi = 120 deegres
rB = [ -0.075, 0.129904,0](m)
rD = [ 0.139333, 0.0444455, 0] (m)
phi2 = phi3 = -21.738 (degrees)
phi4 = phi5 = 17.692 (degrees)
phi = 180 deegres
rB = [ -0.15, 1.83697e-17,0](m)
rD = [ 0.124808, 0.183205, 0] (m)
phi2 = phi3 = 33.6901 (degrees)
phi4 = phi5 = 55.7355 (degrees)
phi = 240 deegres
rB = [ -0.075, -0.129904,0](m)
rD = [ 0.0465207, 0.242604, 0] (m)
phi2 = phi3 = 71.9325 (degrees)
phi4 = phi5 = 79.145 (degrees)
phi = 300 deegres
rB = [ 0.075, -0.129904,0](m)
rD = [ -0.0465207, 0.242604, 0] (m)
phi2 = phi3 = -71.9325 (degrees)
phi4 = phi5 = -79.145 (degrees)
phi = 360 deegres
rB = [ 0.15, -3.67394e-17,0](m)
rD = [ -0.124808, 0.183205, 0] (m)
phi2 = phi3 = -33.6901 (degrees)
phi4 = phi5 = -55.7355 (degrees)
312 A Programs of Chapter 2: Position Analysis
A.5 R-RTR-RTR Mechanism: Complete Rotation Using
Euclidian Distance Function
%A5
% Position analysis - complete rotation
% R-RTR-RTR
% Euclidian distance function
% the program uses the function: Dist(x1,y1,x2,y2)
% the function is defined in the program Dist.m
clear all; clc; close all
% Input data
AB=0.15; AC=0.10; CD=0.15; %(m)
xA=0;yA=0;rA=[xAyA0];
xC=0;yC=AC;rC=[xCyC0];
fprintf(’Results \n\n’)
fprintf(’rA = [ %g, %g, %g ] (m)\n’, rA)
fprintf(’rC = [ %g, %g, %g ] (m)\n’, rC)
% at the initial moment phi=0 => increment = 0
increment=0;
% the step has to be small for this method
step=pi/6;
for phi=0:step:2
*
pi,
fprintf(’phi = %g deegres \n’, phi
*
180/pi)
xB=AB
*
cos(phi); yB = AB
*
sin(phi); rB = [xB yB 0];
fprintf(’rB = [ %g, %g, %g ] (m)\n’, rB)
eqnD1=’( xDsol - xC )ˆ2 + ( yDsol - yC )ˆ2=CDˆ2’;
eqnD2=’(yB-yC)/(xB-xC)=(yDsol-yC)/(xDsol-xC)’;
solD = solve(eqnD1, eqnD2, ’xDsol, yDsol’);
xDpositions = eval(solD.xDsol);
yDpositions = eval(solD.yDsol);
xD1 = xDpositions(1); xD2 = xDpositions(2);
yD1 = yDpositions(1); yD2 = yDpositions(2);
% select the correct position for D
% only for increment == 0
% the selection process is automatic
% for all the other steps
A.5 R-RTR-RTR Mechanism: Complete Rotation Using Euclidian Distance Function 313
if increment == 0
if xD1 <= xC xD=xD1; yD=yD1; else xD=xD2; yD=yD2;
end
else
dist1 = Dist(xD1,yD1,xDold,yDold);
dist2 = Dist(xD2,yD2,xDold,yDold);
if dist1<dist2 xD=xD1; yD=yD1; else xD=xD2; yD=yD2;
end
end
xDold=xD;
yDold=yD;
increment=increment+1;
rD = [xD yD 0];
fprintf(’rD = [ %g, %g, %g ] (m)\n’, rD)
phi2 = atan((yB-yC)/(xB-xC)); phi3 = phi2;
fprintf(’phi2 = phi3 = %g (degrees) \n’, phi2
*
180/pi)
phi4 = atan(yD/xD); phi5 = phi4;
fprintf(’phi4 = phi5 = %g (degrees) \n’, phi4
*
180/pi)
fprintf(’\n’)
% Graphic of the mechanism
plot([xA,xB],[yA,yB],’k-o’,[xB,xC],[yB,yC],’b-o’,...
[xC,xD],[yC,yD],’b-o’)
hold on
plot([xD,xA],[yD,yA],’r-o’)
xlabel(’x (m)’),...
ylabel(’y (m)’),...
title(’positions for \phi=0 to 360 step 30(deg)’),...
text(xA,yA,’ A’),...
text(xB,yB,’ B’),...
text(xC,yC,’ C’),...
text(xD,yD,’ D’),...
axis([-0.3 0.3 -0.2 0.3])
end
% end of program
314 A Programs of Chapter 2: Position Analysis
A.6 Path of a Point on a Link with General Plane Motion:
R-RRT Mechanism
%A6
% Position analysis
% R-RRT
% Path of C2 (mass center of link BC)
clear all; clc; close all
AB=.5;BC=1;xA=0;yA=0;yC=0;
incr=0;
for phi=0:pi/10:2
*
pi,
xB=AB
*
cos(phi); yB = AB
*
sin(phi);
xC = xB + sqrt(BCˆ2-yBˆ2);
incr = incr + 1;
xC2(incr)=(xB+xC)/2; yC2(incr)=(yB+yC)/2;
% Graphic of the mechanism
subplot(2,1,1),...
plot( [xA,xB],[yA,yB],’r-’,...
[xB,xC],[yB,yC],’b-’ ),...
hold on
xlabel(’x (m)’), ylabel(’y (m)’),...
title(’Graphic of the mechanism’),...
text(xC2,yC2,’C2’),...
axis([-0.6 1.6 -0.6 0.6]),...
end % end for
% Path of C2 (mass center of link 2)
subplot(2,1,2),...
plot(xC2, yC2, ’-ko’),...
xlabel(’x (m)’), ylabel(’y (m)’),...
title(’Path described by C2’), grid
% end of program
A.7 Path of a Point on a Link with General Plane Motion: R-RRR Mechanism 315
A.7 Path of a Point on a Link with General Plane Motion:
R-RRR Mechanism
%A7
% R-RRR
% Position analysis
% Path of C2 (mass center of link BC)
clear all; clc; close all
AB=0.15; %(m)
BC=0.35; %(m)
CD=0.30; %(m)
CE=0.15; %(m)
xA=0; yA=0;
xD=0.30; yD=0.30;
incr = 0;
for phi=0:pi/10:2
*
pi,
xB=AB
*
cos(phi); yB = AB
*
sin(phi);
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
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);
316 A Programs of Chapter 2: Position Analysis
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
incr = incr + 1;
XC2(incr)=(xB+xC)/2;
YC2(incr)=(yB+yC)/2;
% Graphic of the mechanism
subplot(2,1,1),...
plot([xA,xB],[yA,yB],’k-’,...
[xB,xC],[yB,yC],’b-’,...
[xD,xE],[yD,yE],’r-’),...
hold on,...
xlabel(’x (m)’), ylabel(’y (m)’), grid,...
axis([-0.2 0.35 -0.25 0.7]),...
title(’Positions of the mechanism’),...
text(XC2,YC2,’C2’),...
end
% Path of C2 (mass center of link 2)
subplot(2,1,2),...
plot(XC2, YC2, ’-ko’),...
xlabel(’x (m)’), ylabel(’y (m)’),...
title(’Path described by C2’), grid
% end of program
Appendix B
Programs of Chapter 3: Velocity and
Acceleration Analysis
B.1 Slider-Crank (R-RRT) Mechanism
%B1
% 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=[xAyA0];
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’)
317