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()
