Қосымша - 1

ТҰЙЫҚ ЖӘНЕ ТҰЙЫҚ ЕМЕС КИНЕМАТИКАЛЫҚ ТІЗБЕКТЕР НЕГІЗІНДЕГІ МАНИПУЛЯТОРЛАРДЫҢ ДИНАМИКАСЫНЫҢ ЕСЕБІНІҢ ПРОГРАММАСЫ

НЕГІЗГІ БАҒДАРЛАМА

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

МЕХАНИЗМІ ТҰЙЫҚ ЕМЕС КИНЕМАТИКАЛЫҚ ТІЗБЕК НЕГІЗІНДЕГІ МАНИПУЛЯТОРДЫҢ ДИНАМИКАСЫН ЕСЕПТЕУ ПРОГРАММАСЫ

EXTERNAL F,OU,ZAC,PROGD1,KOEF1

COMMON Z,TM1,TM2,H1,H2,M1,M2,L1,L2,G,XT

DIMENSION Y(6),PR(5),D(6),AU(48)

REAL L1,L2,M1,M2

OPEN(1,FILE='rzad.dat')

OPEN(2,FILE='rpol.dat')

OPEN(3,FILE='rotk.dat')

OPEN (4, FILE ='RMECH.rez')

WRITE (1,91)

WRITE (2,93)

WRITE (3,90)

WRITE(4,215)

215 FORMAT(43X,'КЕСТЕ')

WRITE(4,115)

115 FORMAT(1X,52('-'))

WRITE(4,16)

16 FORMAT(1X,'! T ! ХC ! YC !',* ' XK ! YK ! DEL !')

WRITE(4,115)

90 FORMAT(2x,'АУЫТҚУ ҚАТЕЛІГІ')

БАҒДАРЛАМАЛЫ ОРЫН АУЫСТЫРУДЫ ТАҢДАУ

DO 14 K=1,8

K=1

PRINT 15,K

15 FORMAT(' ОРЫН АУЫСТЫРУ ',I2)

БАСТАПҚЫ МӘНДЕРІ

L1=1.

L2=1.

M1=20.

M2=10.

H1=328.

H2=328.

TM1=0.09

TM2=0.09

G=9.8

ҚОЗҒАЛЫСТЫ МОДЕЛДЕУ

DO 1 I=1,50,5

T=0.01*I

CALL ZAC(K,T,XC,YC,DXC,DYC)

CALL PROGD1(XC,DXC,YC,DYC,F1,F2,DF1,DF2)

БАСТАПҚЫ ОРНАЛАСУЫ ЖӘНЕ ЖЫЛДАМДЫҒЫ

Y(1)=0.0

Y(2)=0.0

Y(3)=F1

Y(4)=F2

Y(5)=0.08

Y(6)=0.08

MD1=Y(5)

MD4=Y(6)

PR(1)=T

PR(2)=T+0.01

PR(3)=0.0001

PR(4)=0.0001

XT=T+0.01

DO 2 J=1,6

2 D(J)=1.0/6

CALL KOEF1(F1,F2,A11,A12,A22,B1,B2)

CALL RKGS(PR,Y,D,6,IH,F,OU,AU)

IF(ABS(X-T)-0.01) 31,31,1

31 XK=L1*COS(F1)+L2*COS(F2)

YK=L1*SIN(F1)+L2*SIN(F2)

DX=XC-XK

DY=YC-YK

DEL=100*SQRT(DX**2+DY**2)/DL

PRINT 91

91 FORMAT(2X,'ҰСТАМНЫҢ БЕРІЛЕТІН КООРДИНАТТАРЫ')

PRINT 92,XC,YC

write(1,*) XC,YC

92 FORMAT(2X,'XC=',F10.6,5X,'YC=',F10.6)

PRINT 93

93 FORMAT(2X,'ҰСТАМНЫҢ АЛЫНАТЫН КООРДИНАТТАРЫ')

PRINT 92,XK,YK

write(2,*) XK,YK

PRINT 94,DEL

write(3,*) T,DEL

94 FORMAT(2X,'DELTA=',F15.6)

write(4,999) T,XC,YC,XK,YK,DEL

999 FORMAT(1X,'! ',F3.2,' !',5(1X,F5.3,' !'))

T=T+0.01

1 CONTINUE

WRITE(4,115)

14 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-МАНИПУЛЯТОРДЫҢ БАҒДАРЛАМАЛЫ ҚОЗҒАЛЫСЫН ҚҰРУ

SUBROUTINE PROGD1(XC,DXC,YC,DYC,F11,F22,DF11,DF22)

COMMON Z,TM1,TM2,H1,H2,M1,M2,L1,L2,G,XT

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

ПОДПРОГРАММА 3- МАНИПУЛЯТОР ҚОЗҒАЛЫС ТЕҢДЕУІНІҢ ОҢ ЖАҚТАРЫНЫҢ КОЭФФИЦИЕНТТЕРІН ЕСЕПТЕУ

SUBROUTINE KOEF1(F11,F22,A11,A12,A22,B1,B2)

COMMON Z,TM1,TM2,H1,H2,M1,M2,L1,L2,G,XT

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

ПОДПРОГРАММА 4-МАНИПУЛЯТОР ҚОЗҒАЛЫС ТЕҢДЕУІНІҢ ОҢ ЖАҚТАРЫНЫҢ ҚҰРУ

SUBROUTINE F(X,Y,D)

COMMON Z,TM1,TM2,H1,H2,M1,M2,L1,L2,G,XT

DIMENSION Y(6),D(6)

P5=Y(1)*Z

P6=Y(2)*Z

D(5)=Y(5)-P5/N1/T1/W1

D(6)=Y(6)-P6/N2/T2/W2

Q1=Y(5)*Z

Q2=Y(6)*Z

A1=M1

A2=4*M*L**2/3+M*(Y(2)**2-2*L*Y(2))

B1=-M1*(Y(4)-(L/2-Y(2))*Y(1)**2)-M*(Y(2)+L)*Y(3)**2

B2=(2*M*Y(4)*(Y(2)-L)+4*M*L**2/3+M*(Y(2)**2-2*L*Y(2)))* *Y(3)+M1*(L/2-Y(2))**2*Y(1)

C1=-M*G*COS(Y(1))

C2=-M*G*(L/2-Y(2))*SIN(Y(1))

D(1)=Y(3)

D(2)=Y(4)

D(3)=(Y(1)-B1-C1)/A1

D(4)=(Y(2)-B2-C2)/A2

RETURN

END

ПОДПРОГРАММА 5-ШЫҒЫС АҚПАРАТТЫ ӨҢДЕУ

SUBROUTINE OU(X,Y,D,IH,N,PR)

COMMON Z,TM1,TM2,H1,H2,M1,M2,L1,L2,G,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

ПОДПРОГРАММА 6 - ТЕҢДЕУДІ РУНГЕ-КУТ ӘДІСІМЕН ИНТЕГРАЛДАУ

SUBROUTINE RKGS(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