1、附件附件: 常用常用二级阿苏尔杆组二级阿苏尔杆组 SUB LINK(XA, YA, VAX, VAY, AAX, AAY, QAB, W, E, L, XB, YB, VBX, VBY, ABX, ABY) SUB RRR(XA, YA, VAX, VAY, AAX, AAY, XC, YC, VCX, VCY, ACX, ACY, LAB, LCB, QAB, WAB, EAB, QCB, WCB, ECB) SUB RRP(M, YB, L, YA, VAY, AAY, QAB, WAB, EAB) SUB RRP1(M, XB, L, XA, VAX, AAX, QAB, WAB, EA
2、B) SUB RPR(M, XA, YA, VAX, VAY, AAX, AAY, XC, YC, VCX, VCY, ACX, ACY, LAB, QBD, WBD, EBD) 常用常用二级阿苏尔杆组子程序二级阿苏尔杆组子程序 ! 6 SUBS: LINK, RRR, RPR, RRP, RRP1, INTERSECT SUB LINK(XA,YA,VAX,VAY,AAX,AAY,QAB,W,E,L,XB,YB,VBX,VBY,ABX,ABY) XB=XA+L*COS(QAB) YB=YA+L*SIN(QAB) VBX=VAX-L*SIN(QAB)*W VBY=VAY+L*COS(QAB)*
3、W ABX=AAX-L*COS(QAB)*W2-L*SIN(QAB)*E ABY=AAY-L*SIN(QAB)*W2+L*COS(QAB)*E END SUB SUB RRR(XA,YA,VAX,VAY,AAX,AAY,XC,YC,VCX,VCY,ACX,ACY,LAB,LCB,QAB,WAB,EAB,QCB,WCB,ECB) LAC=SQR(XC-XA)2+(YC-YA)2) COSQAC=(XC-XA)/LAC SINQAC=(YC-YA)/LAC QAC=ANGLE(COSQAC,SINQAC) COSQCBA=(LAB2+LAC2-LCB2)/(2*LAB*LAC) SINQCBA=S
4、QR(1-COSQCBA2) QCBA=ANGLE(COSQCBA,SINQCBA) QAB=QAC-QCBA XB=XA+LAB*COS(QAB) YB=YA+LAB*SIN(QAB) COSQCB=(XB-XC)/LCB SINQCB=(YB-YC)/LCB QCB=ANGLE(COSQCB,SINQCB) WAB=(VAX-VCX)*COSQCB+(VAY-VCY)*SINQCB)/LAB/SIN(QAB-QCB) WCB=(VAX-VCX)*COS(QAB)+(VAY-VCY)*SIN(QAB)/LCB/SIN(QAB-QCB) G=AAX-ACX-LAB*COS(QAB)*WAB2+
5、LCB*COSQCB*WCB2 F=AAY-ACY-LAB*SIN(QAB)*WAB2+LCB*SINQCB*WCB2 EAB=(G*COSQCB+F*SINQCB)/LAB/SIN(QAB-QCB) ECB=(G*COS(QAB)+F*SIN(QAB)/LCB/SIN(QAB-QCB) END SUB SUB RPR(M,XA,YA,VAX,VAY,AAX,AAY,XC,YC,VCX,VCY,ACX,ACY,LAB,QBD,W,E) LAC=SQR(XC-XA)2+(YC-YA)2) COSQAC=(XC-XA)/LAC SINQAC=(YC-YA)/LAC QAC=ANGLE(COSQAC
6、,SINQAC) LBC=SQR(LAC2-LAB2) QACB=ATN(LAB/LBC) QBD=QAC+M*QACB DELTA=-(YC-YA)*SIN(QBD)-(XC-XA)*COS(QBD) DELTAW=(VCX-VAX)*SIN(QBD)-(VCY-VAY)*COS(QBD) DELTAV=-(YC-YA)*(VCY-VAY)-(XC-XA)*(VCX-VAX) W=DELTAW/DELTA VLBC=DELTAV/DELTA T1=(ACX-AAX)+(VCY-VAY)*W+SIN(QBD)*W*VLBC T2=(ACY-AAY)-(VCX-VAX)*W-COS(QBD)*W
7、*VLBC DELTAE=T1*SIN(QBD)-T2*COS(QBD) E=DELTAE/DELTA END SUB SUB RRP(M,YB,LAB,YA,VAY,AAY,QAB,W,E) ! THE GUIDEWAY IS HORIZONTAL ! B IS THE REVOLUTE BETWEEN COUPLER AND SLIDING BLOCK ! IF XBXA THEN M=+1,OTHERWISE, M=-1 SINQAB=(YB-YA)/LAB COSQAB=M*SQR(1-SINQAB2) QAB=ANGLE(COSQAB,SINQAB) W=-VAY/(LAB*COSQ
8、AB) E=(-AAY*COSQAB-VAY*SINQAB*W)/(LAB*COSQAB2) END SUB SUB RRP1(M,XB,LAB,XA,VAX,AAX,QAB,W,E) ! THE GUIDEWAY IS VERTICAL ! B IS THE REVOLUTE BETWEEN COUPLER AND SLIDING BLOCK ! IF YBYA THEN M=+1,OTHERWISE, M=-1 COSQAB=(XB-XA)/LAB SINQAB=M*SQR(1-COSQAB2) QAB=ANGLE(COSQAB,SINQAB) W=VAX/(LAB*SINQAB) E=(AAX*SINQAB-VAX*COSQAB*W)/(LAB*SINQAB2) END SUB SUB INTERSECT(X1, Y1, Q1, X2, Y2, Q2, X, Y) X=(X1*TAN(Q1)-X2*TAN(Q2)-Y1+Y2)/(TAN(Q1)-TAN(Q2) Y=(X-X1)*TAN(Q1)+Y1 END SUB
