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)