def testSeries(self, siso):
"""Call series()"""
sys1 = series(siso.ss1, siso.ss2)
sys1 = series(siso.ss1, siso.tf2)
sys1 = series(siso.tf1, siso.ss2)
sys1 = series(1, siso.ss2)
sys1 = series(1, siso.tf2)
sys1 = series(siso.ss1, 1)
sys1 = series(siso.tf1, 1)
import numpy as np
from control import matlab
from matplotlib import pyplot as plt
from matplotlib import animation
m = 1000
c = 100
G = matlab.tf([1], [m, c])
t_delay = 0.5
n_pade = 10
(num_pade, den_pade) = matlab.pade(t_delay, n_pade)
G_delay = matlab.tf(num_pade, den_pade)
G = matlab.series(G_delay, G)
# Kp = 2000
# Ki = 0
# Kd = 0
# num = [Kd, Kp, Ki]
# den = [1, 0]
# K = matlab.tf(num, den)
#
# sys = matlab.feedback(K*G, 1)
t = np.linspace(0, 20, 2000)
target = []
for i in t:
if i < 2.0:
target.append(0.0)
else:
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
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")
grafic_step(Wwpio, "PI")
G = ctl.ss2tf(Sp)
print('G =', G)
'''
################################################################################
Input (Funcao de transferencia Gpid)
################################################################################
'''
num = np.array([1, 15000.1, 1500])
den = np.array([0, 1, 0])
Gpid = ctl.tf(num, den)
print('Gpid:', Gpid)
################################################################################
# Em serie (G*Gpid)
################################################################################
S = ctl.series(G, Gpid)
################################################################################
# Com feedback
################################################################################
F = ctl.feedback(S, 1, -1)
print('Resultado malha fechada:', F)
################################################################################
# Polos e zeros
################################################################################
# (p, z) = ctl.pzmap(G)
# print('polos =', p)
# print('zeros =', z)
# plt.show()
Gca = ctl.tf([2], [1])
Gp = ctl.tf([-10], [1, 10]) # Servo do Profundor
Gma = ctl.tf([-1, -5], [1, 3.5, 6, 0]) # sistema da aero nave
print("------------------------------------------")
print("G(s) = ", Gca)
print("servo profunor = ", Gp)
print("Modelo Aeronave = ", Gma)
print("------------------------------------------")
t = np.arange(0, 15, 1)
y = 0.5 * t
# print(t)
# print("y(t) = ", y)
Ls = mat.series(Gca, Gp, Gma)
print("L(s) = ", Ls)
Hss = ctl.tf([1], [1])
print("H(s) = ", Hss)
Ts = ctl.feedback(Ls, Hss)
print("T(s) = ", Ts)
yout, T, Xo = mat.lsim(Ts, y, t, 0)
# print(T)
plt.plot(T, yout, '-b')
plt.plot(t, y, '-r')
plt.grid()
plt.show()
from control.matlab import tf, feedback, series S1 = tf(1, [1, 1]) S2 = tf(1, [1, 2]) S3 = tf([3, 1], [1, 0]) S4 = tf([2, 0], 1) S12 = feedback(S1, S2) S123 = series(S12, S3) S = feedback(S123, S4) print(S)
import control.matlab as ctl
import numpy as np
import matplotlib.pyplot as plt
Gca = ctl.tf([2, 1], [1, 0])
Gp = ctl.tf([-10], [1, 10]) # Servo do Profundor
Gma = ctl.tf([-1, -5], [1, 3.5, 6, 0]) # sistema da aero nave
t = np.arange(0, 17, 1)
y = 0.5 * t
Ls = ctl.series(Gca, Gp, Gma)
print("L(s) = ", Ls)
Hss = ctl.tf([1], [1])
print("H(s) = ", Hss)
Ts = ctl.feedback(Ls, Hss, sign=-1)
print("T(s) = ", Ts)
yout, T, Xo = ctl.lsim(Ts, y, t, 0)
plt.plot(T, yout, '-k')
plt.plot(t, y, '-r')
plt.grid()
plt.show()
import control as con
import control.matlab as ctl
import numpy as np
import matplotlib.pyplot as plt
controlador = ctl.tf([0.1, 5], [1, 0])
missilDinamica = ctl.tf([100, 100], [1, 2, 100])
print("Controlador = ", controlador)
print("Dinâmica do Missil = ", missilDinamica)
Ls = ctl.series(controlador, missilDinamica)
print("L(s) = ", Ls)
Hss = ctl.tf([1], [1])
Ts = ctl.feedback(Ls, Hss, sign=-1)
print("T(s) = ", Ts)
yout, T = ctl.step(Ts, 3, 0)
info = con.step_info(Ts, 3)
print(info)
plt.plot(T, yout, '-k')
plt.show()
import control as con
import control.matlab as ctl
import numpy as np
k = np.arange(1, 5.1, 0.1, dtype=float)
Gc = ctl.tf([1, -2, 4], [1, 4, 2])
Hss = ctl.tf([1], [1])
for x in k:
Ts = ctl.feedback(ctl.series(x, Gc), Hss, sign=-1)
if ctl.pole(Ts)[0].real < 0:
print("------- Sistema estável ------")
print(f"Polos K = {round(x, 2)}", ctl.pole(Ts), " Sistema estável")