def pidtf(Kp, Ki=0, Kd=0, N_filt=0):
assert Kp != 0, "Proportional parameter must be greater than zero"
p = prop(Kp)
i = integral(Ki)
d = deriv(Kd, N_filt)
return parallel(p, i, d)
def testParallel(self, siso):
"""Call parallel()"""
sys1 = parallel(siso.ss1, siso.ss2)
sys1 = parallel(siso.ss1, siso.tf2)
sys1 = parallel(siso.tf1, siso.ss2)
sys1 = parallel(1, siso.ss2)
sys1 = parallel(1, siso.tf2)
sys1 = parallel(siso.ss1, 1)
sys1 = parallel(siso.tf1, 1)
def pid_design(self):
n_x = self._A.shape[0] # number of sates
n_u = self._B.shape[1] # number of inputs
n_y = self._C.shape[0] # number of outputs
# Augmented state system (for LQR)
A_lqr = np.block([[self._A, np.zeros((n_x, n_y))],
[self._C, np.zeros((n_y, n_y))]])
B_lqr = np.block([[self._B], [np.zeros((n_y, 1))]])
# Define Q,R
Q = np.diag(self._q)
R = np.array([self._r])
# Solve for P in continuous-time algebraic Riccati equation (CARE)
#print("A_lqr shape", A_lqr.shape)
#print("Q shape",Q.shape)
(P, L, F) = cnt.care(A_lqr, B_lqr, Q, R)
if self._verbose:
print("P matrix", P)
print("Feedback gain", F)
# Calculate Ki_bar, Kp_bar
Kp_bar = np.array([F[0][0:n_x]])
Ki_bar = np.array([F[0][n_x:]])
if self._verbose:
print("Kp_bar", Kp_bar)
print("Ki_bar", Ki_bar)
# Calculate the PID kp kd gains
C_bar = np.block(
[[self._C],
[self._C.dot(self._A) - (self._C.dot(self._B)).dot(Kp_bar)]])
if self._verbose:
print("C_bar", C_bar)
print("C_bar shape", C_bar.shape)
# Calculate PID kp ki gains
kpd = Kp_bar.dot(np.linalg.inv(C_bar))
if self._verbose:
print("kpd: ", kpd, "with shape: ", kpd.shape)
kp = kpd[0][0]
kd = kpd[0][1]
# ki gain
ki = (1. + kd * self._C.dot(self._B)).dot(Ki_bar)
self._K = [kp, ki[0][0], kd]
G_plant = cnt.ss2tf(self._A, self._B, self._C, self._D)
# create PID transfer function
d_tc = 1. / 125. # Derivative time constant at nyquist freq
p_tf = self._K[0]
i_tf = self._K[1] * cnt.tf([1], [1, 0])
d_tf = self._K[2] * cnt.tf([1, 0], [d_tc, 1])
pid_tf = cnt.parallel(p_tf, i_tf, d_tf)
open_loop_tf = cnt.series(pid_tf, G_plant)
closedLoop_tf = cnt.feedback(open_loop_tf, 1)
if self._verbose:
print(" *********** PID gains ***********")
print("kp: ", self._K[0])
print("ki: ", self._K[1])
print("kd: ", self._K[2])
print(" *********************************")
return pid_tf, closedLoop_tf
def integral_find(f, name):
Q = integrate.quad(f, 0, math.inf)
print("Интегральная функция {0} = {1} ".format(name, Q))
return Q
W1 = n_w(1, [Tg, 1], "Генератора")
W2 = n_w([0.01 * Tgm, 1], [0.05 * Tg, 1], "Гидравлической турбины")
W3 = n_w(ky, [Ty, 1], "Исполнительного устройства")
# Wk = n_w([k, 0] , [0], "Wk ")
# Wd = n_w ([diff()])
Wkpid = ctr.tf([kpid, 0], [0, 1])
Wtdpid = ctr.tf([Tdpid, 0], [0, 1])
Wtipid = ctr.tf([0, Tipid], [1, 0])
# print("Wtipid = {} \n".format(Wtipid))
Wpid = cm.parallel(Wkpid, Wtdpid, Wtipid)
Wwpid = cm.series(W1, W2, W3, Wpid)
Wwpido = cm.feedback(Wwpid, 1)
Wkpi = ctr.tf([kpi, 0], [0, 1])
Wtipi = ctr.tf([0, Tipi], [1, 0])
Wpi = cm.parallel(Wkpi, Wtipi)
W = cm.series(W1, W2, W3)
Wwpi = cm.series(W, Wpi)
Wwpio = cm.feedback(Wwpi, 1)
Wf = cm.feedback(W, 1)
print("W = \n {}".format(Wf))
print("Wwpid = \n")
print(Wwpido)
print("Wwpi = \n {}".format(Wwpio))
grafic_stepw(Wf, "W")
grafic_step(Wwpido, "PID")