|
10 | 10 |
|
11 | 11 | importos |
12 | 12 | importmatplotlib.pyplotasplt# MATLAB plotting functions |
13 | | -fromcontrol.matlabimport*# MATLAB-like functions |
14 | 13 | importnumpyasnp |
15 | 14 | importmath |
16 | 15 | importcontrolasct |
|
23 | 22 | c=0.05# damping factor (estimated) |
24 | 23 |
|
25 | 24 | # Transfer functions for dynamics |
26 | | -Pi=tf([r], [J,0,0])# inner loop (roll) |
27 | | -Po=tf([1], [m,c,0])# outer loop (position) |
| 25 | +Pi=ct.tf([r], [J,0,0])# inner loop (roll) |
| 26 | +Po=ct.tf([1], [m,c,0])# outer loop (position) |
28 | 27 |
|
29 | 28 | # Use state space versions |
30 | | -Pi=tf2ss(Pi) |
31 | | -Po=tf2ss(Po) |
| 29 | +Pi=ct.tf2ss(Pi) |
| 30 | +Po=ct.tf2ss(Po) |
32 | 31 |
|
33 | 32 | # |
34 | 33 | # Inner loop control design |
|
40 | 39 |
|
41 | 40 | # Design a simple lead controller for the system |
42 | 41 | k,a,b=200,2,50 |
43 | | -Ci=k*tf([1,a], [1,b])# lead compensator |
| 42 | +Ci=k*ct.tf([1,a], [1,b])# lead compensator |
44 | 43 |
|
45 | 44 | # Convert to statespace |
46 | | -Ci=tf2ss(Ci) |
| 45 | +Ci=ct.tf2ss(Ci) |
47 | 46 |
|
48 | 47 | # Compute the loop transfer function for the inner loop |
49 | 48 | Li=Pi*Ci |
50 | 49 |
|
51 | 50 |
|
52 | 51 | # Bode plot for the open loop process |
53 | 52 | plt.figure(1) |
54 | | -bode(Pi) |
| 53 | +ct.bode(Pi) |
55 | 54 |
|
56 | 55 | # Bode plot for the loop transfer function, with margins |
57 | 56 | plt.figure(2) |
58 | | -bode(Li) |
| 57 | +ct.bode(Li) |
59 | 58 |
|
60 | 59 | # Compute out the gain and phase margins |
61 | 60 | #! Not implemented |
62 | 61 | # (gm, pm, wcg, wcp) = margin(Li); |
63 | 62 |
|
64 | 63 | # Compute the sensitivity and complementary sensitivity functions |
65 | | -Si=feedback(1,Li) |
| 64 | +Si=ct.feedback(1,Li) |
66 | 65 | Ti=Li*Si |
67 | 66 |
|
68 | 67 | # Check to make sure that the specification is met |
69 | 68 | plt.figure(3) |
70 | | -gangof4(Pi,Ci) |
| 69 | +ct.gangof4(Pi,Ci) |
71 | 70 |
|
72 | 71 | # Compute out the actual transfer function from u1 to v1 (see L8.2 notes) |
73 | 72 | # Hi = Ci*(1-m*g*Pi)/(1+Ci*Pi); |
74 | | -Hi=parallel(feedback(Ci,Pi),-m*g*feedback(Ci*Pi,1)) |
| 73 | +Hi=ct.parallel(ct.feedback(Ci,Pi),-m*g*ct.feedback(Ci*Pi,1)) |
75 | 74 |
|
76 | 75 | plt.figure(4) |
77 | 76 | plt.clf() |
78 | | -bode(Hi) |
| 77 | +ct.bode(Hi) |
79 | 78 |
|
80 | 79 | # Now design the lateral control system |
81 | 80 | a,b,K=0.02,5,2 |
82 | | -Co=-K*tf([1,0.3], [1,10])# another lead compensator |
| 81 | +Co=-K*ct.tf([1,0.3], [1,10])# another lead compensator |
83 | 82 |
|
84 | 83 | # Convert to statespace |
85 | | -Co=tf2ss(Co) |
| 84 | +Co=ct.tf2ss(Co) |
86 | 85 |
|
87 | 86 | # Compute the loop transfer function for the outer loop |
88 | 87 | Lo=-m*g*Po*Co |
89 | 88 |
|
90 | 89 | plt.figure(5) |
91 | | -bode(Lo,display_margins=True)# margin(Lo) |
| 90 | +ct.bode(Lo,display_margins=True)# margin(Lo) |
92 | 91 |
|
93 | 92 | # Finally compute the real outer-loop loop gain + responses |
94 | 93 | L=Co*Hi*Po |
95 | | -S=feedback(1,L) |
96 | | -T=feedback(L,1) |
| 94 | +S=ct.feedback(1,L) |
| 95 | +T=ct.feedback(L,1) |
97 | 96 |
|
98 | 97 | # Compute stability margins |
99 | 98 | #! Not yet implemented |
100 | 99 | # (gm, pm, wgc, wpc) = margin(L); |
101 | 100 |
|
102 | 101 | plt.figure(6) |
103 | 102 | plt.clf() |
104 | | -out=ct.bode(L,logspace(-4,3),initial_phase=-math.pi/2) |
| 103 | +out=ct.bode(L,np.logspace(-4,3),initial_phase=-math.pi/2) |
105 | 104 | axs=ct.get_plot_axes(out) |
106 | 105 |
|
107 | 106 | # Add crossover line to magnitude plot |
|
111 | 110 | # Nyquist plot for complete design |
112 | 111 | # |
113 | 112 | plt.figure(7) |
114 | | -nyquist(L) |
| 113 | +ct.nyquist(L) |
115 | 114 |
|
116 | 115 | # set up the color |
117 | 116 | color='b' |
|
126 | 125 | # 'EdgeColor', color, 'FaceColor', color); |
127 | 126 |
|
128 | 127 | plt.figure(9) |
129 | | -Yvec,Tvec=step(T,linspace(1,20)) |
| 128 | +Yvec,Tvec=ct.step_response(T,np.linspace(1,20)) |
130 | 129 | plt.plot(Tvec.T,Yvec.T) |
131 | 130 |
|
132 | | -Yvec,Tvec=step(Co*S,linspace(1,20)) |
| 131 | +Yvec,Tvec=ct.step_response(Co*S,np.linspace(1,20)) |
133 | 132 | plt.plot(Tvec.T,Yvec.T) |
134 | 133 |
|
135 | 134 | #TODO: PZmap for statespace systems has not yet been implemented. |
|
142 | 141 | # Gang of Four |
143 | 142 | plt.figure(11) |
144 | 143 | plt.clf() |
145 | | -gangof4(Hi*Po,Co,linspace(-2,3)) |
| 144 | +ct.gangof4(Hi*Po,Co,np.linspace(-2,3)) |
146 | 145 |
|
147 | 146 | if'PYCONTROL_TEST_EXAMPLES'notinos.environ: |
148 | 147 | plt.show() |