hold on; plot(draw_inner(:,1),draw_inner(:,2),'k-'); plot(draw_inner(:,1),draw_inner(:,2),'bo'); plot(draw_outer(:,1),draw_outer(:,2),'r:'); plot(draw_outer(:,1),draw_outer(:,2),'bo'); plot(draw_block(:,1),draw_block(:,2),'k-'); hold off;
function [data,form] = f4bar(r,theta1,theta2,td2,tdd2,mode,linkdrive) %輸入參數為 %r= [r1 r2 r3 r4]: 各桿之長度 %theta1= 桿一之水平角度 %theta2= 驅動桿(可為桿2或3)之水平角度 %td2= 驅動桿之角速度(rad/sec) %tdd2= 驅動桿之角加速度(rad/sec^2) %mode= +1 or -1 組合模數,負值表閉合型;正值表分支型 %linkdrive = 0表示驅動桿為第二桿; 1表示驅動桿為第三桿 if nargin<7,linkdrive=0;end if nargin<6,mode=1;end data=zeros(4,6); % if coupler is the driver, interchange the vetor 3 & 2 if linkdrive==1,r=[r(1) r(3) r(2) r(4)];end rr=r.*r;d2g=pi/180; [theta,td,tdd]=deal(zeros(4,1)); theta(1:2)=[theta1 theta2]*d2g; t1=theta(1);tx=theta(2); s1=sin(t1);c1=cos(t1); sx=sin(tx);cx=cos(tx); % position calculations 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; if pos>=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
>> for i=0:20:360,drawlinks([4 3 3 5],0,i,-1,0); end
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
function [val]=body2(r6,alph,values,link) %輸入參數為: % r6 = length of point from link's origin,Q %alph= angle in deg. between the base link and r %values= vetor output from the call of f4bar.m %link =the link that the point lays on, 1-4 %outputs:(in complex form) %val(1):initial position,(say Q, for link 2) %val(3):middle position,(say P, for link 2) %val(2):final position,or position of A %val(4):velocity of A %val(5):acceleration of A % r2=values(link,1); d2g=pi/180; switch link case 2 % on the crank val(1)=0;v0=0; a0=0; val(3)=values(2,1); case 3 % on the coupler val(1)=values(2,1); val(3)=values(1,1)+values(4,1); v0=values(1,5); a0=values(3,5); case 4 % on the rocker val(1)=values(1,1); val(3)=values(1,1)+values(4,1); v0=0; a0=0; case 1 % on the frame val(1)=0; v0=0; a0=0; val(3)=values(1,1); end th6=(values(link,2)+alph)*d2g; efact=exp(j*th6); w6=values(link,3); a6=values(link,4); val(2)=val(1)+r6*efact; val(4)=v0+j*r6*w6*efact; val(5)=a0+(j*r6*a6-r6*w6*w6)*efact;
function h=draw4link(values,linkdrive) %function draw4link(values,b,linkdrive) %draw the positions of four-bar links % values are data from f4bar.m %linkdrive: 0 for crank, 1 for coupler %clf; if nargin<2, linkdrive=0;end rr=values(:,1); rr(3,1)=rr(1,1)+rr(4,1); rx=real(rr(:,1));rx(4)=0; ry=imag(rr(:,1));ry(4)=0; h(1)=line([0 rx(1)],[0 ry(1)],'color','k','LineWidth',4); h(4)=line([rx(1) rx(3)],[ry(1) ry(3)],'color','g','LineWidth',1.5); if linkdrive==0 h(2)=line([0 rx(2)],[0 ry(2)],'color','b','LineWidth',2); h(3)=line([rx(2) rx(3)],[ry(2) ry(3)],'color','r',... 'LineWidth',1.5,'marker','o'); else h(2)=line([0 rx(2)],[0 ry(2)],'color','r','LineWidth',1.5); h(3)=line([rx(2) rx(3)],[ry(2) ry(3)],'color','b',... 'LineWidth',2,'marker','o'); end text(0,0,' O');text(rx(1),ry(1),' R'); text(rx(2),ry(2),' P');text(rx(3),ry(3),' Q'); axis equal grid on
最後以move_4paths將四連桿運動情形繪製成動畫 做動態模擬
function move_4paths(r,r6,th6,nlink,th1,td2,tdd2,sigma,driver,ntimes,npts) %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=100;end; figure(1); [Qstart, Qstop]=fb_angle_limits(r,th1,driver); npoint=abs(npts); th2=linspace(Qstart,Qstop,npoint); val=zeros(6,npoint); for i=1:npoint, [vr b]=f4bar(r,th1,th2(i),td2,tdd2,sigma,driver); [para]=body2(r6,th6,vr,nlink); val(1:3,i)=[vr(1,1);vr(2,1);vr(1,1)+vr(4,1)]; val(4:6,i)=[para(1);para(3);para(2)]; end x=real(val);y=imag(val); h=drawlimits(r,th1,sigma,driver); line(x(5,:)',y(5,:)','color','r','linestyle',':'); line(x(4,:)',y(4,:)','color','b','linestyle','-.'); line(x(6,:)',y(6,:)','color','k','linestyle',':'); range=1.2*([min(min(x)) max(max(x)) min(min(y)) max(max(y))]); axis(range);axis equal;grid off; for i=2:4,set(h(i),'erasemode','xor');end h0=patch('xdata',[],'ydata',[],'erasemode','xor','facecolor','r',... 'marker','o'); i=0;s=-1;axis off; for m=1:ntimes s=-s; if abs(Qstop-Qstart-360)<1,i=0;s=1;end; while 1, i=i+s; if i>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=drawlimits(r,th1,sigma,driver) %function drawlmits(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=drawlinks(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=drawlinks(values,driver); else fprintf('Combination of links fails at degrees %6.1f\n',Qstart); end axis equal grid on
ground=linkage(ground_no); link=sort(linkage);% sorting the links ig=find(linkage==link(1)); if link(1)+link(4)>link(3)+link(2), ans='Non-Grashof Linkage'; elseif link(1)+link(4)==link(3)+link(2) ans='Neutral Linkage'; elseif link(1)==ground, ans='Double-Crank Linkage'; else switch ig case 1 im=3; case 2 im=4; case 3 im=1; case 4 im=2; end if ground==linkage(im) ans='Double-Rocker Linkage'; else ans='Crank-Rocker Linkage'; end end
function finger(L1,L2,L3,theta1,theta2,theta3) axis equal; a = L1*cosd(theta1-90); %計算出每一指間joint的座標位置 b = L1*sind(theta1-90); c = a + L2*sind(360-theta2-theta1); d = b + L2*cosd(360-theta2-theta1); e = c + L3*sind(theta3-360+theta2+theta1); f = d - L3*cosd(theta3-360+theta2+theta1); finger_part([0 0],[a b],1); %分別繪出每一節指頭 finger_part([a b],[c,d],1); finger_part([c d],[e f],1);
以下為展示手指極限位置之方程式:
L1 = 4; L2 = 2.5; L3 = 2; %假設各節長度以自己尺寸輸入 for i = 0:5:90, clf; finger(L1, L2, L3, 180-i, 180, 180); %第一關節先作轉動 為極限範圍180~90度 pause(0.05); end; for i = 0:5:90, clf; finger(L1, L2, L3, 90, 180-i, 180); %再換第二關節轉動 範圍同樣180~90度 pause(0.05); end; for i = 0:5:90, clf; finger(L1, L2, L3, 90, 90, 180-i); %第三關節轉動 範圍180~90度 pause(0.05); end;
<動畫>依照指定角度範圍做運動之手臂
5-2-3 以下為繪出手指各節速度/加速度之函式:
function [vec,dyadata] = dyad(rho,theta,td,tdd) theta=theta(:);rho=rho(:); n=length(rho); if nargin<4, tdd="zeros(size(rho));" nargin="=" td="ones(size(rho));" td="ones(size(rho))*td;end;" tdd="ones(size(rho))*tdd;end;" td="td(:);tdd=" d2g="pi/180;" tt="exp(i*theta*d2g);" pp="rho.*tt;vv=" aa="-pp.*td.^2+i*pp.*tdd;" dyadata="[pp" vec="[abs(sum(dyadata));angle(sum(dyadata))/d2g];">
以下為繪出手指各節速度/加速度之函式:
function dyad_draw(rho,theta,td,tdd) % Inputs: rho:length of links % theta:incling angles, deg. % td:angular velocity, rad/s % tdd:angular acceleration, rad/s^2
clf; [vec,data] = dyad(rho,theta,td,tdd); x=[0;cumsum(real(data(:,1)))];y=[0;cumsum(imag(data(:,1)))]; for i=1:length(x)-1 linkshape([x(i) y(i)],[x(i+1) y(i+1)],1); end for k=1:length(rho) x0=x(k+1);y0=y(k+1); vx=x0+real(data(k,2));vy=y0+imag(data(k,2)); ax=x0+real(data(k,3));ay=y0+imag(data(k,3)); line([x0 vx],[y0 vy],'marker','s','linewidth',2); line([x0 ax],[y0 ay],'marker','s','color','r','linewidth',3) end sdata=sum(data); ss=[real(sdata(1)) imag(sdata(1))]; vv=[real(sdata(2)) imag(sdata(2))]; aa=[real(sdata(3)) imag(sdata(3))]; line([0 ss(1)],[0 ss(2)],'linestyle',':','linewidth',2) axis equal;grid on