2/6/07

7.7 反置滑桿運動程式之建立

反置滑塊函數sldinv


以MATLAB寫作,可以獲得以上所述之解法,下面為sldinv函數之內容,其輸入內容有七項,分別說明如下:

  • r(1:4) = 各桿之長度
  • theta1 =第一桿之水平角。
  • theta2 =第二桿之水平角。
  • td2 =第二桿(或第三桿)之角速度(rad/sec)。
  • tdd2 =第二桿(或第三桿)之角加速度(rad/sec^2)。
  • sigma = +1 or -1. 組合型式,負值表示閉合型,正值為分支型。
  • driver = 0 (驅動桿為第二桿); 1 (驅動桿為第三桿)







桿序
12(deg)3(rad/s)4(rad/s2)5678
桿1位置r1θ1ω1α1r3'Vq|Vq|∠Vq
桿2位置r2θ2ω2α2r3"Vp|Vp|∠Vp
桿3位置r3θ3ω3α3Q2AQ|AQ|∠AQ
桿4位置r4θ4ω4α4P3AP|AP|∠AP


其中第一行之連桿位置向量,屬於單桿的位置向量。第二行為各桿之水平夾角,第三及第四行為各桿之角度速度及角加速度。第五行為第三桿上之滑塊軸向速度與加速度、P及Q點之位置向量。第六至八行則為P點與Q點之速度與加速度量,第五行為向量,第六行為絕對量,第七行為夾角,單位為角度。

值得一提的是第一行、三行、四行及五行之向量表示法屬於複數之型式。故若要得到其絕對值僅需在MATLAB指令檔中,以abs()這一個函數指令即可求得,而以函數angle()則可求得其夾角,雖然第二行與第七行之輸出亦有相對應之夾角。

程式內容



function [data,form] = sldinv(r,theta1,theta2,td2,tdd2,sigma,driver)
%
% [data,form] = sldlink(r,theta1,theta2,td2,tdd2,sigma,driver)
% Revised from Waldron's program to analyze a slider inversion.
% driver=0, 1 2 for Link 2, 3 & slide on link 3 as input,with td2, tdd2
% being their angular velocity and accerleration.
%The input values are:
% theta1,theta2 are angles of link 1 & the driving link in degrees
% theta2:angles for link 2 & 3; length for r3 as driver=2
% r = [r1 r2 r3 r4], r4 is link perpendicular to slider
% The results are returned in the vector "data". The answers are
% stored in data(I,j) according to the following:
% j | 1 | 2 | 3 | 4 | 5 | 6 | 7 |
% I | r1 | t1 | td1 | tdd1 | r3' | Rq | Aq |
% II | r2 | t2 | td2 | tdd2 | r3" | Rp | Ap |
% III| r3 | t3 | td3 | tdd3 | r4' | Vq |
% IV | r4 | t4 | td4 | tdd4 | r4" | Vp |
%form= assembly flag. form = 0 for failure of assembly.
%Example:
%[pval,form] = sldinv([20,8.5,0,0],0,60,2.5,0,1,0)
%[pval,form] = sldinv([20,8.5,17.385,0],0,154.95,0.10546,3.3012,1,1)
%[pval,form] = sldinv([20,8.5,17.385,0],0,60,-21.171,4.77,-1,2)
f=@(num,ndg) round(num*10^ndg)/10^ndg;
data=zeros(4,7);r3max=r(3);
rr=r.*r;d2g=pi/180;sigma=mode(1);beta=mode(2);
[td,tdd,rd,rdd]=deal(zeros(4,1));
theta=[theta1 theta2 theta2 0]*d2g;
t1=theta(1);s1=sin(t1);c1=cos(t1);
switch driver
case 0 % for link 2 as a crank
td(2)=td2; tdd(2)=tdd2;
s2=sin(theta(2));
c2=cos(theta(2));
% position calculations
KK=rr(2)+rr(1)-rr(4)-2*r(1)*r(2)*(c1*c2+s1*s2);
KK=f(KK,5);
if KK<0, form=0;return;end
r(3)=sqrt(KK);
if r(3)>r3max,form=-2,return;end
rr(3)=r(3)*r(3);
%modr3=sigma*r(3);
CC=r(2)*c2-r(1)*c1;
DD=rr(4)-CC*CC+rr(3);
DD=f(DD,5);
if (DD>=0)
form=1; %assembly flag
t3=2*atan2(r(4)+sigma*sqrt(DD),CC+r(3));
t4=t3+beta*pi/2;
theta(3:4)=[t3 t4];
s3=sin(t3);c3=cos(t3);
s4=sin(t4);c4=cos(t4);
Clc=r(1)*c1+r(3)*c3+r(4)*c4-r(2)*c2;
Cls=r(1)*s1+r(3)*s3+r(4)*s4-r(2)*s2;
if abs(Clc)>0.001|abs(Cls)>0.001,form=-1,return;end
%velocity calculation
AM=[c3, -r(3)*s3-r(4)*s4; s3, r(3)*c3+r(4)*c4];
BM=[-r(2)*td(2)*s2;r(2)*td(2)*c2];
CM=AM\BM;
rd(3)=CM(1); td(3:4)=CM(2);
%acceleration calculation
ww3=td(3)*td(3);ww2=td(2)*td(2);
BM=[-r(2)*tdd(2)*s2-r(2)*ww2*c2+r(3)*ww3*c3+2*rd(3)*td(3)*s3+...
r(4)*ww3*c4;r(2)*tdd(2)*c2-r(2)*ww2*s2+r(3)*ww3*s3-...
2*rd(3)*td(3)*c3+r(4)*ww3*c4];
CM=AM\BM;
rdd(3)=CM(1); tdd(3:4)=CM(2);
else
form=0, return;
end
case 1 % for link 3 as a crank
theta(3:4)=[theta(2) theta(2)-pi/2];
td(3)=td2;tdd(3)=tdd2;
s3=sin(theta(3)); c3=cos(theta(3));
s4=sin(theta(4)); c4=cos(theta(4));
% position calculations
A=2*r(1)*(c1*c3+s1*s3);
B=rr(1)-rr(2)+rr(4)+2*r(1)*r(4)*(c1*s3-s1*c3);
DD=A*A-4*B;
DD=f(DD,5);
if DD>=0,
form=1; %assembly flag
r(3)=(-A+sigma*sqrt(DD))/2;
if r(3)>r3max,form=-2,return;end
rr(3)=r(3)*r(3);
t2=atan2((r(1)*s1+r(3)*s3-r(4)*c3),(r(1)*c1+r(3)*c3+r(4)*s3));
theta(2)=t2; s2=sin(t2); c2=cos(t2);
%velocity calculation
AM=[-r(2)*s2, -c3; r(2)*c2 -s3];
BM=[-r(3)*td(3)*s3-r(4)*td(3)*s4;r(3)*td(3)*c3+r(4)*td(3)*c4];
CM=AM\BM;
td(2)=CM(1); rd(3:4)=CM(2);
%acceleration calculation
ww2=td(2)*td(2);ww3=td(3)*td(3);
BM=[r(2)*ww2*c2-r(3)*tdd(3)*s3-r(3)*ww3*c3-...
2*rd(3)*td(3)*s3-r(4)*ww3*c4-r(4)*tdd(3)*s4;...
r(2)*ww2*s2+r(3)*tdd(3)*c3-r(3)*ww3*s3+...
2*rd(3)*td(3)*c3-r(4)*ww3*s4+r(4)*tdd(3)*c4];
CM=AM\BM;
tdd(2)=CM(1); rdd(3:4)=CM(2);
else
form=0, return;
end
case 2 % r3 as input
r(3)=theta2;rd(3)=td2;rdd(3)=tdd2;
if r(3)>r3max,form=-1;return;end
rr(3)=r(3)*r(3);
A=-2*r(1)*r(2)*c1;
B=-2*r(1)*r(2)*s1;
C=rr(1)+rr(2)-rr(4)-rr(3);
DD=B*B-C*C+A*A;
DD=f(DD,5);
if (DD>=0)
form=1; %assembly flag
t2=2*atan((-B+sigma*sqrt(DD))/(C-A));
s2=sin(t2); c2=cos(t2);
KK=(r(2)*c2-r(1)*c1)/(r(2)*s2-r(1)*s1);
t3=atan2(r(3)+KK*r(4),KK*r(3)-r(4));
t4=t3+beta*pi/2;
theta(2:4)=[t2 t3 t4];
s3=sin(t3);c3=cos(t3);
s4=sin(t4);c4=cos(t4);
Clc=r(1)*c1+r(3)*c3+r(4)*c4-r(2)*c2;
Cls=r(1)*s1+r(3)*s3+r(4)*s4-r(2)*s2;
if abs(Clc)>0.001|abs(Cls)>0.001,form=-1,return;end
%velocity calculation
AM=[-r(2)*s2, r(3)*s3+r(4)*s4; r(2)*c2, -r(3)*c3-r(4)*c4];
BM=[rd(3)*c3;rd(3)*s3];
CM=AM\BM;
td(2:4)=[CM' CM(2)];
%acceleration calculation
BM=[r(2)*td(2)*td(2)*c2-r(3)*td(3)*td(3)*c3-2*rd(3)*td(3)*s3+...
rdd(3)*c3-r(4)*td(3)*td(3)*c4; r(2)*td(2)*td(2)*s2-...
r(3)*td(3)*td(3)*s3+2*rd(3)*td(3)*c3+rdd(3)*s3-...
r(4)*td(3)*td(3)*s4];
CM=AM\BM;
tdd(2:4)=[CM' CM(2)];
else
form=0, return;
end

end % end of switch

%store results in array values
for j=1:4,
data(j,1)=r(j)*exp(i*theta(j));
data(j,2:4)=[theta(j)/d2g td(j) tdd(j)];
end % position vectors
data(:,5)=[rd(3); rdd(3); rd(4);rdd(4)];%[r3' r3" r4' r4"];
data(1:2,6)=[data(2,1);data(1,1)+data(4,1)]; % Rq,Rp
data(3:4,6)=[i*r(2)*td(2)*exp(i*theta(2));...
i*r(1)*td(1)*exp(i*theta(1))+i*r(4)*td(4)*exp(i*theta(4))];%Vq,Vp
data(1:2,7)=[r(2)*(i*tdd(2)-td(2)*td(2))*exp(i*theta(2));...
r(1)*(i*tdd(1)-td(1)*td(1))*exp(i*theta(1))+...
r(4)*(i*tdd(4)-td(4)*td(4))*exp(i*theta(4))];%Aq,Ap





執行例


例一



>> [pval,form] = sldinv([20,8.5,0,0],0,60,2.5,0,1,0)
pval =
Columns 1 through 3
20 0 0
4.25 + 7.3612i 60 2.5
-15.75 + 7.3612i 154.95 -0.10546
0 64.95 0
Columns 4 through 6
0 21.171 4.25 + 7.3612i
0 4.777 20
3.3012 0 -18.403 + 10.625i
0 0 0
Column 7
-26.563 - 46.008i
0
0
0
form =
1


本例中,第一行則已經轉換為複數型式,未來複數型式要轉為x-y座表時,只要使用函數real()及imag()兩指令,即可進行轉換。值得注意的是,第一桿之數值已經變動,將val矩陣取絕對值如下:

>> abs(pval)
ans =
Columns 1 through 4
20 0 0 0
8.5 60 2.5 0
17.385 154.95 0.10546 3.3012
0 64.95 0 0
Columns 5 through 7
21.171 8.5 53.125
4.777 20 0
0 21.25 0
0 0 0


例二



>> [pval,form] = sldinv([20,8.5,17.385,0],0,154.95,0.10546,3.3012,1,1)
pval =
Columns 1 through 3
20 0 0
2.9198 + 7.9828i 69.909 2.7059
-17.08 + 7.9828i 154.95 0.10546
0 64.95 0
Columns 4 through 6
0 22.914 2.9198 + 7.9828i
6.8982 53.245 20
3.3012 0 -21.601 + 7.9008i
0 0 0
Column 7
-76.446 - 38.308i
0
0
0
form =
1

例三



>> [pval,form] = sldinv([20,8.5,17.385,0],0,60,-21.171,4.77,-1,2)
pval =
Columns 1 through 3
20 0 0
4.2503 - 7.361i -59.998 -2.5
15.75 + 7.361i 25.05 -0.10551
0 -64.95 0
Columns 4 through 6
0 -21.171 4.2503 - 7.361i
1.082 4.77 20
2.8332 0 -18.403 - 10.626i
0 0 0
Column 7
-18.601 + 50.607i
0
0
0
form =
1


反置滑塊繪圖函數drawinvlinks



drawinvlinks函數


drawinvlinks之目的在利用MATLAB繪製反置滑塊四連桿之相關位置,其本身會呼叫sldinv.m函數以計算等效四連桿之相對向量位置,然後繪圖。其呼叫格式如下:

function drawinvlinks(r,th1,th2,sigma,driver)

其輸入各式與f4bar.m大體相同,茲說明如下:
  • r(1:4) = 各桿之長度,r(1)為固定桿,其餘分別為曲桿、結合桿及被動桿。
  • theta1 = 第一桿之水平角,或為四連桿之架構角,以角度表示。
  • theta2 = 驅動桿之水平夾角,以角度表示。一般為曲桿角,但若為結合桿驅動,則為結合桿之水平夾角。
  • sigma = +1 or -1. 組合模式,負值表示閉合型,正值為分支型,但有時需視實際情況而定。
  • driver = 0 (驅動桿為第二桿); 1 (驅動桿為第三桿) ;2(滑塊為驅動件)。


drawinvlinks函數程式內容



function [data]=drawinvlinks(r,th1,th2,td2,tdd2,sigma,driver)
%
% drawinvlinks(r,th1,th2,td2,tdd2,sigma,driver)
% draw the positions of slider inversion links
% call sldinv.m
% r: row vector for four links
% th1: frame angle
% th2: crank angle or couple angle
% sigma: assembly mode
% driver: 0 for crank, 1 for coupler, 2 for r3
% Author:DSFON, BIME, NTU. Date: Feb. 15, 2007
% Example:
% drawinvlinks([20,8.5,0,3],0,60,1,0)
% for i=0:20:360,drawinvlinks([20,8.5,0,3],0,i,1,0);end
%
[data b]=sldinv(r,th1,th2,td2,tdd2,sigma,driver);
rr=data(:,1);
rr(3)=rr(1)+rr(4);
rx=real(rr);rx(4)=0;
ry=imag(rr);ry(4)=0;
if b==1
plot([0 rx(1)],[0 0],'k-','LineWidth',4);%Ground line
hold on;
plot([0 rx(1)],[0 ry(1)],'g-','LineWidth',1.5);
if driver==0
plot([0 rx(2)],[0 ry(2)],'b-','LineWidth',1.5);
plot([rx(2) rx(3)],[ry(2) ry(3)],'r-','LineWidth',2);
else
plot([0 rx(2)],[0 ry(2)],'r-','LineWidth',2);
plot([rx(2) rx(3)],[ry(2) ry(3)],'b-','LineWidth',1.5);
end
plot([rx(1) rx(3)],[ry(1) ry(3)],'k-');%link 4
plot(rx,ry,'bo');
text(0,0,' O');text(rx(1),ry(1),' R');
text(rx(2),ry(2),' P');text(rx(3),ry(3),' Q');
length=max(abs(data(1:2,1)));
rxx=real(rr(2)+rr(3))/2;ryy=imag(rr(2)+rr(3))/2;
[coords] = sldbox(.20*length,.10*length,rxx,ryy,data(3,2));
plot(coords(:,1),coords(:,2),'r-','LineWidth',2);
[coords] = sldbox(0.18*length,0.08*length,rxx,ryy,data(3,2));
fill(coords(:,1),coords(:,2),'r');
else
fprintf('Combination of links fails at degrees %6.1f\n',th2);
end
axis equal
grid on



例一、第二桿為驅動桿


>> drawinvlinks([1 2 3 1],30,60,10,0,1,0);
>> drawinvlinks([1 2 3 1],0,60,10,0,1,0);


圖7.20 r=[1 2 3 1],30度與60度而偏置為1之情形

>> drawinvlinks([1 2 3 0],0,30,10,0,1,0);%偏置為零時
>> drawinvlinks([1 2 1 0],0,60,10,0,1,0);


圖7.21 r=[4 2 4 0],角度為30與60度,無偏置之情形


例二、驅動桿為第三桿(coupler)



>> drawinvlinks([1 2 3 0.5],0,30,10,0,1,1);
>> drawinvlinks([1 2 3 0.5],0,60,10,0,1,1);
>> drawinvlinks([1 2 3 0.5],0,150,10,0,1,1);


第三桿角度分別為30、60、150度時之情形

例三、驅動桿為滑塊(第三桿)



>> drawinvlinks([4 2 2 1],0,60,10,0,1,2);
>> drawinvlinks([4 2 3 1],0,60,10,0,1,2);
>> drawinvlinks([4 2 5 1],0,60,10,0,1,2);

驅動件為滑塊時,當r3=2、3、5時之情形

No comments:

Post a Comment