110 DIMENSION A(15),B(15),D(15),E(15),C(5,100) 120 COMMON W(100) 130 WRITE(1,911) 140 911 FORMAT(//////) 150 DO 684 I=2,20 160 D(I)=0. 170 E(I)=0. 180 A(I)=0. 190 684 B(I)=0. 200 D(1)=1. 210 E(1)=1. 220 A(1)=1. 230 B(1)=1. 240 WRITE(1,741) 250 741 FORMAT(1X,"**************************************************" 260 + "*****"/1X,"**** FREQUENCY RESPONSE ANALYSIS OF CONTROL " 270 + "SYSTEMS ****"///) 280 501 FORMAT(1X,/"**** ENTER DATA FOR THE FORWARD TRANSFER" 290 + " FUNCTION, G(S). ****") 300 WRITE(1,0) "** ENTER TYPE OF INPUT DATA:" 310 WRITE(1,0) " --- 1=POLYNOMIAL FORM" 320 WRITE(1,0) " 2=FACTORED FORM" 330 READ(0,0) ITYPE 340 WRITE(1,501) 350 IF (ITYPE.EQ.2) GOTO 20 360 WRITE(1,510) 370 510 FORMAT(1X,"** ENTER ORDERS OF NUM. AND DENOM. POLYNOMIALS", 380 + " OF G(S), NN,ND= ") 390 READ(0,0) NN,ND 400 CALL POLY(NN,ND,A,B) 410 GO TO 530 420 20 CALL FACTOR(NN,ND,A,B) 430 530 WRITE(1,710) 440 710 FORMAT(1X,//"**** ENTER DATA FOR THE FEEDBACK TRANSFER" 450 + " FUNCTION H(S). ****"/) 460 WRITE(1,0) "*** IS H(S)=1? (1=YES,0=NO)" 470 READ(0,0) NUNITY 480 IF (NUNITY.NE.1) GOTO 552 490 MY=0 500 GOTO 731 510 552 IF (ITYPE.EQ.2) GOTO 712 520 WRITE(1,710) 530 711 FORMAT(1X,/"ENTER ORDER OF NUM ANF DENOM POLYNOMIALS OF" 540 + " H(S), L1, L2= ") 550 READ(0,0) L1,L2 560 CALL POLY(L1,L2,D,E) 570 GOTO 713 580 712 CALL FACTOR(L1,L2,D,E) 590 713 CALL MPLY(A,D,NN,L1) 600 CALL MPLY(B,E,ND,L2) 610 MY=L1 620 IF (MY.LT.L2) MY=L2 630 731 CONTINUE 640 WRITE(1,257) 650 WRITE(1,661) 660 661 FORMAT(1X,"NUMERATOR AND DENOMINATOR COEFFICIENTS OF GH(S)"/) 670 DO 251 I=1,NN 680 251 WRITE(1,252) I,A(I) 690 252 FORMAT(1X,"A(",I2,")= ",F8.2) 700 WRITE(1,256) 710 DO 253 I=1,ND 720 253 WRITE(1,254) I,B(I) 730 254 FORMAT(1X,"B(",I2,")= ",F8.2) 740 WRITE(1,256) 750 IF (NUNITY.EQ.1) GOTO 825 760 WRITE(1,714) 770 714 FORMAT(1X,"NUMERATOR AND DENOMINATOR COEFFICIENTS OF H(S)"/) 780 DO 715 I=1,L1 790 715 WRITE(1,716) I,D(I) 800 716 FORMAT(1X,"D(",I2,")= ",F8.2) 810 WRITE(1,256) 820 DO 717 I=1,L2 830 717 WRITE(1,718) I,E(I) 840 718 FORMAT(1X,"E(",I2,")= ",F8.2) 850 WRITE(1,257) 860 256 FORMAT(/) 870 GOTO 826 880 825 WRITE(1,579) 890 579 FORMAT(1X,"THE FEEDBACK TRANSFER FUNCTION H(S) IS UNITY"///) 900 826 CONTINUE 910 WRITE(1,0) "ENTER MIN. & MAX. RADIAN FREQS., W1, W2=" 920 READ(0,0) W1,W2 930 PH=180./3.1415927 940 ADT=-360. 950 AD=-180. 960 WRITE(1,0) "ENTER NUMBER OF POINTS DESIRED, NPOINT=" 970 READ(0,0) NPOINT 980 K=NPOINT 990C==== GENERATE RADIAN FREQUENCY. 1000 CALL WFREQ(W1,W2,K) 1010 MX=NN 1020 IF (ND.GT.NN) MX=ND 1030 DO 333 I=1,5 1040 DO 333 J=1,100 1050 333 C(I,J)=0. 1060C==== COMPUTE LOG-MAGNITUDE OF GH(JW), PHASE, AND OTHERS. 1070 DO 150 J=1,K 1080 OMEGA=W(J) 1090 CALL MAG(A,B,MX,OMEGA,VR,VI,VMAG) 1100 C(1,J)=20.*ALOG10(VMAG) 1110 C(2,J)=ATAN2(VI,VR)*PH 1120 IF (J.LT.2) GOTO 298 1130 IF (C(2,J)*C(2,J-1).GT.0.) GOTO 294 1140 C(2,J)=C(2,J)+ADT 1150 GOTO 298 1160 294 IF (C(2,J-1).GT.AD) GOTO 298 1170 C(2,J)=C(2,J)+ADT 1180 298 C(3,J)=VR 1190 C(4,J)=VI 1200 150 C(5,J)=VMAG 1210 WRITE(1,7) 1220 7 FORMAT(///,4X,"W(I)",5X,"LM(I)",3X,"ANG(I)",5X, 1230 + "RM(I)",5X,"IM(I)",//) 1240 DO 170 I=1,K 1250 WRITE(1,8) W(I),C(1,I),C(2,I),C(3,I),C(4,I) 1260 170 CONTINUE 1270 WRITE(1,257) 1280 257 FORMAT(//) 1290 8 FORMAT(1X,F7.3,3X,F7.2,3X,F6.1,2X,F8.3,2X,F8.3) 1300C==== 1310C==== COMPUTE M AND ALPHA OF THE CLOSED SYSTEM. 1320 533 DO 210 I=1,K 1330 IF (MY.EQ.0) GOTO 610 1340 OMEGA=W(I) 1350 CALL MAG(D,E,MY,OMEGA,VR,VI,VMAG) 1360 610 R2=C(4,I)*C(4,I) 1370 R3=1.+C(3,I) 1380 R4=R3*R3 1390 C(1,I)=C(5,I)/SQRT(R4+R2) 1400 IF (MY.NE.0) C(1,I)=C(1,I)/VMAG 1410 C(2,I)=ATAN2(C(4,I),C(3,I))-ATAN2(C(4,I),R3) 1420 C(2,I)=C(2,I)*PH 1430 IF (MY.NE.0) C(2,I)=C(2,I)-ATAN2(VI,VR)*PH 1440 IF (I.LT.2) GOTO 210 1450 IF (C(2,I)*C(2,I-1).GT.0.) GOTO 219 1460 C(2,I)=C(2,I)+ADT 1470 GOTO 210 1480 219 IF (C(2,I-1).GT.AD) GOTO 210 1490 C(2,I)=C(2,I)+ADT 1500 210 CONTINUE 1510 WRITE(1,215) 1520 WRITE(1,216) 1530 DO 217 I=1,K 1540 217 WRITE(1,218) W(I),C(1,I),C(2,I) 1550 215 FORMAT(/////,10X,"*****************************",/ 1560 + 10X,"***** CLOSED LOOP RESPONSE *****"/) 1570 216 FORMAT(4X,"OMEGA",4X,"MAGNITUDE",5X,"PHASE"/3X, 1580 + "RAD/SEC",16X,"DEGREES"/) 1590 218 FORMAT(2X,F7.2,5X,F7.4,4X,F7.1) 1600 541 FORMAT(1X,//"** ENTER ORDER OF NUM. AND DENOM. POLYNOMIALS" 1610 + " OF H(S), L1,L2= ") 1620 599 STOP 1630 END 1640C 1650C 1660 SUBROUTINE REALP(D,ROOT,I) 1670 DIMENSION E(2),Q(20),D(20) 1680 DO 10 J=1,I 1690 Q(J)=D(J) 1700 10 D(J)=0. 1710 E(2)=1. 1720 E(1)=ROOT 1730 DO 20 N=1,I 1740 DO 20 M=1,2 1750 NM=N+M-1 1760 20 D(NM)=D(NM)+Q(N)*E(M) 1770 RETURN 1780 END 1790C 1800C 1810 SUBROUTINE CMPLX(D,R,X,I) 1820 DIMENSION E(3),Q(20),D(20) 1830 DO 10 J=1,I 1840 Q(J)=D(J) 1850 10 D(J)=0. 1860 E(3)=1. 1870 E(2)=2.*R 1880 E(1)=R*R+X*X 1890 DO 20 N=1,I 1900 DO 20 M=1,3 1910 NM=N+M-1 1920 20 D(NM)=D(NM)+Q(N)*E(M) 1930 RETURN 1940 END 1950C 1960C 1970 SUBROUTINE WFREQ(W1,W2,N) 1980 COMMON W(100) 1990 RN=N 2000 N=N+1 2010 A=ALOG10(W2) 2020 B=ALOG10(W1) 2030 DT=(A-B)/RN 2040 W(1)=W1 2050 DO 10 I=2,100 2060 B=B+DT 2070 10 W(I)=10.**(B) 2080 RETURN 2090 END 2100C 2110C 2120 SUBROUTINE POLY(NN,ND,A,B) 2130 DIMENSION A(15),B(15) 2140 NN=NN+1 2150 ND=ND+1 2160 WRITE(1,0) "** ENTER NUMERATOR COEFFICIENTS:" 2170 DO 10 I=1,NN 2180 WRITE(1,2) I 2190 2 FORMAT(1X,"A(",I2,")= ") 2200 10 READ(0,0) A(I) 2210 WRITE(1,0) "** ENTER DENOMINATOR COEFFICIENTS:" 2220 DO 11 I=1,ND 2230 WRITE(1,3) I 2240 3 FORMAT(1X, "B(",I2,")= ") 2250 READ(0,0) B(I) 2260 11 CONTINUE 2270 RETURN 2280 END 2290C 2300C 2310 SUBROUTINE FACTOR(NN,ND,A,B) 2320 DIMENSION A(15),B(15) 2330 20 WRITE(1,0) "** ENTER NUMBER OF REAL ZEROS AND NUMBER OF" 2340 WRITE(1,0) " COMPLEX ZERO PAIRS, NR,NC= " 2350 READ(0,0) N1,N2 2360 WRITE(1,0) "** ENTER NUMBER OF REAL POLES AND NUMBER OF" 2370 WRITE(1,0) " COMPLEX POLE PAIRS, MR,MC= " 2380 WRITE(1,0) "MR,MC= " 2390 READ(0,0) M1,M2 2400 NN=N1+N2*2+1 2410 ND=M1+M2*2+1 2420 IF (N1.EQ.0) GOTO 621 2430 WRITE(1,0) "** ENTER REAL ZEROS:" 2440 DO 22 I=1,N1 2450 WRITE(1,4) I 2460 4 FORMAT(" RZERO(",I2,")= ") 2470 READ(0,0) ROOT 2480 ROOT=-ROOT 2490 CALL REALP(A,ROOT,I) 2500 22 CONTINUE 2510 621 IF (N2.EQ.0) GOTO 622 2520 N11=N1+1 2530 WRITE(1,0) "** ENTER COMPLEX ZERO PAIRS:" 2540 DO 23 I=1,N2 2550 WRITE(1,5) I 2560 5 FORMAT(" CZERO-R,X-(",I2,")= ") 2570 READ(0,0) R,X 2580 R=-R 2590 K=N11+2*(I-1) 2600 CALL CMPLX(A,R,X,K) 2610 23 CONTINUE 2620 622 IF (M1.EQ.0) GOTO 623 2630 WRITE(1,0) "** ENTER REAL POLES:" 2640 DO 24 I=1,M1 2650 WRITE(1,629) I 2660 629 FORMAT(" RPOLE(",I2,")= ") 2670 READ(0,0) ROOT 2680 ROOT=-ROOT 2690 CALL REALP(B,ROOT,I) 2700 24 CONTINUE 2710 623 IF (M2.EQ.0) GOTO 624 2720 M11=M1+1 2730 WRITE(1,0) "** ENTER COMPLEX POLE PAIRS:" 2740 DO 25 I=1,M2 2750 WRITE(1,628) I 2760 628 FORMAT(" CPOLE-R,X-(",I2,")= ") 2770 READ(0,0) R,X 2780 R=-R 2790 K=M11+2*(I-1) 2800 CALL CMPLX(B,R,X,K) 2810 25 CONTINUE 2820 624 CONTINUE 2830 WRITE(1,6) 2840 6 FORMAT("** ENTER GAIN CONSTANT, GAIN=") 2850 READ(0,0) GAIN 2860 DO 26 I=1,NN 2870 26 A(I)=A(I)*GAIN 2880 30 CONTINUE 2890 RETURN 2900 END 2910C 2920C 2930 SUBROUTINE MPLY(A,B,NN,L1) 2940 DIMENSION A(15),B(15),FM(15) 2950 DO 30 I=1,15 2960 30 FM(I)=0. 2970 DO 10 I=1,NN 2980 DO 10 J=1,L1 2990 L=I+J-1 3000 10 FM(L)=FM(L)+A(I)*B(J) 3010 NN=NN+L1-1 3020 DO 20 I=1,NN 3030 20 A(I)=FM(I) 3040 RETURN 3050 END