|
10 | 10 |
|
11 | 11 | import os |
12 | 12 | import matplotlib.pyplot as plt # MATLAB plotting functions |
13 | | -from control.matlab import * # MATLAB-like functions |
14 | 13 | import numpy as np |
15 | 14 | import math |
16 | 15 | import control as ct |
|
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' not in os.environ: |
148 | 147 | plt.show() |
0 commit comments