|
1 #!/usr/bin/env python |
|
2 # 11.17 |
|
3 |
|
4 import os, sys |
|
5 sys.path += [os.getcwdu() + os.sep + ".." + os.sep + "python"] |
|
6 |
|
7 import pylab as pl |
|
8 from xdync import xdync |
|
9 from zpowk import zpowk |
|
10 from polyfuncs import polmul |
|
11 |
|
12 def gmvc_pid(A, B, k, T, Ts): |
|
13 A = pl.atleast_1d(A) |
|
14 B = pl.atleast_1d(B) |
|
15 dA, dB = len(pl.atleast_1d(A)) - 1, len(pl.atleast_1d(B)) - 1 |
|
16 dT = len(T) - 1 |
|
17 if dA > 2: |
|
18 print 'degree of A cannot be more than 2' |
|
19 exit(1) |
|
20 elif dB>1: |
|
21 print 'degree of B cannot be more than 1' |
|
22 exit(1) |
|
23 delta, ddelta = pl.array([1, -1]), 1 |
|
24 Adelta, dAdelta = polmul(A, dA, delta, ddelta) |
|
25 Q, dQ, P, dP = xdync(Adelta, dAdelta, B, dB, T, dT)[:4] |
|
26 P = pl.atleast_1d(P.squeeze()) |
|
27 PAdelta = P[0]*Adelta |
|
28 |
|
29 zk, dzk = zpowk(k) |
|
30 E, degE, F, degF = xdync(PAdelta, dAdelta, zk, dzk, P, dP)[:4] |
|
31 E = pl.atleast_1d(E.squeeze()) |
|
32 F = pl.atleast_1d(F.squeeze()) |
|
33 nu = P[0]*E[0]*B[0] |
|
34 Kc = -1/nu*(F[1]+2*F[2]) |
|
35 tau_i = -(F[1]+2*F[2])/(F[0]+F[1]+F[2])*Ts |
|
36 tau_d = -F[2]/(F[1]+2*F[2])*Ts |
|
37 L = pl.empty(3) |
|
38 L[0] = 1+Ts/tau_i+tau_d/Ts |
|
39 L[1] = -(1+2*tau_d/Ts) |
|
40 L[2] = tau_d/Ts |
|
41 L = Kc * L.T |
|
42 |
|
43 return Kc, tau_i, tau_d, L |