0
|
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
|