def dead_time_bound(L, Gd, deadtime, freq = np.arange(0.001, 1,0.001)): """ Parameters: L => the loop transfer function Gd => Disturbance transfer function deadtime => the deadtime in seconds of the system Notes: If the cross over frequencies are very large or very small you will have to find them yourself. """ mag, phase, omega = cn.bode_plot(L, omega=freq) mag_d, phase_d, omega_d = cn.bode_plot(Gd, omega=freq) plt.clf() gm, pm, wg, wp_L = cn.margin(mag, phase, omega) gm, pm, wg, wp_Gd = cn.margin(mag_d, phase_d, omega_d) freq_lim = [freq[x] for x in range(len(freq)) if 0.1 < mag[x] < 10] mag_lim = [mag[x] for x in range(len(freq)) if 0.1 < mag[x] < 10] plt.loglog(freq_lim, mag_lim, color="blue", label="|L|") dead_w = 1.0/deadtime ymin, ymax = plt.ylim() plt.loglog([dead_w, dead_w], [ymin, ymax], color="red", ls = "--", label = "dead time frequency") plt.loglog([wp_L, wp_L], [ymin, ymax], color="green", ls =":", label = "w_c") plt.loglog([wp_Gd, wp_Gd], [ymin, ymax], color="black", ls = "--", label = " w_d") print("You require feedback for disturbance rejection up to (w_d) = " + str(wp_Gd) + "\n Remember than w_B < w_c < W_BT and w_d < w_B hence w_d < w_c.") print("The upper bound on w_c based on the dead time\ (wc < w_dead = 1/dead_seconds) = " + str(1.0/deadtime)) plt.legend(loc=3)
def plot_loops(name, G_ol, G_cl): # type: (str, control.tf, control.tf) -> None """ Plot loops :param name: Name of axis :param G_ol: open loop transfer function :param G_cl: closed loop transfer function """ plt.figure() plt.plot(*control.step_response(G_cl, np.linspace(0, 1, 1000))) plt.title(name + ' step response') plt.grid() plt.figure() control.bode(G_ol) print('margins', control.margin(G_ol)) plt.subplot(211) plt.title(name + ' open loop bode plot') plt.figure() control.rlocus(G_ol, np.logspace(-2, 0, 1000)) for pole in G_cl.pole(): plt.plot(np.real(pole), np.imag(pole), 'rs') plt.title(name + ' root locus') plt.grid()
def plot_margins(sys): mag, phase, omega = ctl.bode(sys, dB=True, Plot=False) magdB = 20 * np.log10(mag) phase_deg = phase * 180.0 / np.pi Gm, Pm, Wcg, Wcp = ctl.margin(sys) GmdB = 20 * np.log10(Gm) ##Plot Gain and Phase f, (ax1, ax2) = plt.subplots(2, 1) ax1.semilogx(omega, magdB) ax1.grid(which="both") ax1.set_xlabel('Frequency (rad/s)') ax1.set_ylabel('Magnitude (dB)') ax2.semilogx(omega, phase_deg) ax2.grid(which="both") ax2.set_xlabel('Frequency (rad/s)') ax2.set_ylabel('Phase (deg)') ax1.set_title('Gm = ' + str(np.round(GmdB, 2)) + ' dB (at ' + str(np.round(Wcg, 2)) + ' rad/s), Pm = ' + str(np.round(Pm, 2)) + ' deg (at ' + str(np.round(Wcp, 2)) + ' rad/s)') ###Plot the zero dB line ax1.plot(omega, 0 * omega, 'k--', lineWidth=2) ###Plot the -180 deg lin ax2.plot(omega, -180 + 0 * omega, 'k--', lineWidth=2) ##Plot the vertical line from -180 to 0 at Wcg ax2.plot([Wcg, Wcg], [-180, 0], 'r--', lineWidth=2) ##Plot the vertical line from -180+Pm to 0 at Wcp ax2.plot([Wcp, Wcp], [-180, -180 + Pm], 'g--', lineWidth=2) ##Plot the vertical line from min(magdB) to 0-GmdB at Wcg ax1.plot([Wcg, Wcg], [-GmdB, 0], 'r--', lineWidth=2) ##Plot the vertical line from min(magdB) to 0db at Wcp ax1.plot([Wcp, Wcp], [np.min(magdB), 0], 'g--', lineWidth=2) return Gm, Pm, Wcg, Wcp
def Bode_Margin(sys, omega): w = omega mag, phaserad, w = ctrl.bode(sys, w) magdb = 20 * log(mag) phasedeg = phaserad * 180 / pi gm, pm, wg, wp = ctrl.margin(sys) a1 = f.add_subplot(221) a1.set_xscale("log") a1.plot(w, magdb) a1.plot(wp, 0, '.y') a1.text(wp, 0 + 25, "wc=%.2f" % wp) # a1.title = "Bode" # a1.ylabel = "Magnitude dB " # a1.xlabel = "Angular frequency" a2 = f.add_subplot(223) a2.set_xscale("log") a2.plot(w, phasedeg) a2.plot(wp, pm - 180, '.y') a2.text(wp, pm - 180 + 25, "phase=%.2f" % (pm - 180)) # a2.ylabel = "Phase (degree) " # a2.xlabel = "Angular frequency" a3 = f.add_subplot(122) sysF = sys / (1 + sys) T = np.arange(0, 30, 0.5) T, yout = ctrl.step_response(sysF, T) a3.plot(T, yout) # a3.title = "step response" # a3.xlabel = "time" # a3.ylabel = "Magnitude" return wp, pm
def zpk(z, p, k): Gs = control.tf(36, [1, 3.6, 0]) # Define Compensator transfer function num, den = matlab.zpk2tf(z, p, k) Ds = matlab.tf(num, den) print(Ds) # Draw the open-loop frequency resp. for the comp. sys DsGs = Ds*Gs gm, pm, wg, wp = control.margin(DsGs) print(f"Gain margin = {gm} dB") print(f"Phase margin = {round(pm, 2)} degrees") print(f"Frequency for Gain Margin = {wg} radians/sec") print(f"Frequecny for Phase Margin = {wp} radians/sec") omega_comp = np.logspace(-2,2,2000) mag_comp, phase_comp, omega_comp = control.bode(DsGs, omega=omega_comp) mag_comp = 20 * np.log10(mag_comp) phase_comp = np.degrees(phase_comp) omega_comp = omega_comp.T phase_comp = phase_comp.T mag_comp = mag_comp.T omega_comp = list(omega_comp) phase_comp = list(phase_comp) mag_comp = list(mag_comp) return omega_comp, mag_comp, phase_comp, gm, pm, wp, wg
def margin_zpk(sys, z, p, k): # open-loop system transfer function try: num, den = model(sys) except: # for error detection print("Err: system in not defined") return Gs = control.tf(num, den) # define compensator transfer function # convert zero and pole to numpy arrays z = np.array([z]) p = np.array([p]) num, den = matlab.zpk2tf(z, p, k) Ds = matlab.tf(num, den) # Compensated open-loop transfer function DsGs = Ds*Gs # phase & gain margins gm, pm, wg, wp = control.margin(DsGs) # round phase & gain margin variables ndigits = 2 gm = round(gm, ndigits) pm = round(pm, ndigits) wg = round(wg, ndigits) wp = round(wp, ndigits) return gm, pm, wg, wp
def pid(Kp, Ki, Kd): # PID Controller s = matlab.tf('s') Ds = Kp + Ki/s + Kd*s # Draw the open-loop frequency resp. for the comp. sys Gs = control.tf(36, [1, 3.6, 0]) DsGs = Ds*Gs gm, pm, wg, wp = control.margin(DsGs) print(f"Gain margin = {gm} dB") print(f"Phase margin = {round(pm, 2)} degrees") print(f"Frequency for Gain Margin = {wg} radians/sec") print(f"Frequecny for Phase Margin = {wp} radians/sec") omega_comp = np.logspace(-2,2,2000) mag_comp, phase_comp, omega_comp = control.bode(DsGs, omega=omega_comp) mag_comp = 20 * np.log10(mag_comp) phase_comp = np.degrees(phase_comp) omega_comp = omega_comp.T phase_comp = phase_comp.T mag_comp = mag_comp.T omega_comp = list(omega_comp) phase_comp = list(phase_comp) mag_comp = list(mag_comp) return omega_comp, mag_comp, phase_comp, gm, pm, wp, wg
def margin_pid(sys, Kp, Ki, Kd): # open-loop system transfer function try: num, den = model(sys) except: # for error detection print("Err: system in not defined") return Gs = control.tf(num, den) # PID Controller s = matlab.tf('s') Ds = Kp + Ki/s + Kd*s # Compensated open-loop transfer function DsGs = Ds*Gs # phase & gain margins gm, pm, wg, wp = control.margin(DsGs) # round phase & gain margin variables ndigits = 2 gm = round(gm, ndigits) pm = round(pm, ndigits) wg = round(wg, ndigits) wp = round(wp, ndigits) return gm, pm, wg, wp
def fitness_calc(self, Gp, Time, Input): # Compute location of zeros (self.Z1, self.Z2) = np.roots([1, 2 * self.DP1 * self.WN1, self.WN1**2]) (self.Z3, self.Z4) = np.roots([1, 2 * self.DP2 * self.WN2, self.WN2**2]) # Create PI controller (self.Gc_num, self.Gc_den) = zpk2tf( [self.Z1, self.Z2, self.Z3, self.Z4], [0], self.K) # Controller with one pole in origin and 2 pair of zeros self.Gc = ctrl.tf(self.Gc_num, self.Gc_den) # Evaluate closed loop stability self.gm, self.pm, self.Wcg, self.Wcp = ctrl.margin(self.Gc * Gp) # Dischard solutions with no gain margin if self.gm == None or self.gm <= 1: self.fitness = 999 return # Closed loop system self.M = ctrl.feedback(self.Gc * Gp, 1) # Closed loop step response self.y, self.t, self.xout = ctrl.lsim(self.M, Input, Time) # Evaluate fitness self.fitness = evaluate(Input, self.y)
def stability_analysis(self): """ Perform a stability analysis of the loop with settings defined in the constructor """ # create the phase detector transfer function K_pd = control.tf([1], [1]) # create the low pass filter transfer function iir_taps = self.iir.read_coeffs() iir_b = iir_taps[0:2] iir_a = iir_taps[3:5] K_lpf = control.tf(iir_b, iir_a) # create the PI filter transfer function K_pi = control.tf([self.proportional_gain, self.integral_gain], [1, 0]) # create the VCO transfer function K_vco = control.tf([1], [1, 0]) # create the final transfer function K_ol = K_pd * K_lpf * K_pi * K_vco # find the bode plot mag, phase, omega = control.bode(K_ol) # plot the bode plot plt.plot(omega / (2 * np.pi), mag) plt.plot(omega / (2 * np.pi), phase) plt.show() # find the gain and phase margins gm, pm, wg, wp = control.margin(K_ol) print("Gain margin = ", gm, "Phase margin = ", pm) print(K_ol)
def Gs(): # Define plant transfer function Gs = control.tf(36, [1, 3.6, 0]) [n, d] = control.tfdata(Gs) b = str(control.tf(d,1 )) print(b[5]) num = "36" den = "s^2 + 3.6s" # Draw open-loop frequency response for the planet gm, pm, wg, wp = control.margin(Gs) # print(f"Gain margin = {gm} dB") # print(f"Phase margin = {round(pm, 2)} degrees") # print(f"Frequency for Gain Margin = {wg} radians/sec") # print(f"Frequecny for Phase Margin = {wp} radians/sec") omega = np.logspace(-2,2,2000) mag, phase, omega = control.bode(Gs, omega=omega) mag = 20 * np.log10(mag) phase= np.degrees(phase) omega = omega.T phase = phase.T mag = mag.T omega= list(omega) phase= list(phase) mag= list(mag) #mag = 20*np.log(mag,10) return num, den, omega, mag, phase,gm, pm, wg, wp
def dead_time_bound(L, Gd, deadtime, freq=np.arange(0.001, 1, 0.001)): """ Parameters: L => the loop transfer function Gd => Disturbance transfer function deadtime => the deadtime in seconds of the system Notes: If the cross over frequencies are very large or very small you will have to find them yourself. """ mag, phase, omega = cn.bode_plot(L, omega=freq) mag_d, phase_d, omega_d = cn.bode_plot(Gd, omega=freq) plt.clf() gm, pm, wg, wp_L = cn.margin(mag, phase, omega) gm, pm, wg, wp_Gd = cn.margin(mag_d, phase_d, omega_d) freq_lim = [ freq[x] for x in range(len(freq)) if mag[x] > 0.1 and mag[x] < 10 ] mag_lim = [ mag[x] for x in range(len(freq)) if mag[x] > 0.1 and mag[x] < 10 ] plt.loglog(freq_lim, mag_lim, color="blue", label="|L|") dead_w = 1.0 / deadtime ymin, ymax = plt.ylim() plt.loglog([dead_w, dead_w], [ymin, ymax], color="red", ls="--", label="dead time frequency") plt.loglog([wp_L, wp_L], [ymin, ymax], color="green", ls=":", label="w_c") plt.loglog([wp_Gd, wp_Gd], [ymin, ymax], color="black", ls="--", label=" w_d") print("You require feedback for disturbance rejection up to (w_d) = " + str(wp_Gd) + "\n Remember than w_B < w_c < W_BT and w_d < w_B hence w_d < w_c.") print "The upper bound on w_c based on the dead time\ (wc < w_dead = 1/dead_seconds) = " + str(1.0 / deadtime) plt.legend(loc=3)
def bode (tf, plot = False): print "=================================================================="; gm, pm, wg, wp = ctrl.margin (tf); print "Gain Margin: ", gm, " dB in ", wg, " rad/s"; #Verificar informacoes print "Phase Margin: ", gm, "deg in", wp, " rad/s"; mag, pha, w = ctrl.bode_plot (tf); if (plot == True): p = Plotter ({'type' : 'log', 'grid' : True}); p.subplot ([(w, 20*np.log10(mag)), (w, (180*pha/np.pi))], ["Gain (dB)", "Phase (deg)"]); return gm, pm, wg, wp
def urCheck(sys,wctar,pmtar,omega): thres=0.5 R.urOffset = 1 gm, pm, wg, wp = ctrl.margin(sys) mag, phaserad, w = ctrl.bode(sys, omega) phasedeg = phaserad * 180 / pi if pm < pmtar: for i in range(len(omega)): if phasedeg[i] < pmtar-180+thres and phasedeg[i] > pmtar-180-thres and mag[i]>1: R.urOffset=mag[i]**-1 break return sys * R.urOffset
def draw_lines_bode(TF): gm,pm,x1,x2 = control.margin(TF) plt.subplot(2,1,1) xmin, xmax = plt.xlim() plt.plot([xmin, xmax],[0,0],'g') ymin, ymax = plt.ylim() plt.plot([x1,x1],[ymin, ymax],'r') plt.subplot(2,1,2) xmin, xmax = plt.xlim() plt.plot([xmin, xmax],[-180,-180],'g') ymin, ymax = plt.ylim() plt.plot([x1,x1],[ymin, ymax],'r') fig = plt.gcf() fig.set_size_inches(9,9)
def frequency_requirements(g, gain_margin=None, phase_margin=None): gains = [] gm, pm, _, _ = ct.margin(g) if gain_margin is not None: gains.append(10**(-(gain_margin - gm) / 20)) if phase_margin is not None: mag, phase, omega = ct.bode(g, Plot=False) arg = np.argmin(abs(phase - (phase_margin - np.pi))) m = 20 * np.log10(mag[arg]) gains.append(10**(-m / 20)) return np.prod(gains)
def draw_lines_bode(TF): gm, pm, x1, x2 = control.margin(TF) plt.subplot(2, 1, 1) xmin, xmax = plt.xlim() plt.plot([xmin, xmax], [0, 0], 'g') ymin, ymax = plt.ylim() plt.plot([x1, x1], [ymin, ymax], 'r') plt.subplot(2, 1, 2) xmin, xmax = plt.xlim() plt.plot([xmin, xmax], [-180, -180], 'g') ymin, ymax = plt.ylim() plt.plot([x1, x1], [ymin, ymax], 'r') fig = plt.gcf() fig.set_size_inches(9, 9)
def __init__(self, Gp, Time, Input): while (True): # Try random parameter values self.DP1 = np.random.uniform(0, DAMPING_MAX) self.WN1 = np.random.uniform(0, WN_MAX) self.DP2 = np.random.uniform(0, DAMPING_MAX) self.WN2 = np.random.uniform(0, WN_MAX) # Compute location of zeros (self.Z1, self.Z2) = np.roots([1, 2 * self.DP1 * self.WN1, self.WN1**2]) (self.Z3, self.Z4) = np.roots([1, 2 * self.DP2 * self.WN2, self.WN2**2]) # Create test PI controller (used to calculate the maximum K for closed loop stable) (self.Gc_num, self.Gc_den) = zpk2tf( [self.Z1, self.Z2, self.Z3, self.Z4], [0], 1) # Controller with one pole in origin and 2 pair of zeros self.Gc = ctrl.tf(self.Gc_num, self.Gc_den) # Evaluate closed loop stability self.gm, self.pm, self.Wcg, self.Wcp = ctrl.margin(self.Gc * Gp) # Dischard solutions with no gain margin if self.Wcg == None or (self.Wcp != None and self.Wcg >= self.Wcp): continue if self.gm == None: # None = inf self.gm = K_MAX # If K < gm => closed loop stable (gm > 0dB) self.K = np.random.uniform(0, self.gm) # Create PI controller for closed loop stable system (self.Gc_num, self.Gc_den) = zpk2tf( [self.Z1, self.Z2, self.Z3, self.Z4], [0], self.K ) # Controller with one pole in origin and 2 pair of zeros self.Gc = ctrl.tf(self.Gc_num, self.Gc_den) # Closed loop system self.M = ctrl.feedback(self.Gc * Gp, 1) # Closed loop step response self.y, self.t, self.xout = ctrl.lsim(self.M, Input, Time) # Evaluate fitness self.fitness = evaluate(Input, self.y) break
def draw_lines_nyquist(TF): plt.hold("True") gm,pm,x1,x2 = control.margin(TF) plt.plot([0,cos(pi - pm/180*pi)],[0,sin(pi - pm/180*pi)],'m') t = np.linspace(pi - pm/180*pi,pi,1000) x = 0.5 * np.cos(t) y = 0.5 * np.sin(t) plt.plot(x,y,'m') plt.hold(True) plt.axhline(0,0,1) plt.axvline(0,0,1) plt.plot(np.cos(np.linspace(0,2*pi,1000)),np.sin(np.linspace(0,2*pi,1000)),'r') fig= plt.gcf() fig.set_size_inches(9,9) plt.axis('equal')
def plot_attitude_rate_design(name, G_ol, G_cl): import matplotlib.pyplot as plt plt.figure() plt.plot(*control.step_response(G_cl, np.linspace(0, 1, 1000))) plt.title(name + ' rate step resposne') plt.figure() control.bode(G_ol) print(control.margin(G_ol)) plt.figure() control.rlocus(G_ol, np.logspace(-2, 0, 1000)) for pole in G_cl.pole(): plt.plot(np.real(pole), np.imag(pole), 'rs') plt.title(name + ' rate step root locus')
def draw_lines_nyquist(TF): plt.hold("True") gm, pm, x1, x2 = control.margin(TF) plt.plot([0, cos(pi - pm / 180 * pi)], [0, sin(pi - pm / 180 * pi)], 'm') t = np.linspace(pi - pm / 180 * pi, pi, 1000) x = 0.5 * np.cos(t) y = 0.5 * np.sin(t) plt.plot(x, y, 'm') plt.hold(True) plt.axhline(0, 0, 1) plt.axvline(0, 0, 1) plt.plot(np.cos(np.linspace(0, 2 * pi, 1000)), np.sin(np.linspace(0, 2 * pi, 1000)), 'r') fig = plt.gcf() fig.set_size_inches(9, 9) plt.axis('equal')
def margin_cal(cont_frd, plant, freq): # open loop ol_frd = cont_frd * plant gm1, ph1, wg1, wp1 = ct.margin(ol_frd) if (gm1 > 100) or (ph1 > 150) or (ph1 < 1) or (wp1 is None): return None ol_rsp = ol_frd.freqresp(freq) ol_mag = list(ol_rsp[0][0][0]) # error transfer function etf_frd = 1/(1 + ol_frd) etf_rsp = etf_frd.freqresp(freq) etf_mag = list(etf_rsp[0][0][0]) return Cost.CostData(gm1, ph1, wg1, wp1, ol_mag, etf_mag)
def plot_slope(sys, *args, **dict): """ Plots the slope (in dB) of the transfer function parameter (from a bode diagram). It also plots the position of the cross over frequency as a black vertical line (for slope comparisons). Currently works for SISO only. Parameter: sys => a transfer function object *args, **dict => the usual plotting parameters Returns: a matplotlib figure containing the slope as a function of frequency Notes: This function assumes you input the loop transfer function (L(s)). As such it will show you where the cross over frequency is so that you may compare slopes against it. """ mag, phase, omega = cn.bode_plot(sys) # Get Bode information plt.clf() # Clear the previous Bode plot from the figure end = len(mag) slope = [] freqs = [] for x in range(end - 1): # Calculate the slope incrementally slope.append((np.log(mag[x + 1]) - np.log(mag[x])) / (np.log(omega[x + 1]) - np.log(omega[x]))) freqs.append((omega[x + 1] + omega[x]) / 2) # w = cross_over_freq(sys) # Something is throwing an error but this returns just wp gm, pm, wg, wp = cn.margin(sys) length = len(slope) cross_freqs = [wp] * length plt.plot(cross_freqs, slope, color="black", linewidth=3.0) plt.plot(freqs, slope, *args, **dict) plt.plot() plt.xscale('log') plt.xlabel("Frequency") plt.ylabel("Logarithmic Slope") plt.grid() current_fig = plt.gcf() return current_fig
def plot_slope(sys, *args, **dict): """ Plots the slope (in dB) of the transfer function parameter (from a bode diagram). It also plots the position of the cross over frequency as a black vertical line (for slope comparisons). Currently works for SISO only. Parameter: sys => a transfer function object *args, **dict => the usual plotting parameters Returns: a matplotlib figure contraining the slope as a function of frequency Notes: This function assumes you input the loop transfer function (L(s)). As such it will show you where the cross over frequency is so that you may compare slopes against it. """ mag, phase, omega = cn.bode_plot(sys) # Get Bode information plt.clf() # Clear the previous Bode plot from the figure end = len(mag) slope = [] freqs = [] for x in range(end - 1): # Calculate the slope incrementally slope.append((np.log(mag[x + 1]) - np.log(mag[x])) / (np.log(omega[x + 1]) - np.log(omega[x]))) freqs.append((omega[x + 1] + omega[x]) / 2) # w = cross_over_freq(sys) # Something is throwing an error but this returns just wp gm, pm, wg, wp = cn.margin(sys) length = len(slope) cross_freqs = [wp] * length plt.plot(cross_freqs, slope, color="black", linewidth=3.0) plt.plot(freqs, slope, *args, **dict) plt.plot() plt.xscale('log') plt.xlabel("Frequency") plt.ylabel("Logarithmic Slope") plt.grid() current_fig = plt.gcf() return current_fig
def lead_compensator(g, err_step=None, err_ramp=None, err_para=None, pm_desired=None, psi=None): s = ct.TransferFunction([1, 0], [1]) # all radians policy if psi is not None: pm_desired = 100 * psi * np.pi / 180 if pm_desired > 2 * np.pi: print("remember, I need phase in radians") kc, ess = ss_error(g, err_step, err_ramp, err_para) if abs(ess) == np.inf or abs(ess) == np.nan: ess = None if ess is None or kc is None: assert "Inconsistent system" kc = kc / np.real(ess) print(f"kc= {kc}") _, pm, _, wpm = ct.margin(kc * g) print(pm, wpm) k_angle = (pm_desired - pm * np.pi / 180 + 5 * np.pi / 180) # 5deg extra alpha = (1 + np.sin(k_angle)) / (1 - np.sin(k_angle)) print(f"alpha= {alpha}") a = 10 * np.log10(alpha) print(f"A= {a}") mag, phase, omega = ct.bode(kc * g, Plot=False) arg = np.argmin(abs(20 * np.log10(mag) - -a)) wcg = omega[arg] tau = 1 / np.sqrt(alpha) / wcg print(f"tau= {tau}") lead = kc * (alpha * tau * s + 1) / (tau * s + 1) return lead
def advise(f_res,Q): #f_res=resonant frequency #Q=quality factor tc=Q/(np.pi*f_res) LTI=control.tf([-360*tc],[tc, 1]) gm,pm,wg,wp = control.margin(LTI) kp=abs(0.45*gm) if wg!=0: Ti=(2*np.pi/wg)/1.2 ki=kp/Ti else: Ti=0 ki=0 return kp,ki,Ti
def margin_sys(sys): # open-loop system transfer function try: num, den = model(sys) except: # for error detection print("Err: system in not defined") return Gs = control.tf(num, den) # phase & gain margins gm, pm, wg, wp = control.margin(Gs) # round phase & gain margin variables ndigits = 2 gm = round(gm, ndigits) pm = round(pm, ndigits) wg = round(wg, ndigits) wp = round(wp, ndigits) return gm, pm, wg, wp
def info_bode(self): tf = self.tf if self.loop_type == 'ol' else self.tf_plant * self.tf_comp try: gm, pm, wg, wp = control.margin(tf) gm = 20 * np.log10(gm) if np.isinf(gm): g_txt = gm else: g_txt = '{:.2f} dB (at {:.2f} rad/s)'.format(gm, wg) if np.isinf(pm): p_txt = pm else: p_txt = '{:.2f} deg (at {:.2f} rad/s)'.format(pm, wp) res = 'GM : {} | PM : {} '.format(g_txt, p_txt) return res except: # Temporary fix for numpy error 'Eigenvalues did not converge' return 'Error. Plot could not be updated.'
import numpy as np import matplotlib.pyplot as plt import control from scipy import signal #if using termux import subprocess import shlex #end if k=96 num=[k] den=[1, 12, 44, 48, 0] # num=[40] # den=[1, 1, 0] sys=control.tf(num,den) gm, pm, wg, wp = control.margin(sys) print ("phase margin:",pm) print ("gain crossover frequency:",wp ) s = signal.lti(num,den) w, mag, phase =signal.bode(s) gain_y=np.full((len(w)),-4.81) phase_y=np.full((len(w)),-180) plt.subplot(2,1,1) plt.semilogx(w, mag) plt.plot(w,gain_y) plt.plot(2.06, -4.81 ,'ro') #plt.text(wp ,0,'({}, {})'.format(wp, 0)) plt.ylabel("Gain")
######################################### # Control Design ######################################### Control = tf([1], [1]) # -----proportional control: change cross over frequency---- kp = 30 Control = kp * Control if PLOT: mag, phase, omega = cnt.bode(Plant * Control, dB=True, Plot=False) plt.subplot(211) openMagPlot, = plt.semilogx(omega, mag, color='r', label='OPEN') plt.subplot(212) openPhasePlot, = plt.semilogx(omega, phase, color='r', label='OPEN') # Calculate the phase and gain margin gm, pm, Wcg, Wcp = cnt.margin(Plant * Control) print("pm: ", pm, " gm: ", gm, " Wcp: ", Wcp, " Wcg: ", Wcg) # -----low pass filter: decrease gain at high frequency (noise)---- p = 10.0 LPF = tf([p], [1, p]) Control = Control * LPF if PLOT: mag, phase, omega = cnt.bode(Plant * Control, dB=True, Plot=False) plt.subplot(211) openMagPlot, = plt.semilogx(omega, mag, color='r', label='OPEN') plt.subplot(212) openPhasePlot, = plt.semilogx(omega, phase, color='r', label='OPEN') # Calculate the phase and gain margin gm, pm, Wcg, Wcp = cnt.margin(Plant * Control) print("pm: ", pm, " gm: ", gm, " Wcp: ", Wcp, " Wcg: ", Wcg)
import matplotlib.pyplot as plt from matplotlib.pyplot import * import control #if using termux #import subprocess #import shlex #end if Num =[25, 0.925] Den =[12.3, 73.8, 62, 0.2, 0] s1 = signal.lti(Num,Den) w, mag, phase = signal.bode(s1) sys = control.tf([25, 0.925], [12.3, 73.8, 62, 0.2, 0] ) gm, pm, wg, wp = control.margin(sys) #These are the gain margin(not in dB),phase margin,gain cross over frequency and phase crossover frequency fig=subplot(2,1,1) plt.semilogx(w, mag) plt.axhline(y = 0,xmin=0,xmax= 4.25226694) plt.axvline(x = 0.379,ymin=0,color='g',linestyle='dashed') plt.text(0.379,0,'(0.379,0)') plt.xlabel('Magnitude Plot') plt.grid() fig=subplot(2,1,2) plt.semilogx(w, phase) plt.axhline(y = -180,xmin=0) plt.axvline(x = 0.379,ymin=0,color ='g',linestyle='dashed') plt.text(0.379,-180,'(0.379,-180)')
C_out = tf([ P10.kd_z + P10.kp_z * P10.sigma, P10.kp_z + P10.ki_z * P10.sigma, P10.ki_z ], [P10.sigma, 1, 0]) # Plot the closed loop and open loop bode plots for the inner loop plt.figure(1), plt.clf(), plt.hold(True), plt.grid(True) cnt.matlab.bode(P_in * C_in, dB=True) cnt.bode(P_in * C_in / (1 + P_in * C_in), dB=True) # Plot the closed loop and open loop bode plots for the outer loop plt.figure(2), plt.clf(), plt.hold(True), plt.grid(True) cnt.matlab.bode(P_out * C_out, dB=True) cnt.bode(P_out * C_out / (1 + P_out * C_out), dB=True) # Calculate the phase and gain margin gm, pm, Wcg, Wcp = cnt.margin(P_in * C_in) print("gm: ", gm, " pm: ", pm, " Wcg: ", Wcg, " Wcp: ", Wcp) gm, pm, Wcg, Wcp = cnt.margin(P_out * C_out) print("gm: ", gm, " pm: ", pm, " Wcg: ", Wcg, " Wcp: ", Wcp) # # display bode plots of transfer functions # plt.figure(1), plt.clf, plt.hold(True), plt.grid(True) # cnt.matlab.bode(P_in, P_in*C_in, dB=True) # #plt.legend('No control', 'PD') # plt.title('Inverted Pendulum, Inner Loop') # # plt.figure(2), plt.clf, plt.hold(True), plt.grid(True) # cnt.matlab.bode(P_out, P_out*C_out, tf([1.0], [1.0, 0.0]), dB=True) # #legend('No control', 'PID','1/s') # plt.title('Inverted Pendulum, Outer Loop')
from pylab import * import control from control import tf #if using termux import subprocess import shlex #end if #Transfer Function = 25/s(s+1)(s+5) before compensator s1 = signal.lti([25], [1, 6, 5, 0]) #signal.bode takes transfer function as input and returns frequency,magnitude and phase arrays w, mag, phase = signal.bode(s1) #signal.bode takes transfer function as input and returns frequency,magnitude and phase arrays sys = tf([25], [1, 6, 5, 0]) gm, pm, Wpc, Wgc = control.margin(sys) print('------------------------------------------------') print("Phase Margin=", pm) #Phase margin print("Gain Margin=", gm) #Gain margin print("Gain crossover frequency(dB)=", Wgc) #Gain crossover freq.(dB) print("Phase crossover frequency(dB)=", Wpc) #Phase crossover freq.(dB) print("-----------------------------------------------") subplot(2, 1, 1) plt.xlabel('Frequency(rad/s)') plt.ylabel('Magnitude(deg)') plt.semilogx(w, mag, color='r') plt.axhline(y=0, xmin=0, xmax=6.438, color='r', linestyle='dashed') plt.axvline(x=2.03, ymin=0, color='k', linestyle='dashed') plt.plot(2.03, 0, 'o') plt.text(2.03, 0, '({}, {})'.format(2.03, 0)) plt.grid()
w = np.logspace(-1, 2, 1000) s = control.tf([1, 0], 1) G = 4 /((s - 1)*(0.02*s + 1)**2) Kc = 1.25 tau1 = 1.5 K = Kc*(1+1/(tau1*s)) L = K*G S = feedback(1, L) T = feedback(L, 1) mag, phase, omega = control.bode(L, w) magS, phaseS, omega = control.bode(S, w) magT, phaseT, omega = control.bode(T, w) plt.legend(["L", "S", "T"], bbox_to_anchor=(0, 1.01, 1, 0), loc=3, ncol=3) Ms = max(magS) Mt = max(magT) gm, pm, wg, wp = control.margin(mag, phase, omega) Lu_180 = 1/np.abs(control.evalfr(L, wg)) P = np.angle(control.evalfr(L, wp)) + np.pi print "Lower GM:", Lu_180 print "PM:", np.round(P*180/np.pi, 1), "deg or", np.round(P, 2), "rad" print "Ms:", Ms print "Mt:", Mt plt.show()
from utils import feedback w = np.logspace(-1, 2, 1000) s = control.tf([1, 0], 1) G = 4 / ((s - 1) * (0.02 * s + 1)**2) Kc = 1.25 tau1 = 1.5 K = Kc * (1 + 1 / (tau1 * s)) L = K * G S = feedback(1, L) T = feedback(L, 1) mag, phase, omega = control.bode(L, w) magS, phaseS, omega = control.bode(S, w) magT, phaseT, omega = control.bode(T, w) plt.legend(["L", "S", "T"], bbox_to_anchor=(0, 1.01, 1, 0), loc=3, ncol=3) Ms = max(magS) Mt = max(magT) gm, pm, wg, wp = control.margin(mag, phase, omega) Lu_180 = 1 / np.abs(control.evalfr(L, wg)) P = np.angle(control.evalfr(L, wp)) + np.pi print "Lower GM:", Lu_180 print "PM:", np.round(P * 180 / np.pi, 1), "deg or", np.round(P, 2), "rad" print "Ms:", Ms print "Mt:", Mt plt.show()
from scipy import signal import control import matplotlib.pyplot as plt #if using termux import subprocess import shlex #end if H = 0.01 #setting the value of H for required phase margin num = [4e10*((np.pi)**2)*H] den = [1,6.28*(10+1e4),4e5*((np.pi)**2)] G = signal.lti(num,den) w, mag, phase = signal.bode(G,w=np.linspace(1,1e5,100000)) sys = control.tf(num, den) gm, pm, wpc, wgc = control.margin(sys) print('Phase Margin in degrees:', pm) print('Gain cross-over frequency in rad/sec:', wgc) #Magnitude plot plt.subplot(2, 1, 1) plt.semilogx(w, mag,'g') plt.plot(wgc, 0, 'o') plt.text(49404,0, '({}, {})'.format(49404,0)) plt.ylabel("20$log_{10}(|G(j\omega)|$") plt.title("Magnitude Plot") plt.grid() # Phase plot plt.subplot(2, 1, 2)
def plot_loops(name, G_ol, G_cl): """ Plot loops :param name: Name of axis :param G_ol: open loop transfer function :param G_cl: closed loop transfer function """ plt.figure() plt.plot(*control.step_response(G_cl, np.linspace(0, 1, 1000))) plt.title(name + ' step resposne') plt.grid() plt.figure() control.bode(G_ol) print('margins', control.margin(G_ol)) plt.subplot(211) plt.title(name + ' open loop bode plot') plt.figure() control.rlocus(G_ol, np.logspace(-2, 0, 1000)) for pole in G_cl.pole(): plt.plot(np.real(pole), np.imag(pole), 'rs') plt.title(name + ' root locus') plt.grid()