ПРИЛОЖЕНИЕ
.docҚОСЫМША А
ТҰЙЫҚ ЖӘНЕ ТҰЙЫҚ ЕМЕС КИНЕМАТИКАЛЫҚ ТІЗБЕКТЕР НЕГІЗІНДЕГІ МАНИПУЛЯТОРЛАРДЫҢ ДИНАМИКАСЫНЫҢ ЕСЕБІНІҢ ПРОГРАММАСЫ
НЕГІЗГІ ПРОГРАММА
PROGRAM UPR
EXTERNAL F,OU,FD,OUD,ZAC,PROGD,UR,KOEF
EXTERNAL FORC,RKGSD,KOEF1,FORC1,PROGD1
EXTERNAL RKGS
COMMON /S/M1,M2,M3,M4
COMMON /S1/L1,L2,L3,L4,L5
COMMON Z,G,T
COMMON /S3/T1,TD1
COMMON /SS/Z1,TM1,TM4,H1,H4,C2,C3,C5,C6,E1,E2,E3,E4,E5,E6,E7,DU
COMMON H1,H4,TM1,TM4
COMMON MD1,MD4
COMMON /R/Y3,Y4
COMMON /RR/KK
DIMENSION Y(4),PR(5),D(4),AU(48)
DIMENSION YD(2),PRD(5),DD(2),AUD(48)
REAL L1,L2,L3,L4,L5,M1,M2,M3,M4
DO 13 KK=1,2
IF(KK-2)11,12,12
PRINT 5,KK
5 FORMAT(//2X,'MАНИПУЛЯТОР ТИПА ',I4/)
11 OPEN(1,FILE='ZM F1.DAT')
OPEN(2,FILE='ZM F4.DAT')
WRITE(1,8)
WRITE(2,9)
8 FORMAT(2X,'ТҰЙЫҚ МЕХАНИЗМ')
9 FORMAT(2X,'ТҰЙЫҚ МЕХАНИЗМ')
GOTO 10
12 OPEN(3,FILE='NZM F1.DAT')
OPEN(4,FILE='NZM F2.DAT')
WRITE(3,6)
WRITE(4,7)
6 FORMAT(2X,'ТҰЙЫҚ ЕМЕС МЕХАНИЗМ')
7 FORMAT(2X,'ТҰЙЫҚ ЕМЕС МЕХАНИЗМ')
БАҒДАРЛАМАЛЫ ОРЫН АУЫСТЫРУДЫҢ ТАҢДАУЫ
10 DO 14 K=1,8
PRINT 15,K
15 FORMAT(' ОРЫН АУЫСТЫРУ ',I2)
ИСХОДНЫЕ ДАННЫЕ
G=9.8
L1=1.
L2=1.
L3=L2
L4=L1
L5=L1+L4
M1=20.
M2=10.
M3=M2
M4=M1
H1=328.
H4=328.
TM1=0.09
TM4=0.09
DL=0.1
T1=0.0
TD1=0.0
Y(1)=0.0
Y(2)=0.0
Y(3)=F1
Y(4)=F4
YF3=F1
YF4=F4
YD(1)=0.08
YD(2)=0.08
MD1=YD(1)
MD4=YD(2)
ҚОЗҒАЛЫСТЫ МОДЕЛЬДЕУ
DO 1 I=1,50,5
T=0.01*I
PRINT 101
101 FORMAT(2X,'БЕРІЛЕТІН ШАМАЛАР')
PRINT 100,I,T
100 FORMAT(3X,'I=',I2,3X,'T=',F3.2)
CALL ZAC(K,T,XC,YC,DXC,DYC)
PRINT 99,XC,YC,DXC,DYC
99 FORMAT(2X,'XC=',E10.4,2X,'YC=',E10.4,2X,
'DXC=',E10.4,2X,'DYC=',E10.4)
IF(KK-2)20,21,21
1 ТИПТІ МАНИПУЛЯТОР ҮШІН КЕРІ ЕСЕБІ
20 CALL PROGD(XC,DXC,YC,DYC,F1,F4,DF1,DF4)
PRINT 98,F1,F4,DF1,DF4
98 FORMAT(2X,'F1=',E10.4,2X,'F4=',E10.4,2X,
'DF1=',E10.4,2X,'DF4=',E10.4)
CALL UR(F1,F2,F3,F4)
PRINT 97,F2,F3
97 FORMAT(2X,'F2=',E10.4,2X,'F3=',E10.4)
GOTO 25
2 ТИПТІ МАНИПУЛЯТОР ҮШІН КЕРІ ЕСЕБІ
21 CALL PROGD1(XC,DXC,YC,DYC,F1,F2,DF1,DF2)
PRINT 18,F1,F2,DF1,DF2
18 FORMAT(2X,'F1=',E10.4,2X,'F2=',E10.4,2X,
'DF1=',E10.4,2X,'DF2=',E10.4)
25 PR(1)=T1
PR(2)=T1+0.01
PR(3)=0.0001
PR(4)=0.0001
XT=T1+0.01
DO 2 J=1,4
2 D(J)=1.0/4
PRD(1)=TD1
PRD(2)=TD1+0.001
PRD(3)=0.0001
PRD(4)=0.0001
XTD=TD1+0.001
DO 32 J=1,2
32 DD(J)=1.0/2
IF(KK-2)38,39,39
38 CALL KOEF(F1,F2,F3,F4,A11,A14,A44,B1,B4)
CALL FORC(F1,F2,F3,F4,A11,A14,A44,B1,B4)
CALL RKGS(PR,Y,D,4,IH,F,OU,AU)
Y3=Y(3)
Y4=Y(4)
CALL RKGSD(PRD,YD,DD,2,IHD,FD,OUD,AUD)
F1,F4--НАҚТЫЛЫ АЛЫНҒАН ЖАЛПЫЛАМА КООРДИНАТА
СӘЙКЕСІНШЕ 1-ШІ ТИПТІ МАНИПУЛЯТОРҒА
F1=D(1)
F4=D(2)
CALL UR(F1,F2,F3,F4)
XK,YK-- НАҚТЫЛЫ АЛЫНҒАН ҰСТАМ КООРДИНАТАСЫ
СӘЙКЕСІНШЕ 1-ШІ ТИПТІ МАНИПУЛЯТОРҒА
GOTO 31
39 CALL KOEF1(F1,F2,A11,A12,A22,B1,B2)
CALL FORC1(F1,F2,A11,A12,A22,B1,B2)
CALL RKGS(PR,Y,D,4,IH,F,OU,AU)
Y3=Y(3)
Y4=Y(4)
CALL RKGSD(PRD,YD,DD,2,IHD,FD,OUD,AUD)
F1=D(1)
F2=D(2)
31 XK=L1*COS(F1)+L2*COS(F2)
YK=L1*SIN(F1)+L2*SIN(F2)
DX=XC-XK
DY=YC-YK
DEL=SQRT(DX**2+DY**2)/DL
PRINT 91
91 FORMAT(2X,'БЕРІЛЕТІН КООРДИНАТАЛАР ')
PRINT 92,XC,YC,YF3,YF4
92 FORMAT(2X,'XC=',F10.5,3X,'YC=',F10.5,2X,
'F1=',F10.5,2X,'F4=',F10.5/)
PRINT 93
93 FORMAT(2X,'АЛЫНАТЫН КООРДИНАТАЛАР ')
PRINT 92,XK,YK,D(1),D(2)
WRITE(1,88)XK,YK
WRITE(2,89)Y(1),Y(2)
88 FORMAT(2X,'XC=',F10.5,2X,'YC=',F10.5)
89 FORMAT(2X,'FI1=',F10.5,2X,'FI2=',F10.5)
WRITE(3,86)XK,YK
WRITE(4,87)Y(1),Y(2)
86 FORMAT(2X,'XC=',F10.5,2X,'YC=',F10.5)
87 FORMAT(2X,'FI1=',F10.5,2X,'FI2=',F10.5)
PRINT 94,DEL
94 FORMAT(2X,'БЕРІЛГЕННЕН АУЫТҚУЫ =',F7.3,'%')
1 CONTINUE
14 CONTINUE
13 CONTINUE
STOP
END
ПОДПРОГРАММА 1-ҚОЗҒАЛЫС ЗАҢЫН АНЫҚТАУ
SUBROUTINE ZAC(K,T,XC,YC,DXC,DYC)
REAL K11,K12,K21
IF(K.EQ.1) GOTO 1
IF(K.EQ.2) GOTO 2
IF(K.EQ.3) GOTO 3
IF(K.EQ.4) GOTO 4
IF(K.EQ.5) GOTO 5
IF(K.EQ.6) GOTO 6
IF(K.EQ.7) GOTO 7
IF(K.EQ.8) GOTO 8
1 X0=0.8
K11=-0.4
K12=1.1
GOTO 9
2 K21=0.8
Y0=1.1
K11=0.4
GOTO 10
3 X0=0.8
K11=0.4
K12=1.1
GOTO 9
4 K21=0.8
Y0=1.1
K11=-0.4
GOTO 10
5 X0=0.5
K11=-0.4
K12=0.6
GOTO 9
6 K21=0.5
Y0=0.6
K11=0.4
GOTO 10
7 X0=0.5
K11=0.4
K12=0.6
GOTO 9
8 K21=0.5
Y0=0.6
K11=-0.4
GOTO 10
9 XC=X0+K11*T
YC=K12
DXC=K11
DYC=0.0
GOTO 11
10 XC=K21
YC=Y0+K11*T
DXC=0.0
DYC=K11
11 CONTINUE
RETURN
END
ПОДПРОГРАММА 2- МАНИПУЛЯТОРА 1-ДІҢ БАҒДАРЛАМАЛЫҚ ҚОЗҒАЛЫСЫН ҚҰРУ
SUBROUTINE PROGD(XC,DXC,YC,DYC,F11,F44,DF11,DF44)
COMMON /S1/DL1,DL2,DL3,DL4,DL5
R1=SQRT(XC**2+YC**2)
R4=SQRT((DL5-XC)**2+YC**2)
ARG11=YC/XC
ARG21=(DL1**2+R1**2-DL2**2)/2/R1/DL1
ARG12=YC/(DL5-XC)
ARG22=(DL4**2+R4**2-DL3**2)/2/R4/DL4
F11=ATAN(ARG11)+ACOS(ARG21)
F44=ATAN(ARG12)+ACOS(ARG22)
AT1=YC*DXC-XC*DYC
AT4=DYC*(DL5-XC)+DXC*YC
BT1=XC**2+YC**2
BT4=(DL5-XC)**2+YC**2
CT1=DXC*XC+DYC*YC
CT4=DXC*(DL5-XC)+DYC*YC
DR1=CT1/SQRT(BT1)
DR4=CT4/SQRT(BT4)
DT1=DR1*(R1**2-DL1**2-DL2**2)
DT4=DR4*(R4**2-DL4**2-DL3**2)
SQ1=SQRT(4*(DL1*R1)**2-(DL1**2+R1**2-DL2**2)**2)
SQ4=SQRT(4*(DL4*R4)**2-(DL4**2+R4**2-DL3**2)**2)
ET1=2*R1*SQ1
ET4=2*R4*SQ4
DF11=AT1/BT1+DT1/ET1
DF44=AT4/BT4+DT4/ET4
RETURN
END
ПОДПРОГРАММА 3 - МАНИПУЛЯТОРДЫҢ 2-ШІ ЖӘНЕ 3-ШІ БІЫНДАРЫНА СӘЙКЕС БҰРЫШТЫҚ МӘНДЕРІН ЕСЕПТЕУ
SUBROUTINE UR(F11,F22,F33,F44)
COMMON /S1/DL1,DL2,DL3,DL4,DL5
XD=DL4*SIN(F44)
YD=DL4*COS(F44)
XB=DL1*SIN(F11)
YB=DL1*COS(F11)
D1=XD-XB
D2=YD-YB
D=SQRT(D1**2+D2**2)
D3=2*DL3*D
ALFA=ACOS((DL2**2+D**2-DL3**2)/D3)
BETA=ACOS((DL3**2+D**2-DL2**2)/D3)
TETA=ATAN(D2/D1)
F22=11.0-F11+TETA-ALFA
F33=1.57-F44+TETA-BETA
RETURN
END
ПОДПРОГРАММА 4- МАНИПУЛЯТОР 1 ҚОЗҒАЛЫС ТЕҢДЕУІНІҢ ОҢ ЖАҚ БӨЛІГІНІҢ КОЭФФИЦИЕНТТЕРІН ЕСЕПТЕУ
SUBROUTINE KOEF(F11,F22,F33,F44,A11,A14,A44,B1,B4)
COMMON /S/DM1,DM2,DM3,DM4
COMMON /S1/DL1,DL2,DL3,DL4,DL5
COMMON Z,G,T
S0=DM2*DL1*DL2
S1=(DM1*DL1**2+DM2*DL2**2)/3+DM2*DL1**2
S2=(DM1/2+DM2)*G*DL1
S3=DM2*G*DL2/2
S4=(DM4*DL4**2+DM3*DL3**2+3*DM3*DL4**2)/6
S5=DM3*DL3*DL4
S6=(DM4/2+DM3)*G*DL4
S7=DM3*G*DL3/2
S22=DM2*DL2**2
S33=DM3*DL3**2
S34=(DM1/2+DM2)*G*DL1
S20=S22*2/3+S0*COS(F11-F22)
S35=S33/3+2*S5*COS(F11-F22)
A11=S1+S0*COS(F11-F22)+S22*U21**2/3+U21*S20+S33*U31**2/3
A14=S22*U21*U24/3+U24*S20/2+S33/3*U31*U34+U31*S35
A44=S22*U24**2/3+2*S4+S34*COS(F33-F44)/3+4*S5*
COS(F33-F44)/3+S33*U34**2/3+U34*2*S35
B1=-S2*SIN(F11)-S3*(1+U21)*SIN(F22)-S7*U31*SIN(F33)
B4=-S3*SIN(F22)*U24-S6*SIN(F44)-S7*(U34+1)*SIN(F33)
RETURN
END
ПОДПРОГРАММА 5 - МАНИПУЛЯТОРДЫҢ ҚОЗҒАЛЫС ТЕҢДЕУІНІҢ КОМЕКШҢ КОЭФФИЦИЕНТТЕРІН ЕСЕПТЕУ
SUBROUTINE FORC(F11,F22,F33,F44,A11,A14,A44,B11,B44)
COMMON /SS/Z1,TM1,TM4,H1,H4,C2,C3,C5,C6,E1,E2,E3,E4,E5,E6,E7,DU
COMMON /S/DM1,DM2,DM3,DM4
COMMON /S1/DL1,DL2,DL3,DL4,DL5
COMMON Z,G,T
COMMON MD1,MD4
C1=MD1*Z-B11
C4=MD4*Z-B44
C2=C1*A44
C3=C4*A14
C5=C1*A14
C6=C4*A11
U1=DL2*SIN(F22-F33)
U2=DL3*SIN(F22-F33)
U21=DL1*SIN(F33-F11)/U1-1
U24=DL4*SIN(F33-F44)/U1
U31=DL1*SIN(F11-F22)/U2
U34=DL4*SIN(F44-F22)/U2-1
Q1=S22*(U21+1)/3+S0/2*COS(F22)
Q2=S33/3*U31
Q3=S0/2*SIN(F22)
Q4=U24*S22/3
Q5=U31*S34/6*SIN(F33)
Q6=2/3*S5*SIN(F33)
Q7=S33/3*(U34+1)+2/3*S5*COS(F33)
G1=SIN(F22-F33)
G2=TAN(F22-F33)
DU211=(DL3*U31**2-DL1*COS(F11-F33))/(DL2*G1)-
((1-U21)**2)/G2
DU214=DL3*U31*(U34+1)/DL2*G1-(U24*(U21+1))/G2
DU241=DU214
DU311=-(U31**2)/G2-(DL1*SIN(F22)+DL2*(1+U21)**2)/(DL3*G1)
DU244=(DL4*COS(F33)+DL3*(1+U34)**2)/DL2*G1-U24**2/G2
DU344=(DL4*COS(F22-F33)-DL2*U24**2)/(DL3*G1)-
(1+U34)/G2
DU314=U31*(U34+1)/G2-DL1*(U21+1)*U24/DL3*G1
DU341=DU314
DA111=2*(DU211*Q1+Q2*DU311-Q3*U21*(U21+1))
DA441=2*(DU211*Q4-Q5-Q6*U31*(U34+1)+Q7*DU341)
DA141=DU311*Q7+DU241*Q1+Q4*DU211+
Q2*DU341-Q3*U24*U21-Q6*U31*U31
DA114=2*(DU214*Q1+Q2*DU314-Q3*U24*(U21+1))
DA444=2*(DU214*Q4-Q5-Q6*U34*(U34+1)+Q7*DU344)
DA144=DU314*Q7+DU244*Q1+Q4*DU214+
Q2*DU344-Q3*U24*U24-Q6*U31*U34
D1=DA111
D2=DA444
D3=DA114
D4=DA441
D5=DA141
D6=DA144
DU=A11*A44-A14**2
E1=(D1*A44*F11**2-D2*A14*F44**2)/2
E2=(D1*A14*F11**2-D2*A11*F44**2)/2
E3=(D3*A44-D4*A14)*F11*F44
E4=(D3*A14-D4*A11)*F11*F44
E5=(D6-D4/2)*A44*F44**2
E6=(D6-D4/2)*A14*F44**2
E7=(D5-D3/2)*A14*F11**2
E8=(D5-D3/2)*A11*F11**2
RETURN
END
ПОДПРОГРАММА 6 - МАНИПУЛЯТОР ҚОЗҒАЛЫС ТЕҢДЕУІНІҢ ОҢ ЖАҚТАРЫН ҚҰРУ
SUBROUTINE F(X,Y,D)
COMMON /RR/KK
COMMON /SS/Z,TM1,TM4,H1,H4,C2,C3,C5,C6,E1,E2,E3,E4,E5,E6,E7,DU
DIMENSION Y(4),D(4)
IF(KK-2)1,2,2
1 D(1)=Y(3)
D(2)=Y(4)
D(3)=(C2-C3-E1+E3+E5-E7)/DU
D(4)=-(C5-C6-E2+E4+E6)/DU
GOTO 3
2 D(1)=Y(3)
D(2)=Y(4)
D(3)=(C3-C2+E1)/DU
D(4)=-(C6-C5+E2)/DU
3 RETURN
END
ПОДПРОГРАММА 7 - ШЫҒЫС АҚПАРАТТЫ ӨҢДЕУ
SUBROUTINE OU(X,Y,D,IH,N,PR)
COMMON /S3/XT
DIMENSION PR(5),Y(6),D(6)
IF(ABS(X-XT)-0.0001) 8,8,6
8 XT=XT+0.01
6 RETURN
END
ПОДПРОГРАММА 8-РУНГЕ-КУТ ӘДІСІ ТЕҢДЕУІН ИНТЕГРАЛДАУ
SUBROUTINE RKGSD(PRMT,Y,DERY,NDIM,IHLF,FCT,OUTP,AUX)
DIMENSION Y(1),DERY(1),AUX(8,1),A(4),B(4),C(4),PRMT(1)
DO 1 I=1,NDIM
1 AUX(8,I)=.06666667*DERY(I)
X=PRMT(1)
XEND=PRMT(2)
H=PRMT(3)
PRMT(5)=0.
CALL FCT(X,Y,DERY)
IF(H*(XEND-X))38,37,2
2 A(1)=.5
A(2)=.2928932
A(3)=1.707107
A(4)=.1666667
B(1)=2.
B(2)=1.
B(3)=1.
B(4)=2.
C(1)=.5
C(2)=.2928932
C(3)=1.707107
C(4)=.5
DO 3 I=1,NDIM
AUX(1,I)=Y(I)
AUX(2,I)=DERY(I)
AUX(3,I)=0.
3 AUX(6,I)=0.
IREC=0
H=H+H
IHLF=-1
ISTEP=0
IEND=0
4 IF((X+H-XEND)*H)7,6,5
5 H=XEND-X
6 IEND=1
7 CALL OUTP(X,Y,DERY,IREC,NDIM,PRMT)
IF(PRMT(5))40,8,40
8 ITEST=0
9 ISTEP=ISTEP+1
J=1
10 AJ=A(J)
BJ=B(J)
CJ=C(J)
DO 11 I=1,NDIM
R1=H*DERY(I)
R2=AJ*(R1-BJ*AUX(6,I))
Y(I)=Y(I)+R2
R2=R2+R2+R2
11 AUX(6,I)=AUX(6,I)+R2-CJ*R1
IF(J-4)12,15,15
12 J=J+1
IF(J-3)13,14,13
13 X=X+.5*H
14 CALL FCT(X,Y,DERY)
GOTO 10
15 IF(ITEST)16,16,20
16 DO 17 I=1,NDIM
17 AUX(4,I)=Y(I)
ITEST=1
ISTEP=ISTEP+ISTEP-2
18 IHLF=IHLF+1
X=X-H
H=.5*H
DO 19 I=1,NDIM
Y(I)=AUX(1,I)
DERY(I)=AUX(2,I)
19 AUX(6,I)=AUX(3,I)
GOTO 9
20 IMOD=ISTEP/2
IF(ISTEP-IMOD-IMOD)21,23,21
21 CALL FCT(X,Y,DERY)
DO 22 I=1,NDIM
AUX(5,I)=Y(I)
22 AUX(7,I)=DERY(I)
GOTO 9
23 DELT=0.
DO 24 I=1,NDIM
24 DELT=DELT+AUX(8,I)*ABS(AUX(4,I)-Y(I))
IF(DELT-PRMT(4))28,28,25
25 IF(IHLF-10)26,36,36
26 DO 27 I=1,NDIM
27 AUX(4,I)=AUX(5,I)
ISTEP=ISTEP+ISTEP-4
X=X-H
IEND=0
GOTO 18
28 CALL FCT(X,Y,DERY)
DO 29 I=1,NDIM
AUX(1,I)=Y(I)
AUX(2,I)=DERY(I)
AUX(3,I)=AUX(6,I)
Y(I)=AUX(5,I)
29 DERY(I)=AUX(7,I)
CALL OUTP(X-H,Y,DERY,IHLF,NDIM,PRMT)
IF(PRMT(5))40,30,40
30 DO 31 I=1,NDIM
Y(I)=AUX(1,I)
31 DERY(I)=AUX(2,I)
IREC=IHLF
IF(IEND)32,32,39
32 IHLF=IHLF-1
ISTEP=ISTEP/2
H=H+H
IF(IHLF)4,33,33
33 IMOD=ISTEP/2
IF(ISTEP-IMOD-IMOD)4,34,4
34 IF(DELT-.02*PRMT(4))35,35,4
35 IHLF=IHLF-1
ISTEP=ISTEP/2
H=H+H
GOTO 4
36 IHLF=11
CALL FCT(X,Y,DERY)
GOTO 39
37 IHLF=12
GOTO 39
38 IHLF=13
39 CALL OUTP(X,Y,DERY,IHLF,NDIM,PRMT)
40 RETURN
END
ПОДПРОГРАММА 9- СИПАТТАЙТЫН ҚОЗҒАЛТҚЫШ ТЕҢДЕУІНІҢ ОҢ ЖАҒЫН ҚҰРУ
SUBROUTINE FD(XD,YD,DD)
COMMON /SS/Z1,TM1,TM4,H1,H4,C2,C3,C5,C6,E1,E2,E3,E4,E5,E6,E7,DU
COMMON /SS/Z,C2,C3,C5,C6,E1,E2,E3,E4,E5,E6,E7,DU
COMMON H1,H4,TM1,TM4
COMMON /R/Y3,Y4
DIMENSION YD(2),DD(2)
DD(1)=-(H1*Z*Y3+YD(1))/TM1
DD(2)=-(H4*Z*Y4+YD(2))/TM4
RETURN
END
ПОДПРОГРАММА 10-ШЫҒЫС АҚПАРАТТЫ ӨҢДЕУ
SUBROUTINE OUD(XD,YD,DD,IHD,N,PRD)
COMMON /S3/XT,XTD
DIMENSION PRD(5),YD(2),DD(2)
IF(ABS(XD-XTD)-0.0001) 8,8,6
8 XTD=XTD+0.001
6 RETURN
END
ПОДПРОГРАММА 11- МАНИПУЛЯТОР 2-НІҢ ҚОЗҒАЛЫС ТЕҢДЕУІН ҚҰРУ
SUBROUTINE PROGD1(XC,DXC,YC,DYC,F11,F22,DF11,DF22)
COMMON /S1/DL1,DL2
R1=XC**2+YC**2-DL1**2-DL2**2+2*(DL1**2-YC*DL1)
R2=XC**2+YC**2-DL1**2-DL2**2
R3=SQRT(4*DL1**2*DL2**2-(XC**2+YC**2-DL1**2-DL2**2)**2)
R4=2*DL1*DL2
F11=ATAN(R1/R3)
F22=ATAN(R2/R4)
AT=XC**2+YC**2-DL1**2-DL2**2
FT=SQRT(4*DL1**2*DL2**2-AT**2)
BT=2*(DL1**2-YC*DL1)+AT
DAT=2*(XC*DXC+YC*DYC)
DFT=-AT*DAT/FT
CT=DFT+2*DXC*DL1
RT=CT**2+BT**2
DBT=DAT-2*DL1*DYC
DCT=DFT+2*DXC*DL1
PT=CT*DBT-BT*DCT
DF11=2*PT/RT
DF22=-DAT/FT
RETURN
END
ПОДПРОГРАММА 12 - МАНИПУЛЯТОР 2-НІҢ ҚОЗҒАЛЫС ТЕҢДЕУІНІҢ ОҢ ЖАҚТАРЫНЫҢ КОЭФФИЦИЕНТТЕРІН ЕСЕПТЕУ
SUBROUTINE KOEF1(F11,F22,A11,A12,A22,B1,B2)
COMMON /S/DM1,DM2
COMMON /S1/DL1,DL2
COMMON Z,G,T
S0=DM2*DL1*DL2
S1=(DM1*DL1**2+DM2*DL2**2)/3+DM2*DL1**2
S2=(DM1/2+DM2)*G*DL1
S3=DM2*G*DL2/2
S22=DM2*DL2**2
A11=S1+S0*COS(F22)
A12=S22/3+S0/2*COS(F22)
A22=S22/3
B1=-S2*SIN(F11)-S3*SIN(F22)
B2=-S3*SIN(F22)
RETURN
END
ПОДПРОГРАММА 13- МАНИПУЛЯТОР 2-НІҢ ҚОЗҒАЛЫС ТЕҢДЕУІНІҢ КӨМЕКШІ КОЭФФИЦИЕНТТЕРІН ЕСЕПТЕУ
SUBROUTINE FORC1(F1,F2,A11,A12,A22,B1,B2)
COMMON /SS/Z1,TM1,TM4,H1,H4,C2,C3,C5,C6,E1,E2,E3,E4,E5,E6,E7,DU
COMMON /SS/Z1,TM1,TM4,H1,H4,C2,C3,C5,C6,E1,E2,DU
COMMON /S/DM1,DM2
COMMON /S1/DL1,DL2
COMMON Z,G,T
COMMON MD1,MD4
C1=MD1*Z-B1
C4=MD4*Z-B2
C2=C1*A22
C3=C4*A12
C5=C1*A12
C6=C4*A11
D1=-DM2*DL1*DL2*SIN(F2)
D2=-DM2*DL1*DL2/2*SIN(F2)
DU=A11*A22-A12**2
E1=(D1*A12*F1**2+D2*A22*F2**2)/2+D1*A22*F1*F2
E2=(D1*A11*F1**2+D2*A12*F2**2)/2+D1*A12*F1*F2
RETURN
END
ҚОСЫМША Б
-
T
XC
YC
XK
YK
DEL
0,01
0,796
1,1
0,781
1,105
0,079
0,02
0,792
1,1
0,777
1,106
0,08
0,03
0,788
1,1
0,773
1,106
0,082
0,04
0,784
1,1
0,769
1,107
0,083
0,05
0,78
1,1
0,765
1,107
0,084
0,06
0,776
1,1
0,761
1,107
0,085
0,07
0,772
1,1
0,757
1,108
0,087
0,08
0,768
1,1
0,752
1,108
0,088
0,09
0,764
1,1
0,748
1,109
0,089
0,1
0,76
1,1
0,744
1,109
0,09
0,11
0,756
1,1
0,74
1,109
0,092
0,12
0,752
1,1
0,736
1,11
0,093
0,13
0,748
1,1
0,732
1,11
0,094
0,14
0,744
1,1
0,728
1,111
0,096
0,15
0,74
1,1
0,724
1,111
0,097
0,16
0,736
1,1
0,72
1,111
0,099
0,17
0,732
1,1
0,716
1,112
0,1
0,18
0,728
1,1
0,712
1,112
0,102
0,19
0,724
1,1
0,708
1,113
0,103
0,2
0,72
1,1
0,704
1,113
0,105
0,21
0,716
1,1
0,7
1,114
0,106
0,22
0,712
1,1
0,696
1,114
0,108
0,23
0,708
1,1
0,692
1,115
0,109
0,24
0,704
1,1
0,688
1,115
0,111
0,25
0,7
1,1
0,684
1,115
0,113
0,26
0,696
1,1
0,68
1,116
0,115
0,27
0,692
1,1
0,675
1,116
0,116
0,28
0,688
1,1
0,671
1,117
0,118
0,29
0,684
1,1
0,667
1,117
0,12
0,3
0,68
1,1
0,663
1,118
0,122
0,31
0,676
1,1
0,659
1,118
0,124
0,32
0,672
1,1
0,655
1,119
0,126
0,33
0,668
1,1
0,651
1,119
0,128
0,34
0,664
1,1
0,647
1,12
0,13
0,35
0,66
1,1
0,643
1,121
0,133
0,36
0,656
1,1
0,639
1,121
0,135
0,37
0,652
1,1
0,635
1,122
0,137
0,38
0,648
1,1
0,631
1,122
0,14
0,39
0,644
1,1
0,627
1,123
0,143
0,4
0,64
1,1
0,623
1,124
0,145
0,41
0,636
1,1
0,619
1,124
0,148
0,42
0,632
1,1
0,615
1,125
0,151
0,43
0,628
1,1
0,611
1,126
0,154
0,44
0,624
1,1
0,607
1,126
0,157
0,45
0,62
1,1
0,603
1,127
0,16
0,46
0,616
1,1
0,599
1,128
0,164
0,47
0,612
1,1
0,595
1,129
0,167
0,48
0,608
1,1
0,591
1,129
0,171
0,49
0,604
1,1
0,587
1,13
0,175
0,5
0,6
1,1
0,582
1,131
0,179