def degrau(G):
y1,t1 = co.step(co.feedback(G,1))
plt.figure()
plt.plot(t1,y1)
plt.title("Step")
plt.xlabel("Tempo[s]")
plt.ylabel("Amplitude")
plt.grid()
print( co.stepinfo(co.feedback(G,1)))
def pid_lti_feedback(set_point, *, Kp, Ki, Kd, Nfilt, plant):
pid = pidtf(Kp, Ki, Kd, Nfilt)
plant = tf([1], [1, 10, 20])
sys = feedback(pid * plant)
return set_point | lti(sys=sys, init_x=0)
def pidplot(num, den, Kp, Ki, Kd, desired_settle=1.):
'''
Plot system step response when open loop system is subjected to feedback PID compensation.
Also plots 2% settling lines, and a vertical line at a desired settling time.
y, t =pidplot(num,den,Kp,Ki,Kd,desired_settle=1.)
Parameters:
:param num: [array-like], coefficients of numerator of open loop transfer function
:param den: [array-like], coefficients of denominator of open loop transfer function
:param Kp: [float] Proportional gain
:param Ki: [float] Integral gain
:param Kd: [float] Derivative gain
:param desired_settle: [float] Desired settling time, for tuning PID controllers, default = 1.0
Returns:
:return: y: [array-like] time step response
:return: t: [array-like] time vector
'''
numc = [Kd, Kp, Ki]
denc = [0, 1, 0]
numcg = convolve(numc, num)
dencg = convolve(denc, den)
Gfb = feedback(tf(numcg, dencg), 1)
y, t = step(Gfb)
yss = dcgain(Gfb)
plot(t, y, 'r')
plot(t, 1.02 * yss * ones(len(t)), 'k--')
plot(t, 0.98 * yss * ones(len(t)), 'k--')
plot(desired_settle * ones(15), linspace(0, yss + 0.25, 15), 'b-.')
xlim(0)
xlabel('Time [s]')
ylabel('Magnitude')
grid()
show()
return y, t
def control(s, Y, D):
end = 10
G = 1 / (3 * s * (s + 1))
F = (3 * s * (s + 1))
KP, KD, KI = 16, 7, 4
H_pd = (KP + KD * s)
H_pid = (KP + KD * s + KI / s)
PD = cm.feedback((KP + KD * s) * G, 1) # PD controller
PID = cm.feedback((KP + KD * s + KI / s) * G, 1) # PID controller
PD_FF = cm.tf([1], [1])
PID_FF = cm.tf([1], [1]) # PID controller
D_PD = (G) / (1 + (KP + KD * s) * (G))
D_PID = (G) / (1 + (KP + KD * s + KI / s) * (G))
# Now, according to the Example, we calculate the response of
# the closed loop system to a step input of size 10:
out_PD, t = cm.step(Y * PD, np.linspace(0, end, 200))
out_PID, t = cm.step(Y * PID, np.linspace(0, end, 200))
out_PD_FF, t = cm.step(Y * PD_FF, np.linspace(0, end, 200))
out_PID_FF, t = cm.step(Y * PID_FF, np.linspace(0, end, 200))
out_PD_D, t = cm.step(-D * D_PD, np.linspace(0, end, 200))
out_PID_D, t = cm.step(-D * D_PID, np.linspace(0, end, 200))
theta_PD = out_PD + out_PD_D
theta_PID = out_PID + out_PID_D
theta_PD_FF = out_PD_FF + out_PD_D
theta_PID_FF = out_PID_FF + out_PID_D
y_out, t = cm.step(Y, np.linspace(0, end, 200))
plt.plot(t, theta_PD, lw=2, label="PD")
plt.plot(t, theta_PID, lw=2, label="PID")
if D != 0:
plt.plot(t, theta_PD_FF, lw=2, label="PD_FF")
plt.plot(t, theta_PID_FF, lw=2, label="PID_FF")
plt.plot(t, y_out, lw=1, label="Reference")
plt.xlabel('Time')
plt.ylabel('Position')
plt.legend()
def testFeedback(self, siso):
"""Call feedback()"""
sys1 = feedback(siso.ss1, siso.ss2)
sys1 = feedback(siso.ss1, siso.tf2)
sys1 = feedback(siso.tf1, siso.ss2)
sys1 = feedback(1, siso.ss2)
sys1 = feedback(1, siso.tf2)
sys1 = feedback(siso.ss1, 1)
sys1 = feedback(siso.tf1, 1)
def rlocfind(sys, desired_zeta, kvectin=None):
'''
Interactive gain selection from the root locus plot
of the SISO system SYS.
rlocfind lets you select a pole location
in the graphics window on the root locus
computed from SYS. The root locus gain associated
with this point is returned as K
and the system poles for this gain are returned as POLES.
:param sys: [transfer function object] transfer function of open loop system
:param desired_zeta: [float] desired damping coefficient value
:param kvectin: [array-like] k vector of values for root locus determination, default = None
Returns:
:return: K: [float] gain at point clicked on root locus
:return: POLES: [array-like] all (complex) pole locations for the gain chosen
'''
rlist, klist, Gdict = root_locus(sys, kvect=kvectin, PrintGain=True)
zetaline(desired_zeta)
show()
K = squeeze(Gdict["k"].real)
POLES = pole(feedback(K * sys, 1))
return K, POLES
################################################################################
'''
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()
################################################################################
# Plot da resp no dominio do tempo ao degrau unitário
################################################################################
T, yout = cnt.step_response(G)
plt.plot(T, 1000 * yout)
k_i = enter("k_i")
if (k_i == None): break
k_d = enter("k_d")
if (k_d == None): break
# ПИД W pid = Kp + Ki/p + Kd*p
w_pid = ml.tf([k_p], [1]) + ml.tf([0, k_i], [1, 0]) + ml.tf([k_d, 0],
[0, 1])
w_pid = ml.tf([k_d, k_p, k_i], [1., 1.])
# ПИ Wpi = Kp + Ki/p
#w_pi = ml.tf([k_p], [1]) + ml.tf([0, k_i], [1, 0])
print("передаточная регулятора", w_pid) #тут менять
w_sum = w_pid * w_sumKnown #тут менять
w = ml.feedback(1, w_sum) # передаточная функция
w = w_sum / (1 + w_sum)
w_zam = w_sumKnown / (1 + w_sumKnown)
y, x = ml.step(w_zam)
plt.plot(x, y, label="переходная системы")
y, x = ml.step(w)
plt.plot(x, y, label="переходная системы c регулированием")
plt.grid(True)
plt.legend()
plt.show()
print("Передаточная функция системы без регулятора: ", w_sumKnown)
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")
import matplotlib.pyplot as plt
from IPython.display import Image
import numpy as np
end = 8
s = cm.tf([1, 0], [1])
A = np.array([[0, 1, 0, 0],
[-1, -1, 1, 0],
[0, 0, 0, 1],
[1, 0, -1, 1]])
b = np.array([[0], [0], [0], [1]])
c = np.array([1, 0, 0, 0])
d = [0]
SS = cm.feedback(cm.ss(A, b, c, d), 1)
TF_conv = cm.ss2tf(SS)
TF = cm.feedback((1) / (s**4 + s**2))
out_SS, t = cm.step(1 * SS, np.linspace(0, end, 200))
out_TF, t = cm.step(1 * TF, np.linspace(0, end, 200))
out_TF_conv, t = cm.step(1 * TF_conv, np.linspace(0, end, 200))
plt.plot(t, out_SS, lw=2, label="ss")
plt.plot(t, out_TF, lw=1, label="tf")
plt.plot(t, out_TF_conv, lw=0.5, label="tf_conv")
plt.legend()
plt.show()
G = 1/(s*(3*s + 3))
KPKD = [(16, 7), (9, 9), (1200, 117)] # The pairs of PD gains
plt.figure(figsize=(10, 8))
for K in KPKD:
KP, KD = K
KI = 4
# To construct the closed-loop transfer function Gcl from theta^d to theta ,
# we have a number of options:
# (1) we can use the command "feedback" as introduced earlier.
######
Gcl = cm.feedback((KP + KD * s) * G, 1) # PD controller
# Gcl = cm.feedback((KP + KD * s + KI/s) * G, 1) # PID controller
print(Gcl)
print(cm.pole(Gcl))
cm.damp(Gcl, True)
######
# (2) If you are interested to know how Eq. (6.18) is derived,
# you may want to use the following general rule:
# +++++++++++++++++++
# Gcl = <Transfer function of the open loop between the input and output>/(1
# + <Transfer function of the closed loop>)
# +++++++++++++++++++
# Accordingly:
######
# -*- coding: utf-8 -*- """ Created on Thu Aug 1 21:31:12 2019 https://qiita.com/hikaruyaku/items/021769a76e72ed34a881 @author: PC """ from control import matlab K = 0.75 # Proprotional Gain Ti = 8 # Integral Time Td = 2 # Derivarive Time # 伝達関数用の係数に変換 kp = K ki = kp/Ti kd = Td * kp num = [kd, kp, ki] den = [1, 0] C = matlab.tf(num, den) print(C) # プロセスモデルを作成する。 # 今回は上記のPの関数と、Pade近似の4次を用いて作成する。 G = P * P4 #feedback loopを作成する sys = matlab.feedback(G*C,1,-1) print(sys)
# exercicio de atraso / PI #G = co.tf(1,[1,3,2]) * co.tf(1,[1,10]) #Gc = 64 ##Gc2 = co.tf(64*np.asarray([1,0.1]),[1,0]) #Gc2 = co.tf(64*10*np.asarray([10,1]),[100,1]) # exercicio de avanço / PD G = co.tf(1,[1,10,24,0]) Gc = 63 Gc2 = co.tf(100*np.asarray([1/3,1]),[1/12,1]) #avanco #Gc2 = co.tf(191*np.asarray([1/6,1]),[1]) #PD T = np.linspace(0,20,3000) co.damp(co.feedback(G*Gc,1)) y,t = co.step(co.feedback(G*Gc,1),T) u,t = co.step(co.feedback(Gc,G),T) co.damp(co.feedback(G*Gc2,1)) y2,t = co.step(co.feedback(G*Gc2,1),T) u2,t = co.step(co.feedback(Gc2,G),T) plt.subplot(2,1,1) plt.plot(t,y,t,y2) plt.grid() plt.legend(['y1','y2']) plt.subplot(2,1,2) plt.plot(t,u,t,u2) plt.grid() plt.legend(['u1','u2'])
#%
#% Funcao de transferencia em malha fechada
#% pode ser definida em Matlab utilizando-se
#% O comando feedback(S1,S2)
#% Onde o sistema S1 se encontra na malha direta
#% e S2 se encontra na malha de realimentacao
#% No caso desse sistema a malha direta
#% e' G(s)H(s) e malha de realimentacao e'
#% unitaria
#%
#% R(s) E(s)|------| |------| U(s)
#%---->(+)---| H(s) |--| G(s) |------->
#% _ ^ |------| |------| |
#% |---------------------------
#%
cloop1 = co.feedback(GHw1, 1)
cloop2 = co.feedback(GHw2, 1)
cloop3 = co.feedback(GHw3, 1)
#%
#% Polos e zeros de malha fechada
#% comando pole() obtem os polos do sistema
#% e zero() obtem os zeros do sistema
#%
print('\nPolos e zeros cloop1')
print(co.pole(cloop1))
print(co.zero(cloop1))
print('\nPolos e zeros cloop2')
print(co.pole(cloop2))
print(co.zero(cloop2))
print('\nPolos e zeros cloop3')
print(co.pole(cloop3))
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")
target.append(0.0)
else:
target.append(10.0)
fig, ax = plt.subplots()
ims = []
kp = 500
ki = 0
kd = 0
for ki in np.arange(0, 500, 100):
num = [kd, kp, ki]
den = [1, 0]
K = matlab.tf(num, den)
sys = matlab.feedback(K * G, 1)
(Y, t, X) = matlab.lsim(sys, target, t)
ax.legend()
ax.set_xlim(0, 20)
ax.set_xlabel('time [sec]')
ax.set_ylabel('velocity [m/s]')
im = ax.plot(t, Y, label='Ki=' + str(ki))
im += ax.plot(t, target, color='r')
ims.append(im)
# ani = animation.ArtistAnimation(fig, ims, interval=100)
# ani.save('kp.gif', writer='pillow')
# (Y, T) = matlab.step(sys, t)
# (Y, t, X) = matlab.lsim(sys, target, t)
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
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()
#
# Funcao de transferencia em malha fechada
# pode ser definida em Matlab utilizando-se
# O comando feedback(S1,S2)
# Onde o sistema S1 se encontra na malha direta
# e S2 se encontra na malha de realimentacao
# No caso desse sistema a malha direta
# e' G(s)H(s) e malha de realimentacao e'
# unitaria
#
# R(s) E(s)|------| |------| U(s)
#---->(+)---| H(s) |--| G(s) |------->
# _ ^ |------| |------| |
# |---------------------------
#
cloop1 = co.feedback(GHp1, 1)
cloop2 = co.feedback(GHp2, 1)
cloop3 = co.feedback(GHp3, 1)
print('\nFUNCAO DE TRANSFERENCIA DE MALHA FECHADA')
print('GHp1/(1+GHp1) = ', cloop1)
print('GHp2/(1+GHp2) = ', cloop2)
print('GHp3/(1+GHp3) = ', cloop3)
#
# Polos e zeros de malha fechada
# comando pole() obtem os polos do sistema
# e zero() obtem os zeros do sistema
#
print('\nPOLOS E ZEROS DE MALHA FECHADA')
print('-------------')
print('Polos e zeros cloop1')
print('Polos = ', co.pole(cloop1))
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)
