******************************************************************* function drawsldpaths(r6,th6,r,th1,td2,tdd2,sigma,npts,driver,mode) clf; figure(1); warning off; r(abs(r)＜eps)=eps; [Qstart, Qstop]=sld_angle_limits(r,th1,driver) npoint=abs(npts); th2=linspace(Qstart,Qstop,npoint); val=zeros(11,npoint); for i=1:npoint, if driver==2, r(1)=th2(i);end [vr b]=sldlink(r,th1,th2(i),td2,tdd2,sigma,driver); [para]=body(r6,th6,vr,3); if mod(i,5)==0|i==1|i==npoint, drawsldlinks(r,th1,th2(i),sigma,driver);
end val(1:3,i)=[vr(1,1)+vr(4,1);vr(2,1);para(2)];%Sq,Sp,Sa %找q,p,a switch driver case 0 val(4:7,i)=[abs(vr(1,1));vr(3,2);vr(3,3);vr(3,4)]; case 1 val(4:7,i)=[abs(vr(1,1));vr(2,2);vr(2,3);vr(2,4)]; case 2 val(4:7,i)=[abs(vr(2,2));vr(3,2);vr(2,3);vr(3,3)]; end val(8:11,i)=[vr(1,5);para(4);vr(4,6);para(5);]; %Vs, Va, As, Aa end warning on; %路徑繪制 plot(val(1,:),'k-','LineWidth',1.5,'linestyle',':');% path of Q plot(val(2,:),'k-','LineWidth',1.5);% path of P plot(val(3,:),'g-','LineWidth',1.5);% path of A axis equal if mode==0, return;end; th2=th2(3:end-3);val=val(:,3:end-3); title0={'Crank Angle','Coupler Angle','Slider Pos'}; title1={'\Theta3(r) & r1(k)', '\Theta2(r) & r1(k)',... '\Theta2(r) & \Theta3(k)' }; title2={'Vel of A (r) & Slider(k)',... 'Acc of A(r) & Slider(k)' }; title3={'\omega(r) & \alpha(b) of Coupler',... '\omega(r) & \alpha(b) of Crank',... '\omega of Crank(r) & Coupler(b)'}; intitle=title0(driver+1); val(abs(val)＞10e+5)=NaN; val(8:11,:)=abs(val(8:11,:)); figure(2); clf; subplot(2,2,1); plot(th2,val(4,:),'k-'); hold on;fact=round(max(val(5,:))/max(val(4,:))*10)/10; plot(th2,val(5,:)/fact,'r-');% crank or coupler angle xlabel(intitle);ylabel(title1(driver+1)); grid on subplot(2,2,2); plot(th2,val(6,:),'r-'); fact=round(max(val(7,:))/max(val(6,:))*10)/10; hold on;plot(th2,val(7,:)/fact,'b-'); xlabel(intitle);ylabel(title3(driver+1)); grid on; subplot(2,2,3); plot(th2,val(8,:),'k-'); hold on;plot(th2,val(9,:),'r-'); xlabel(intitle);ylabel(title2(1)); grid on; subplot(2,2,4); plot(th2,val(10,:),'k-'); hold on;plot(th2,val(11,:),'r-'); xlabel(intitle);ylabel(title2(2)); grid on; *****************************************************************
*********************************************************************** function [data,form] = f4bar(r,theta1,theta2,td2,tdd2,mode,linkdrive) % %function [data,form] = f4bar(r,theta1,theta2,td2,tdd2,mode,linkdrive) % This function analyzes a four-bar linkage when the driving link is % crank or coupler. The input data are: % theta1,theta2 are angles in degrees % r=[r1 r2 r3 r4]= lengths of links(1=frame) %td2 = crank or coupler angular velocity (rad/sec) %tdd2 = crank or coupler angular acceleration (rad/sec^2) %mode = +1 or -1. Identifies assembly mode %linkdrive = 0 for crank as driver; 1 for coupler as driver %data (1:4,1) = link positions for 4 links %data (1:4,2) = link angles in degrees %data (1:4,3) = link angular velocities %data (1:4,4) = link angular accelerations %data (1,5) = velocity of point Q %data (2,5) = velocity of point P %data (3,5) = acceleration of point Q %data (4,5) = acceleration of point P %data (1,6) = position of Q %data (2,6) = position of P %form = assembly status. form = 0, mechanism fails to % assemble. % program revised from Waldron's Textbook % Revised:DSFON, BIME, NTU. Date: Feb. 7, 2007 if nargin<7,linkdrive=0;end mode="1;end" data="zeros(4,6);" linkdrive="=" r="[r(1)" rr="r.*r;d2g=" t1="theta(1);tx=" s1="sin(t1);c1=" sx="sin(tx);cx=" a="2*r(1)*r(4)*c1-2*r(2)*r(4)*cx;" c="rr(1)+rr(2)+rr(4)-rr(3)-2*r(1)*r(2)*(c1*cx+s1*sx);" b="2*r(1)*r(4)*s1-2*r(2)*r(4)*sx;" pos="B*B-C*C+A*A;">=0, form=1; % Check for the denominator equal to zero if abs(C-A)>=1e-5 t4=2*atan((-B+mode*sqrt(pos))/(C-A)); s4=sin(t4);c4=cos(t4); t3=atan2((r(1)*s1+r(4)*s4-r(2)*sx),(r(1)*c1+r(4)*c4-r(2)*cx)); s3=sin(t3);c3=cos(t3); else % If the denominator is zero, compute theta(3) first A=-2*r(1)*r(3)*c1+2*r(2)*r(3)*cx; B=-2*r(1)*r(3)*s1+2*r(2)*r(3)*sx; C=rr(1)+rr(2)+rr(3)-rr(4)-2*r(1)*r(2)*(c1*cx+s1*sx); pos=B*B-C*C+A*A; if pos>=0, t3=2*atan((-B-mode*sqrt(pos))/(C-A)); s3=sin(t3); c3=cos(t3); t4=atan2((-r(1)*s1+r(3)*s3+r(2)*sx),... (-r(1)*c1+r(3)*c3+r(2)*cx)); s4=sin(t4);c4=cos(t4); end end theta(3)=t3;theta(4)=t4; %velocity calculation td(2)=td2; AM=[-r(3)*s3, r(4)*s4; -r(3)*c3, r(4)*c4]; BM=[r(2)*td(2)*sx;r(2)*td(2)*cx]; CM=AM\BM; td(3)=CM(1);td(4)=CM(2);
%acceleration calculation
tdd(2)=tdd2; BM=[r(2)*tdd(2)*sx+r(2)*td(2)*td(2)*cx+r(3)*td(3)*td(3)*c3-... r(4)*td(4)*td(4)*c4;r(2)*tdd(2)*cx-r(2)*td(2)*td(2)*sx-... r(3)*td(3)*td(3)*s3+r(4)*td(4)*td(4)*s4]; CM=AM\BM; tdd(3)=CM(1);tdd(4)=CM(2); %store results in array data % coordinates of P and Q if linkdrive==1, c2=c3;c3=cx;s2=s3;s3=sx; r(2:3)=[r(3) r(2)];theta(2:3)=[theta(3) theta(2)]; td(2:3)=[td(3) td(2)];tdd(2:3)=[tdd(3) tdd(2)]; else c2=cx;s2=sx; end for j=1:4, data(j,1:4)=[r(j)*exp(i*theta(j)) theta(j)/d2g td(j) tdd(j)] ; end % position vectors data(1,5)=r(2)*td(2)*exp(i*theta(2));%velocity for point Q data(2,5)=r(4)*td(4)*exp(i*theta(4));%velocity for point P data(3,5)=r(2)*(i*tdd(2)-td(2)*td(2))*exp(i*theta(2));%acc of Q data(4,5)=r(4)*(i*tdd(4)-td(4)*td(4))*exp(i*theta(4));%acc of P data(1,6)=data(2,1);%position of Q, again data(2,6)=data(1,1)+data(4,1);% position of P
%find the accelerations else form=0; if linkdrive==1, r=[r(1) r(3) r(2) r(4)]; for j=1:4, data(j,1)=r(j).*exp(i*theta(j));end % positions end end ***********************************************************************
************************************************************* function [Ang1, Ang2]=fb_angle_limits(r,theta1,linkdrive) % %function [Ang1, Ang2]=fb_angle_limits(r,theta1,linkdrive) % Find initital & final angles for the driving link % linkdrive= (0 for crank; 1 for coupler as the driver). % Variables: % r=linkage row vector (cm) % theta1=frame angle(degree); % Ang1,Ang2=initial & final angles of the driving link(deg) %Program if nargin<3,linkdrive=0;end theta1="0;end" linkdrive="=" r="[r(1)" r1="r(1);r2=" r3="r(3);r4=" rmin="min(r);rmax=" rtotal="sum(r);" ang1="0;" ang2="2*pi;">(r3+r4)& abs(r1-r2)(r3+r4)& abs(r1-r2)>=abs(r3-r4) Ang1=acos((r2^2-(r4+r3)^2+r1^2)/(2*r1*r2)); Ang2=-Ang1; end % if (r1+r2)<=(r3+r4)&abs(r1-r2)>=abs(r3-r4) % Ang1=0; % Ang2=2*pi; % end if (r1+r2)<=(r3+r4)&abs(r1-r2) [Ang1, Ang2]=fb_angle_limits([4 3 3 5],0,0)
跑完程式後,Matlab顯示了這些訊息: Combination of links fail at degrees 0.0 Combination of links fail at degrees 20.0 Combination of links fail at degrees 340.0 Combination of links fail at degrees 360.0 這也就是我們在第三題裡面,所求出的四連桿之限制角度,是從28.955到331.045間才可做旋轉,因此其他角度是不會發生的.換個方式來講,沒有值的部份,其實就是沒有符合網路上第六章講義起先提到的:我們的四連桿必要滿足r2ejθ2+r3ejθ3=r1ejθ1+r4ejθ4,當四連桿滿足條件就代表實際情形中,它是可以轉到該角度的,故可得知我們上一題所算的限制角度,其實也就是運用這個閉合關係,完全是息息相關的.
***************************************************************************** function move_4paths(r,r6,th6,nlink,th1,td2,tdd2,sigma,driver,ntimes,npts) % %function move_4paths(r,r6,th6,nlink,th1,td2,tdd2,sigma,driver,ntimes,npts) % %draw the positions of four-bar links %call f4bar.m funcion, f4limits.m, fb_angle_limits.m, body.m % %Inputs: % r: row vector for four links % th1: frame angle % th2: crank angle or couple angle % td2,tdd2:angular velocity and acceleration of the driving link. % sigma: assembly mode % driver: 0 for crank, 1 for coupler % ntimes: no. of cycles % npts: number of points divided % r6,rh6,nlink:additional length and angle for nlink link. %example: % move_4paths([4 2 3 4],2,-30,3,0,10,0,1,0,4,100) % %clf; if nargin<10, ntimes="3;npts=" npoint="abs(npts);" th2="linspace(Qstart,Qstop,npoint);" val="zeros(6,npoint);" i="1:npoint," x="real(val);y=" h="f4limits(r,th1,sigma,driver);" range="1.2*([min(min(x))" i="2:4,set(h(i),'erasemode','xor');end" h0="patch('xdata',[],'ydata',[],'erasemode','xor','facecolor','r',..." i="0;s=" m="1:ntimes" s="-s;" i="0;s=" i="i+s;">npoint|i==0,break;end; set(h(2),'xdata',[0 x(2,i)], 'ydata',[0 y(2,i)]);%crank set(h(3),'xdata',[x(2,i) x(3,i)], 'ydata',[y(2,i) y(3,i)]);%coupler set(h(4),'xdata',[x(1,i) x(3,i)], 'ydata',[y(1,i) y(3,i)]);%Rocker set(h0,'xdata',[x(4:6,i)], 'ydata',[y(4:6,i)]); drawnow; %flush the draw buffer pause(0.1); end end % for m loop
% function h=f4limits(r,th1,sigma,driver) %function f4lmits(r,th1,sigma,driver) %draw the positions of four-bar links %call f4bar funcion %r: row vector for four links %th1: frame angle %sigma: assembly mode %driver: 0 for crank, 1 for coupler % Example:h=f4limits([4 2 3 4],0,1,0) [Qstart, Qstop]=fb_angle_limits(r,th1,driver) [values b]=f4bar(r,th1,Qstart,0,0,sigma,driver); if b==1, h=draw4link(values,driver); else fprintf('Combination of links fails at degrees %6.1f\n',Qstart); end [values b]=f4bar(r,th1,Qstop,0,0,sigma,driver); if b==1, h=draw4link(values,driver); else fprintf('Combination of links fails at degrees %6.1f\n',Qstart); end axis equal grid on **************************************************************************
