|
1 #!/usr/bin/python |
|
2 # 9.21 |
|
3 |
|
4 import os, sys |
|
5 sys.path += [os.getcwdu() + os.sep + ".." + os.sep + "python"] |
|
6 |
|
7 import scipy as sp |
|
8 from myc2d import myc2d |
|
9 from scipy import signal |
|
10 from desired import desired |
|
11 from pd import pd |
|
12 |
|
13 # Motor control problem |
|
14 # Transfer function |
|
15 |
|
16 a = sp.array([[-1, 0], [1, 0]]) |
|
17 b = sp.array([[1],[0]]) |
|
18 c = sp.array([0, 1]) |
|
19 d = 0 |
|
20 G = signal.lti(a,b,c,d) |
|
21 Ts = 0.25 |
|
22 B, A, k = myc2d(G,Ts) |
|
23 num, den = G.num, G.den |
|
24 |
|
25 # Transient specifications |
|
26 rise = 3 |
|
27 epsilon = 0.05 |
|
28 phi = desired(Ts,rise,epsilon) |
|
29 |
|
30 # Controller design |
|
31 Delta = 1 # No internal model of step used |
|
32 Rc, Sc = pp_pid(B, A, k, phi, Delta) |
|
33 |
|
34 # continuous time controller |
|
35 K, taud, N = pd(Rc, Sc, Ts) |
|
36 numb = K*sp.array([1, taud*(1+1/N)]) |
|
37 denb = sp.array([1, taud/N]) |
|
38 numf = 1 |
|
39 denf = 1 |
|
40 |
|
41 # simulation parameters |
|
42 st = 1 # desired change in position |
|
43 t_init = 0 # simulation start time |
|
44 t_final = 20 # simulation end time |
|
45 st1 = 0 |
|
46 |
|
47 # continuous controller simulation: g_s_cl3.cos |
|
48 num1 = 0 |
|
49 den1 = 1 |
|
50 |
|
51 # discrete controller simulation: g_s_cl2.cos |
|
52 # u1: -0.1 to 0.8 |
|
53 # y1: 0 to 1.4 |
|
54 C = 0 |
|
55 D = 1 |
|
56 N = 1 |
|
57 gamm = 1 |
|
58 Tc = Sc |