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_impedance(self, diag_only:bool=True, axs=None):
"""
Parameters
----------
diag_only : bool, optional
Only plot diagonal elements. The default is True.
axs : matplotlib.axes._subplots.AxesSubplot, optional
Raises
------
NotImplementedError
DESCRIPTION.
Returns
-------
None.
"""
if axs is None:
fig, axs = plt.subplots()
if diag_only:
for idx, dof in enumerate(self.hydro.radiating_dof.data.tolist()):
Zi_frd = control.frd(self.hydro.Zi[:,idx,idx], self.hydro.omega)
control.bode_plot(Zi_frd, self.hydro.omega,
dB=True,
Hz=True,
marker='.',
label=dof)
plt.legend()
else:
raise NotImplementedError() # TODO
def test_bode_margin(dB, maginfty1, maginfty2, gminv, deg, p0, pm, Hz, Wcp,
Wcg):
"""Test bode margins"""
num = [1000]
den = [1, 25, 100, 0]
sys = ctrl.tf(num, den)
plt.figure()
ctrl.bode_plot(sys, margins=True, dB=dB, deg=deg, Hz=Hz)
fig = plt.gcf()
allaxes = fig.get_axes()
mag_to_infinity = (np.array([Wcp, Wcp]), np.array([maginfty1, maginfty2]))
assert_allclose(mag_to_infinity, allaxes[0].lines[2].get_data(), rtol=1e-5)
gm_to_infinty = (np.array([Wcg, Wcg]), np.array([gminv, maginfty2]))
assert_allclose(gm_to_infinty, allaxes[0].lines[3].get_data(), rtol=1e-5)
one_to_gm = (np.array([Wcg, Wcg]), np.array([maginfty1, gminv]))
assert_allclose(one_to_gm, allaxes[0].lines[4].get_data(), rtol=1e-5)
pm_to_infinity = (np.array([Wcp, Wcp]), np.array([1e5, pm]))
assert_allclose(pm_to_infinity, allaxes[1].lines[2].get_data(), rtol=1e-5)
pm_to_phase = (np.array([Wcp, Wcp]), np.array([pm, p0]))
assert_allclose(pm_to_phase, allaxes[1].lines[3].get_data(), rtol=1e-5)
phase_to_infinity = (np.array([Wcg, Wcg]), np.array([0, p0]))
assert_allclose(phase_to_infinity,
allaxes[1].lines[4].get_data(),
rtol=1e-5)
def test_bode_margin(self):
num = [1000]
den = [1, 25, 100, 0]
sys = ctrl.tf(num, den)
ctrl.bode_plot(sys, margins=True, dB=False, deg=True)
fig = plt.gcf()
allaxes = fig.get_axes()
mag_to_infinity = (np.array([6.07828691, 6.07828691]),
np.array([1.00000000e+00, 1.00000000e-08]))
assert_array_almost_equal(mag_to_infinity,
allaxes[0].lines[2].get_data())
gm_to_infinty = (np.array([10., 10.]),
np.array([4.00000000e-01, 1.00000000e-08]))
assert_array_almost_equal(gm_to_infinty,
allaxes[0].lines[3].get_data())
one_to_gm = (np.array([10., 10.]), np.array([1., 0.4]))
assert_array_almost_equal(one_to_gm, allaxes[0].lines[4].get_data())
pm_to_infinity = (np.array([6.07828691, 6.07828691]),
np.array([100000., -157.46405841]))
assert_array_almost_equal(pm_to_infinity,
allaxes[1].lines[2].get_data())
pm_to_phase = (np.array([6.07828691,
6.07828691]), np.array([-157.46405841,
-180.]))
assert_array_almost_equal(pm_to_phase, allaxes[1].lines[3].get_data())
phase_to_infinity = (np.array([10., 10.]),
np.array([1.00000000e-08, -1.80000000e+02]))
assert_array_almost_equal(phase_to_infinity,
allaxes[1].lines[4].get_data())
def test_bode_margin(self):
num = [1000]
den = [1, 25, 100, 0]
sys = ctrl.tf(num, den)
ctrl.bode_plot(sys, margins=True,dB=False,deg = True)
fig = plt.gcf()
allaxes = fig.get_axes()
mag_to_infinity = (np.array([6.07828691, 6.07828691]),
np.array([1.00000000e+00, 1.00000000e-08]))
assert_array_almost_equal(mag_to_infinity, allaxes[0].lines[2].get_data())
gm_to_infinty = (np.array([10., 10.]), np.array([4.00000000e-01, 1.00000000e-08]))
assert_array_almost_equal(gm_to_infinty, allaxes[0].lines[3].get_data())
one_to_gm = (np.array([10., 10.]), np.array([1., 0.4]))
assert_array_almost_equal(one_to_gm, allaxes[0].lines[4].get_data())
pm_to_infinity = (np.array([6.07828691, 6.07828691]),
np.array([100000., -157.46405841]))
assert_array_almost_equal(pm_to_infinity, allaxes[1].lines[2].get_data())
pm_to_phase = (np.array([6.07828691, 6.07828691]), np.array([-157.46405841, -180.]))
assert_array_almost_equal(pm_to_phase, allaxes[1].lines[3].get_data())
phase_to_infinity = (np.array([10., 10.]), np.array([1.00000000e-08, -1.80000000e+02]))
assert_array_almost_equal(phase_to_infinity, allaxes[1].lines[4].get_data())
def exercise_1():
# system transfer function
num = [1, 2]
den = [1, 1, 5, 2]
# create system
sys_1 = system_1()
# nyquist plot
plt.figure()
ctl.nyquist_plot([sys_1])
# plot circle
t = np.linspace(0, 2 * np.pi, 100)
plt.plot(np.cos(t), np.sin(t))
# save plot
plt.savefig("plots/nyquist_plot.png")
plt.clf()
plt.figure()
# bode plot
ctl.bode_plot([sys_1], dB=True)
# save plot
plt.savefig("plots/bode_plot.png")
plt.clf()
def pi_pid(OLTF, q, plot=True):
# SETTLING & RISE
CLTF = ct.feedback(OLTF)
var = cm.stepinfo(CLTF)
print(var)
plt.figure("Q{} - Bode plot".format(q))
ct.bode_plot(CLTF, dB=True, margins=True)
t, c = ct.step_response(CLTF)
plt.figure("Q{} - Step responce question".format(q))
plt.plot(t, c)
if plot: plt.show()
def test_bode_number_of_samples(self):
# Set the number of samples (default is 50, from np.logspace)
mag_ret, phase_ret, omega_ret = ct.bode_plot(self.sys, omega_num=87)
assert len(mag_ret) == 87
# Change the default number of samples
ct.config.defaults['freqplot.number_of_samples'] = 76
mag_ret, phase_ret, omega_ret = ct.bode_plot(self.sys)
assert len(mag_ret) == 76
# Override the default number of samples
mag_ret, phase_ret, omega_ret = ct.bode_plot(self.sys, omega_num=87)
assert len(mag_ret) == 87
def rule_three_four(G,
Gd,
R=1.1,
perfect=True,
freq=np.arange(0.001, 1, 0.001)):
"""
Parameters: G => transfer function of system
Gd => transfer function of disturbances on system
R => max allowed reference change relative to e (R > 1)
perfect => Boolean: True then assumes perfect control
"""
mag_g, phase_g, freq_g = cn.bode_plot(G, omega=freq)
plt.clf()
mag_gd, phase_gd, freq_gd = cn.bode_plot(Gd, omega=freq)
plt.clf()
plt.subplot(211)
if perfect:
plt.loglog(freq_gd, mag_g, color="red", label="|G|", ls="--")
plt.loglog(freq_gd, mag_gd, color="blue", label="|Gd|")
else:
gd_min = mag_gd - 1
gd_min = [x for x in gd_min if x > 0]
freq_all = [
freq_gd[x] for x in range(len(freq_gd)) if (mag_gd[x] - 1) > 0
]
mag_g_all = [
mag_g[x] for x in range(len(freq_gd)) if (mag_gd[x] - 1) > 0
]
plt.loglog(freq_all, gd_min, color="blue", label="|Gd| - 1")
plt.loglog(freq_all, mag_g_all, color="red", label="|G|", ls="--")
plt.legend(loc=3)
plt.grid()
plt.ylabel("Magnitude")
plt.xlabel("Frequency [rad/s]")
plt.subplot(212)
plt.loglog(freq_g, mag_g, color="red", label="|G|", ls="--")
if perfect:
R_stretch = len(freq_g) * [np.abs(R)]
plt.loglog(freq_g, R_stretch, color="blue", label="|R|")
else:
R_stretch = np.subtract(len(freq_g) * [np.abs(R)], [1] * len(freq_g))
plt.loglog(freq_g, R_stretch, color="blue", label="|R|-1")
plt.legend(loc=1)
plt.grid()
plt.ylabel("Magnitude")
plt.xlabel("Frequency [rad/s]")
def test_superimpose():
"""Test superimpose multiple calls.
Test to make sure that multiple calls to plots superimpose their
data on the same axes unless told to do otherwise
"""
# Generate two plots in a row; should be on the same axes
plt.figure(1)
plt.clf()
ctrl.bode_plot(ctrl.tf([1], [1, 2, 1]))
ctrl.bode_plot(ctrl.tf([5], [1, 1]))
# Check that there are two axes and that each axes has two lines
len(plt.gcf().axes) == 2
for ax in plt.gcf().axes:
# Make sure there are 2 lines in each subplot
assert len(ax.get_lines()) == 2
# Generate two plots as a list; should be on the same axes
plt.figure(2)
plt.clf()
ctrl.bode_plot([ctrl.tf([1], [1, 2, 1]), ctrl.tf([5], [1, 1])])
# Check that there are two axes and that each axes has two lines
assert len(plt.gcf().axes) == 2
for ax in plt.gcf().axes:
# Make sure there are 2 lines in each subplot
assert len(ax.get_lines()) == 2
# Generate two separate plots; only the second should appear
plt.figure(3)
plt.clf()
ctrl.bode_plot(ctrl.tf([1], [1, 2, 1]))
plt.clf()
ctrl.bode_plot(ctrl.tf([5], [1, 1]))
# Check to make sure there are two axes and that each axes has one line
assert len(plt.gcf().axes) == 2
for ax in plt.gcf().axes:
# Make sure there is only 1 line in the subplot
assert len(ax.get_lines()) == 1
# Now add a line to the magnitude plot and make sure if is there
for ax in plt.gcf().axes:
if ax.get_label() == 'control-bode-magnitude':
break
ax.semilogx([1e-2, 1e1], 20 * np.log10([1, 1]), 'k-')
assert len(ax.get_lines()) == 2
def test_bode_number_of_samples(self):
# Set the number of samples (default is 50, from np.logspace)
mag_ret, phase_ret, omega_ret = ct.bode_plot(self.sys, omega_num=87)
self.assertEqual(len(mag_ret), 87)
# Change the default number of samples
ct.config.defaults['freqplot.number_of_samples'] = 76
mag_ret, phase_ret, omega_ret = ct.bode_plot(self.sys)
self.assertEqual(len(mag_ret), 76)
# Override the default number of samples
mag_ret, phase_ret, omega_ret = ct.bode_plot(self.sys, omega_num=87)
self.assertEqual(len(mag_ret), 87)
ct.reset_defaults()
def test_bode_number_of_samples(self):
# Set the number of samples (default is 50, from np.logspace)
mag_ret, phase_ret, omega_ret = ct.bode_plot(self.sys, omega_num=87)
self.assertEqual(len(mag_ret), 87)
# Change the default number of samples
ct.config.bode_number_of_samples = 76
mag_ret, phase_ret, omega_ret = ct.bode_plot(self.sys)
self.assertEqual(len(mag_ret), 76)
# Override the default number of samples
mag_ret, phase_ret, omega_ret = ct.bode_plot(self.sys, omega_num=87)
self.assertEqual(len(mag_ret), 87)
ct.reset_defaults()
def question2(OLTF, plot=True):
print(spacer + "Question 2" + spacer)
# 1. BODE PLOT
plt.figure("Q2 - Bode plot")
ct.bode_plot(OLTF, dB=True, margins=True)
if plot: plt.show()
# 2. GAIN
gainHz = ct.stability_margins(OLTF)
gaindB = ct.mag2db(gainHz)
print("\t The maximum gain in Hz is {} and in dB is {}".format(
gainHz, gaindB))
return (gainHz, gaindB)
def plot(self, type, entry1, entry2):
transfer_function = self.get_TF(entry1, entry2)
if type == "bode":
co.bode_plot(transfer_function)
elif type == "nyquist":
co.nyquist(transfer_function)
elif type == "rootlocus":
co.root_locus(transfer_function)
else:
[t1, y1] = self.plot_type(type, transfer_function)
plt.plot(t1, y1)
plt.xlabel('Time(s)')
plt.ylabel('Amplitude')
plt.grid()
plt.show()
def cross_over_freq(sys, tol=0.05):
"""
Only returns the cross over frequency for a transfer function.
The tf may cross from above or below...
Very similar to margin due to a programming glitch on my part...
Parameter: sys => Transfer function like object
tol => tolerance (will increase by 20 % for each cycle of searching)
Returns: cross_over_freq => the cross over frequency i.e. where mag ~ 1
"""
mag, phase, omega = cn.bode_plot(sys, plot=False)
cross_over_dist = np.abs(1 - mag)
# There is a more elegant solution using argmin but that doesn't guarantee
# that you return the first possible match...
index = -1
flag = True
while flag:
for x in range(len(cross_over_dist)):
if cross_over_dist[x] < tol:
index = x
break
if index != -1:
flag = False
else:
tol *= 1.2
return omega[index]
def cross_over_freq(sys, tol=0.05):
"""
Only returns the cross over frequency for a transfer function.
The tf may cross from above or below...
Very similar to margin due to a programming glitch on my part...
Parameter: sys => Transfer function like object
tol => tolerance (will increase by 20 % for each cycle of searching)
Returns: cross_over_freq => the cross over frequency i.e. where mag ~ 1
"""
mag, phase, omega = cn.bode_plot(sys, plot=False)
cross_over_dist = np.abs(1 - mag)
# There is a more elegant solution using argmin but that doesn't guarantee
# that you return the first possible match...
index = -1
flag = True
while flag:
for x in range(len(cross_over_dist)):
if cross_over_dist[x] < tol:
index = x
break
if index != -1:
flag = False
else:
tol = 1.2 * tol
return omega[index]
def plot(self, tf, w=None, label="sys"):
line = dict(color=self.get_next_color())
sys = self.get_sys(tf)
mag_list, phase_list, w = bode_plot(tf,
omega=w,
Plot=False,
omega_limits=None,
omega_num=None,
margins=None)
mag = 20 * np.log10(mag_list)
phase = phase_list * 180 / np.pi
data = {
"x": phase,
"y": mag,
"name": label,
"line": line,
"hovertemplate":
"<b>w</b>: %{text:.3f} rad/s<br><b>mag</b>: %{y:.3f} dB<br><b>phase</b>: %{x:.3f} deg<br>",
"text": w,
"showlegend": False,
}
self.update_min_max(mag, phase)
self.data.append(data)
def position():
vmax = 2
kl = 3.1
lmax = (vmax / kl)**2
lrange = np.arange(-lmax, lmax, 0.0001)[np.newaxis].T
vrange = (np.sign(lrange) * kl * (np.abs(lrange))**0.5)
k1 = np.linalg.pinv(lrange) @ vrange
plt.plot(lrange, vrange)
plt.plot(lrange, k1 * lrange)
#print(k1)
#plt.show()
#plt.clf()
Hs = ct.tf([0, 0, k1], [1, 0, 0])
#print(Hs)
Hz = ct.matlab.c2d(Hs, 1, method='zoh')
#print(Hz)
p = [-0.5, .8] #pole locations
z = [-0.5, .999] #zero loations
gain = 2e-7 #gain
freq = 0.001 #at frequency
Fs0 = 1 #sample rate
a, b = fn.generic_biquad(p, z, gain, freq, Fs0)
print('compensator', a, b)
Kz = ct.tf(b, a, 1)
a, b = fn.biquad_lowpass(0.01, 0.5, 20)
print('lowpass', a, b)
a, b = fn.biquad_lowpass(0.005, 0.707, 1)
Fz = ct.tf(b, a, 1)
ct.bode_plot(Hz * Kz * Fz, np.logspace(-4, -1, 1000))
ct.bode_plot(Hz * Kz, np.logspace(-4, -1, 1000))
plt.show()
plt.clf()
sys = ct.feedback(Hz * Kz * Fz, 1)
y, t = ct.step(sys, np.arange(0, 10000, 1))
plt.plot(t, y.T)
sys = ct.feedback(Hz * Kz, 1)
y, t = ct.step(sys, np.arange(0, 10000, 1))
plt.plot(t, y.T)
plt.show()
'''
def rule_three_four(G, Gd, R=1.1, perfect=True,
freq=np.arange(0.001, 1, 0.001)):
"""
Parameters: G => transfer function of system
Gd => transfer function of disturbances on system
R => max allowed reference change relative to e (R > 1)
perfect => Boolean: True then assumes perfect control
"""
mag_g, phase_g, freq_g = cn.bode_plot(G, omega=freq)
plt.clf()
mag_gd, phase_gd, freq_gd = cn.bode_plot(Gd, omega=freq)
plt.clf()
plt.subplot(211)
if perfect:
plt.loglog(freq_gd, mag_g, color="red", label="|G|", ls="--")
plt.loglog(freq_gd, mag_gd, color="blue", label="|Gd|")
else:
gd_min = mag_gd - 1
gd_min = [x for x in gd_min if x > 0]
freq_all = [
freq_gd[x] for x in range(len(freq_gd)) if (mag_gd[x]-1) > 0]
mag_g_all = [
mag_g[x] for x in range(len(freq_gd)) if (mag_gd[x]-1) > 0]
plt.loglog(freq_all, gd_min, color="blue", label="|Gd| - 1")
plt.loglog(freq_all, mag_g_all, color="red", label="|G|", ls="--")
plt.legend(loc=3)
plt.grid()
plt.ylabel("Magnitude")
plt.xlabel("Frequency [rad/s]")
plt.subplot(212)
plt.loglog(freq_g, mag_g, color="red", label="|G|", ls="--")
if perfect:
R_stretch = len(freq_g)*[np.abs(R)]
plt.loglog(freq_g, R_stretch, color="blue", label="|R|")
else:
R_stretch = np.subtract(len(freq_g)*[np.abs(R)], [1]*len(freq_g))
plt.loglog(freq_g, R_stretch, color="blue", label="|R|-1")
plt.legend(loc=1)
plt.grid()
plt.ylabel("Magnitude")
plt.xlabel("Frequency [rad/s]")
def rule_one_two(S, Gd, R = 1.1, freq = np.arange(0.001, 1,0.001)):
"""
Parameters: S => sensitivity transfer function
Gd => transfer function of the disturbance
w => frequency range (mostly there to make the plots look nice
You want the red dashed line above the blue solid line for adequate
disturbance rejection.
"""
mag_s, phase_s, freq_s = cn.bode_plot(S, omega=freq)
plt.clf()
inv_gd = 1/Gd
mag_i, phase_i, freq_i = cn.bode_plot(inv_gd, omega=freq)
plt.clf()
unity = [1] * len(freq)
inv_R = [1.0/R] * len(freq)
plt.xlabel("Frequency [rad/s]")
plt.ylabel("Magnitude")
def rule_one_two(S, Gd, R=1.1, freq=np.arange(0.001, 1, 0.001)):
"""
Parameters: S => sensitivity transfer function
Gd => transfer function of the disturbance
w => frequency range (mostly there to make the plots look nice
You want the red dashed line above the blue solid line for adequate
disturbance rejection.
"""
mag_s, phase_s, freq_s = cn.bode_plot(S, omega=freq)
plt.clf()
inv_gd = 1 / Gd
mag_i, phase_i, freq_i = cn.bode_plot(inv_gd, omega=freq)
plt.clf()
unity = [1] * len(freq)
inv_R = [1.0 / R] * len(freq)
plt.xlabel("Frequency [rad/s]")
plt.ylabel("Magnitude")
def test_superimpose(self):
# Test to make sure that multiple calls to plots superimpose their
# data on the same axes unless told to do otherwise
# Generate two plots in a row; should be on the same axes
plt.figure(1); plt.clf()
ctrl.bode_plot(ctrl.tf([1], [1,2,1]))
ctrl.bode_plot(ctrl.tf([5], [1, 1]))
# Check to make sure there are two axes and that each axes has two lines
self.assertEqual(len(plt.gcf().axes), 2)
for ax in plt.gcf().axes:
# Make sure there are 2 lines in each subplot
assert len(ax.get_lines()) == 2
# Generate two plots as a list; should be on the same axes
plt.figure(2); plt.clf();
ctrl.bode_plot([ctrl.tf([1], [1,2,1]), ctrl.tf([5], [1, 1])])
# Check to make sure there are two axes and that each axes has two lines
self.assertEqual(len(plt.gcf().axes), 2)
for ax in plt.gcf().axes:
# Make sure there are 2 lines in each subplot
assert len(ax.get_lines()) == 2
# Generate two separate plots; only the second should appear
plt.figure(3); plt.clf();
ctrl.bode_plot(ctrl.tf([1], [1,2,1]))
plt.clf()
ctrl.bode_plot(ctrl.tf([5], [1, 1]))
# Check to make sure there are two axes and that each axes has one line
self.assertEqual(len(plt.gcf().axes), 2)
for ax in plt.gcf().axes:
# Make sure there is only 1 line in the subplot
assert len(ax.get_lines()) == 1
# Now add a line to the magnitude plot and make sure if is there
for ax in plt.gcf().axes:
if ax.get_label() == 'control-bode-magnitude':
break
ax.semilogx([1e-2, 1e1], 20 * np.log10([1, 1]), 'k-')
self.assertEqual(len(ax.get_lines()), 2)
def test_bode_feature_periphery_decade(self):
# Generate a sample Bode plot to figure out the range it uses
ct.reset_defaults() # Make sure starting state is correct
mag_ret, phase_ret, omega_ret = ct.bode_plot(self.sys, Hz=False)
omega_min, omega_max = omega_ret[[0, -1]]
# Reset the periphery decade value (should add one decade on each end)
ct.config.defaults['freqplot.feature_periphery_decades'] = 2
mag_ret, phase_ret, omega_ret = ct.bode_plot(self.sys, Hz=False)
np.testing.assert_almost_equal(omega_ret[0], omega_min / 10)
np.testing.assert_almost_equal(omega_ret[-1], omega_max * 10)
# Make sure it also works in rad/sec, in opposite direction
mag_ret, phase_ret, omega_ret = ct.bode_plot(self.sys, Hz=True)
omega_min, omega_max = omega_ret[[0, -1]]
ct.config.defaults['freqplot.feature_periphery_decades'] = 1
mag_ret, phase_ret, omega_ret = ct.bode_plot(self.sys, Hz=True)
np.testing.assert_almost_equal(omega_ret[0], omega_min * 10)
np.testing.assert_almost_equal(omega_ret[-1], omega_max / 10)
def c2v_design(R,
L,
n_pp,
J_s,
KE,
B=0,
CLBW_Hz=1000,
CL_TS=1 / 20e3,
fignum=5):
currentKp, currentKi = get_coeffs_dc_motor_current_regulator(R, L, CLBW_Hz)
currentKiCode = currentKi * currentKp * CL_TS
if True:
# 这里打印的用于实验中CCS的debug窗口检查电流环PI系数
上位机电流KP = CLBW_Hz
上位机电流KI = 1000
iSMC_currentKp = 上位机电流KP * L * 2 * np.pi
iSMC_currentKi = 上位机电流KI / 1000 * R / L
iSMC_currentKiCode = iSMC_currentKi * CL_TS * iSMC_currentKp
print(f'\tiSMC_currentKp={iSMC_currentKp:g}, \
iSMC_currentKi={iSMC_currentKi:g}, \
iSMC_currentKiCode={iSMC_currentKiCode:g}')
print(f'\tSimC_currentKp={currentKp:g}, \
SimC_currentKi={currentKi:g}, \
SimC_currentKiCode={currentKiCode:g}')
print(f'\t上位机电流KP={上位机电流KP:g}, \
上位机电流KI={上位机电流KI:g}')
Gi_closed = control.tf(
[1], [L / currentKp, 1]) # current loop zero-pole cancelled already
currentBandwidth_radPerSec = currentKp / L
KT = 1.5 * n_pp * KE
dc_motor_motion = control.tf([KT],
[J_s / n_pp, B]) # [Apk] to [elec.rad/s]
print(dc_motor_motion)
# quit()
# 注意，我们研究的开环传递函数是以电流给定为输入的，而不是以转速控制误差为输入，这样仿真和实验容易实现一点。
# Gw_open = dc_motor_motion * Gi_closed * speedPI
c2v_tf = dc_motor_motion * Gi_closed
# fig5 = plt.figure(fignum)
# plt.title('Designed Current Ref. to Velocity Meas. Transfer Function')
mag, phase, omega = control.bode_plot(c2v_tf,
2 * np.pi * np.logspace(0, 4, 500),
dB=1,
Hz=1,
deg=1,
lw='0.5',
label=f'{CLBW_Hz:g} Hz')
open_cutoff_frequency_HZ = omega[(np.abs(mag - 0.0)).argmin()] / 2 / np.pi
# print('\tCut-off frequency (without speed PI regulator):', open_cutoff_frequency_HZ, 'Hz')
return (currentKp, currentKi), \
(上位机电流KP, 上位机电流KI), \
(mag, phase, omega)
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 peaks(sys):
"""
Returns the H infinity norms and the corresponding frequencies for
the sensitivity and complementary sensitivity functions.
Currently works for SISO only.
Parameter: sys => GK (loop transfer function) in tf format
Returns: 2 tuples of the format ('Type', peak, peak freq)
"""
S = 1 / (1 + sys)
T = sys / (1 + sys)
fg = plt.figure()
mag_S, phase_S, omega_S = cn.bode_plot(S)
mag_T, phase_T, omega_T = cn.bode_plot(T)
plt.close(fg) # Prevents interference with current plotting ;)
pos_S = np.argmax(mag_S)
pos_T = np.argmin(mag_T)
s_data = ("Peak |S|", mag_S[pos_S], phase_S[pos_S])
t_data = ("Peak |T|", mag_T[pos_T], phase_T[pos_T])
return s_data, t_data
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 test_custom_bode_default(self):
ct.config.defaults['bode.dB'] = True
ct.config.defaults['bode.deg'] = True
ct.config.defaults['bode.Hz'] = True
# Generate a Bode plot
plt.figure()
omega = np.logspace(-3, 3, 100)
ct.bode_plot(self.sys, omega, dB=True)
mag_x, mag_y = (((plt.gcf().axes[0]).get_lines())[0]).get_data()
np.testing.assert_almost_equal(mag_y[0], 20 * log10(10), decimal=3)
# Override defaults
plt.figure()
ct.bode_plot(self.sys, omega, Hz=True, deg=False, dB=True)
mag_x, mag_y = (((plt.gcf().axes[0]).get_lines())[0]).get_data()
phase_x, phase_y = (((plt.gcf().axes[1]).get_lines())[0]).get_data()
np.testing.assert_almost_equal(mag_x[0], 0.001 / (2 * pi), decimal=6)
np.testing.assert_almost_equal(mag_y[0], 20 * log10(10), decimal=3)
np.testing.assert_almost_equal(phase_y[-1], -pi, decimal=2)
def peaks(sys):
"""
Returns the H infinity norms and the corresponding frequencies for
the sensitivity and complementary sensitivity functions.
Currently works for SISO only.
Parameter: sys => GK (loop transfer function) in tf format
Returns: 2 tuples of the format ('Type', peak, peak freq)
"""
S = 1 / (1 + sys)
T = sys / (1 + sys)
fg = plt.figure()
mag_S, phase_S, omega_S = cn.bode_plot(S)
mag_T, phase_T, omega_T = cn.bode_plot(T)
plt.close(fg) # Prevents interference with current plotting ;)
pos_S = np.argmax(mag_S)
pos_T = np.argmin(mag_T)
s_data = ("Peak |S|", mag_S[pos_S], phase_S[pos_S])
t_data = ("Peak |T|", mag_T[pos_T], phase_T[pos_T])
return s_data, t_data
def test_bode_feature_periphery_decade(self):
# Generate a sample Bode plot to figure out the range it uses
ct.reset_defaults() # Make sure starting state is correct
mag_ret, phase_ret, omega_ret = ct.bode_plot(self.sys, Hz=False)
omega_min, omega_max = omega_ret[[0, -1]]
# Reset the periphery decade value (should add one decade on each end)
ct.config.bode_feature_periphery_decade = 2
mag_ret, phase_ret, omega_ret = ct.bode_plot(self.sys, Hz=False)
np.testing.assert_almost_equal(omega_ret[0], omega_min/10)
np.testing.assert_almost_equal(omega_ret[-1], omega_max * 10)
# Make sure it also works in rad/sec, in opposite direction
mag_ret, phase_ret, omega_ret = ct.bode_plot(self.sys, Hz=True)
omega_min, omega_max = omega_ret[[0, -1]]
ct.config.bode_feature_periphery_decade = 1
mag_ret, phase_ret, omega_ret = ct.bode_plot(self.sys, Hz=True)
np.testing.assert_almost_equal(omega_ret[0], omega_min*10)
np.testing.assert_almost_equal(omega_ret[-1], omega_max/10)
ct.reset_defaults()
def test_options(editsdefaults):
"""Test ability to set parameter values"""
# Generate a Bode plot of a transfer function
sys = ctrl.tf([1000], [1, 25, 100, 0])
fig1 = plt.figure()
ctrl.bode_plot(sys, dB=False, deg=True, Hz=False)
# Save the parameter values
left1, right1 = fig1.axes[0].xaxis.get_data_interval()
numpoints1 = len(fig1.axes[0].lines[0].get_data()[0])
# Same transfer function, but add a decade on each end
ctrl.config.set_defaults('freqplot', feature_periphery_decades=2)
fig2 = plt.figure()
ctrl.bode_plot(sys, dB=False, deg=True, Hz=False)
left2, right2 = fig2.axes[0].xaxis.get_data_interval()
# Make sure we got an extra decade on each end
assert_allclose(left2, 0.1 * left1)
assert_allclose(right2, 10 * right1)
# Same transfer function, but add more points to the plot
ctrl.config.set_defaults('freqplot',
feature_periphery_decades=2,
number_of_samples=13)
fig3 = plt.figure()
ctrl.bode_plot(sys, dB=False, deg=True, Hz=False)
numpoints3 = len(fig3.axes[0].lines[0].get_data()[0])
# Make sure we got the right number of points
assert numpoints1 != numpoints3
assert numpoints3 == 13
def P_Bode(self):
s = control.tf([1, 0], [1])
CE = 1 / (s**2 + 8.8 * s + 40)
P = control.tf([1], [1])
OL = P * CE
CL = control.feedback(OL, 1, sign=-1)
control.root_locus(CL, Plot=True)
plt.figure()
P = control.tf([100], [1])
OL1 = P * CE
CL1 = control.feedback(OL1, 1, sign=-1)
P = control.tf([500], [1])
OL2 = P * CE
CL2 = control.feedback(OL2, 1, sign=-1)
P = control.tf([2000], [1])
OL3 = P * CE
CL3 = control.feedback(OL3, 1, sign=-1)
print("Closed Loop")
print([CL1, CL2, CL3])
control.bode_plot([OL1, OL2, OL3], Plot=True)
print(CL)
def test_custom_bode_default(self):
ct.bode_dB = True
ct.bode_deg = True
ct.bode_Hz = True
# Generate a Bode plot
plt.figure()
omega = np.logspace(-3, 3, 100)
ct.bode_plot(self.sys, omega, dB=True)
mag_x, mag_y = (((plt.gcf().axes[0]).get_lines())[0]).get_data()
np.testing.assert_almost_equal(mag_y[0], 20*log10(10), decimal=3)
# Override defaults
plt.figure()
ct.bode_plot(self.sys, omega, Hz=True, deg=False, dB=True)
mag_x, mag_y = (((plt.gcf().axes[0]).get_lines())[0]).get_data()
phase_x, phase_y = (((plt.gcf().axes[1]).get_lines())[0]).get_data()
np.testing.assert_almost_equal(mag_x[0], 0.001 / (2*pi), decimal=6)
np.testing.assert_almost_equal(mag_y[0], 20*log10(10), decimal=3)
np.testing.assert_almost_equal(phase_y[-1], -pi, decimal=2)
ct.reset_defaults()
def rule_one_two(S, Gd, R=1.1, freq=np.arange(0.001, 1, 0.001)):
"""
Parameters: S => sensitivity transfer function
Gd => transfer function of the disturbance
w => frequency range (mostly there to make the plots look nice
You want the red dashed line above the blue solid line for adequate
disturbance rejection.
"""
mag_s, phase_s, freq_s = cn.bode_plot(S, omega=freq)
plt.clf()
inv_gd = 1 / Gd
mag_i, phase_i, freq_i = cn.bode_plot(inv_gd, omega=freq)
plt.clf()
unity = [1] * len(freq)
inv_R = [1.0 / R] * len(freq)
plt.loglog(freq_s, inv_R, color="green", lw=3.0, ls="--", label="1/R")
plt.loglog(freq_s, unity, color="black", ls=":")
plt.loglog(freq_s, mag_s, color="blue", label="|S|")
plt.loglog(freq_i, mag_i, color="red", ls="--", label="1/|Gd|")
plt.legend(loc=4)
plt.grid()
plt.xlabel("Frequency [rad/s]")
plt.ylabel("Magnitude")
def rule_one_two(S, Gd, R=1.1, freq=np.arange(0.001, 1, 0.001)):
"""
Parameters: S => sensitivity transfer function
Gd => transfer function of the disturbance
w => frequency range (mostly there to make the plots look nice
You want the red dashed line above the blue solid line for adequate
disturbance rejection.
"""
mag_s, phase_s, freq_s = cn.bode_plot(S, omega=freq)
plt.clf()
inv_gd = 1 / Gd
mag_i, phase_i, freq_i = cn.bode_plot(inv_gd, omega=freq)
plt.clf()
unity = [1] * len(freq)
inv_R = [1.0 / R] * len(freq)
plt.loglog(freq_s, inv_R, color="green", lw=3.0, ls="--", label="1/R")
plt.loglog(freq_s, unity, color="black", ls=':')
plt.loglog(freq_s, mag_s, color="blue", label="|S|")
plt.loglog(freq_i, mag_i, color="red", ls='--', label="1/|Gd|")
plt.legend(loc=4)
plt.grid()
plt.xlabel("Frequency [rad/s]")
plt.ylabel("Magnitude")
def test_matlab_bode(self):
ct.use_matlab_defaults()
# Generate a Bode plot
plt.figure()
omega = np.logspace(-3, 3, 100)
ct.bode_plot(self.sys, omega)
# Get the magnitude line
mag_axis = plt.gcf().axes[0]
mag_line = mag_axis.get_lines()
mag_data = mag_line[0].get_data()
mag_x, mag_y = mag_data
# Make sure the x-axis is in rad/sec and y-axis is in dB
np.testing.assert_almost_equal(mag_x[0], 0.001, decimal=6)
np.testing.assert_almost_equal(mag_y[0], 20 * log10(10), decimal=3)
# Get the phase line
phase_axis = plt.gcf().axes[1]
phase_line = phase_axis.get_lines()
phase_data = phase_line[0].get_data()
phase_x, phase_y = phase_data
# Make sure the x-axis is in rad/sec and y-axis is in degrees
np.testing.assert_almost_equal(phase_x[-1], 1000, decimal=1)
np.testing.assert_almost_equal(phase_y[-1], -180, decimal=0)
# Override the defaults and make sure that works as well
plt.figure()
ct.bode_plot(self.sys, omega, dB=True)
mag_x, mag_y = (((plt.gcf().axes[0]).get_lines())[0]).get_data()
np.testing.assert_almost_equal(mag_y[0], 20 * log10(10), decimal=3)
plt.figure()
ct.bode_plot(self.sys, omega, Hz=True)
mag_x, mag_y = (((plt.gcf().axes[0]).get_lines())[0]).get_data()
np.testing.assert_almost_equal(mag_x[0], 0.001 / (2 * pi), decimal=6)
plt.figure()
ct.bode_plot(self.sys, omega, deg=False)
phase_x, phase_y = (((plt.gcf().axes[1]).get_lines())[0]).get_data()
np.testing.assert_almost_equal(phase_y[-1], -pi, decimal=2)
ct.reset_defaults()
def test_matlab_bode(self):
ct.use_matlab_defaults();
# Generate a Bode plot
plt.figure()
omega = np.logspace(-3, 3, 100)
ct.bode_plot(self.sys, omega)
# Get the magnitude line
mag_axis = plt.gcf().axes[0]
mag_line = mag_axis.get_lines()
mag_data = mag_line[0].get_data()
mag_x, mag_y = mag_data
# Make sure the x-axis is in Hertz and y-axis is in dB
np.testing.assert_almost_equal(mag_x[0], 0.001 / (2*pi), decimal=6)
np.testing.assert_almost_equal(mag_y[0], 20*log10(10), decimal=3)
# Get the phase line
phase_axis = plt.gcf().axes[1]
phase_line = phase_axis.get_lines()
phase_data = phase_line[0].get_data()
phase_x, phase_y = phase_data
# Make sure the x-axis is in Hertz and y-axis is in degrees
np.testing.assert_almost_equal(phase_x[-1], 1000 / (2*pi), decimal=1)
np.testing.assert_almost_equal(phase_y[-1], -180, decimal=0)
# Override the defaults and make sure that works as well
plt.figure()
ct.bode_plot(self.sys, omega, dB=True)
mag_x, mag_y = (((plt.gcf().axes[0]).get_lines())[0]).get_data()
np.testing.assert_almost_equal(mag_y[0], 20*log10(10), decimal=3)
plt.figure()
ct.bode_plot(self.sys, omega, Hz=True)
mag_x, mag_y = (((plt.gcf().axes[0]).get_lines())[0]).get_data()
np.testing.assert_almost_equal(mag_x[0], 0.001 / (2*pi), decimal=6)
plt.figure()
ct.bode_plot(self.sys, omega, deg=False)
phase_x, phase_y = (((plt.gcf().axes[1]).get_lines())[0]).get_data()
np.testing.assert_almost_equal(phase_y[-1], -pi, decimal=2)
ct.reset_defaults()
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 bode(tf_list=[], omega=None, name=None):
hovertemplate_mag = "<b>w</b>: %{x:.3f} rad/s<br><b>mag</b>: %{y:.3f} dB<br><b>phase</b>: %{text:.3f} deg<br>"
hovertemplate_phase = "<b>w</b>: %{x:.3f} rad/s<br><b>mag</b>: %{text:.3f} dB<br><b>phase</b>: %{y:.3f} deg<br>"
fig = make_subplots(rows=2, cols=1, shared_xaxes=True)
for index, tf in enumerate(tf_list):
mag_list, phase_list, omega = ctl.bode_plot(tf,
omega=omega,
Plot=False,
omega_limits=None,
omega_num=None,
margins=None)
mag = 20 * np.log10(mag_list)
phase = phase_list * 180 / np.pi
tf_name = "tf {}".format(index + 1)
data_mag = {
"x": omega,
"y": mag,
"name": tf_name,
"hovertemplate": hovertemplate_mag,
"text": phase,
"showlegend": False
}
data_phase = {
"x": omega,
"y": phase,
"name": tf_name,
"hovertemplate": hovertemplate_phase,
"text": mag,
"showlegend": False
}
#add to plotly
fig.add_trace(data_mag, row=1, col=1)
fig.add_trace(data_phase, row=2, col=1)
fig.update_yaxes(title_text="Magnitude", row=1, col=1)
fig.update_xaxes(title_text="w (rad/s)", type="log", row=1, col=1)
fig.update_yaxes(title_text="Phase", row=2, col=1)
fig.update_xaxes(title_text="w (rad/s)", type="log", row=2, col=1)
fig.show()
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 showBode(self):
fig1, ax3 = plt.subplots(nrows=2, ncols=1)
control.bode_plot(self.sys)
plt.show()
# ft_vel_a4 = lambda x,y: ft_vel1(x,y) + ft_vel4(x,y,aa1[4])
# ft_vel_a5 = lambda x,y: ft_vel1(x,y) + ft_vel4(x,y,aa1[5])
# pole = (4*a*b + v**2)/a1
# pole_log = np.log10(pole)
# print('pole', pole, pole_log)
# ft_vel6 = lambda x,y: q*np.exp(v*x/(2*a))/(4*np.pi*K*a*s)*(v*kv(0,R(x,y)*D(s)/(2*a)) - v*kv(0,R(x,y)*D(0)/(2*a))\
# + x*(4*a*b + v**2)*kv(0,R(x,y)*D(0)/(2*a))/(4*a) - x*(4*a*b + v**2)*kv(0,R(x,y)*D(0)/(2*a))/(4*a)/(4*a/(4*a*b + v**2)*s + 1))
# ft_vel7 = lambda x,y: q*np.exp(v*x/(2*a))/(4*np.pi*K*a*s)*(v*kv(0,R(x,y)*D(s)/(2*a)) - v*kv(0,R(x,y)*D(0)/(2*a))\
# + (4*a*b + v**2)*kv(0,R(x,y)*D(0)/(2*a)) - (4*a*b + v**2)*kv(0,R(x,y)*D(0)/(2*a))/(x**2/(4*a*b + v**2)*s + 1))
ft_vel0 = lambda x,y: q*(-v*R(x,y)*kv(1, R(x,y)*D(0)/(2*a)) + x*D(0)*kv(0, R(x,y)*D(0)/(2*a)))*np.exp(v*x/(2*a))/(4*np.pi*K*a*D(0))
if False:
H1 = U * np.fft.fft(X[:, 4]) / U ** 2
control.bode_plot((control.frd(H1[idx], w),control.frd(ft_vel(*ops[1]), w)),w, dB=True)
ft_vel1 = lambda x, y: q * np.exp(v * x / (2 * a)) / (4 * np.pi * K * a * s) * (
v * kv(0, R(x, y) * D(s) / (2 * a)) - v * kv(0, R(x, y) * D(0) / (2 * a))\
+ x * kv(0, R(x, y) * D(0) / (2 * a))/(R(x, y) * D(s)))
control.bode_plot(control.frd(ft_vel1(.01,0), w), w, dB=True)
U = np.fft.fft(X[:,2])
for i in range(3,X.shape[1]-4):
H1 = U * np.fft.fft(X[:,i]) / U**2
plt.figure('Bode T/Pow ' + str(ops[i - 3]))
control.bode_plot((control.frd(H1[idx], w), #control.frd(ft_vel1(*ops[i - 3]), w), #control.frd(ft_vel8(*ops[i - 3]), w)),w, dB=True)
# control.frd(ft_vel_a1(*ops[i - 3]), w),control.frd(ft_vel_a2(*ops[i - 3]), w),
# control.frd(ft_vel61(*ops[i - 3]), w), control.frd(ft_vel62(*ops[i - 3]), w),
# control.frd(ft_vel_a5(*ops[i - 3]), w),
control.frd(ft_vel3(*ops[i - 3]), w), control.frd(ft_vel6(*ops[i - 3]), w)),w, dB=True)
plt.legend(('fem', 'fdt3', 'fdt6'))
# freqLin_hz = np.logspace(np.log10(0.01), np.log10(25), 800)
freqLin_hz = np.linspace(1e-1, 1e1, 400)
freqLin_rps = freqLin_hz * hz2rps
# OL : Mixer -> Plant -> SCAS_FB [Remain Open at vFb]
connectName = sysMixer.OutputName + sysScas.InputName[1::3]
inKeep = sysMixer.InputName[1:] + sysPlant.InputName[7:]
outKeep = sysPlant.OutputName + sysScas.OutputName[0::4] + sysScas.OutputName[
1::4] + sysScas.OutputName[2::4]
sysOL = Systems.ConnectName([sysMixer, sysPlant, sysScas], connectName, inKeep,
outKeep)
if False:
_ = control.bode_plot(sysOL[7, 0],
omega_limits=[0.01, 500],
Hz=True,
dB=True)
_ = control.bode_plot(sysOL[8, 1],
omega_limits=[0.01, 500],
Hz=True,
dB=True)
_ = control.bode_plot(sysOL[9, 2],
omega_limits=[0.01, 500],
Hz=True,
dB=True)
# CL: Ctrl -> Plant [Connect at vCmd]
connectName = sysMixer.OutputName + sysScas.InputName[
1::3] + sysMixer.InputName[1:]
inKeep = sysScas.InputName[2::3] + sysScas.InputName[
0::3] + sysPlant.InputName[7:]
X=np.load('output_data_vel.npy')
# plt.plot(X[:1000,2])
dt = X[1,0]-X[0,0]
N = int(X.shape[0]/2)
freqs = np.arange(N)/(2*N*dt)
wf = freqs*2*np.pi
idx = np.logspace(0,3,100).astype(int)
w = wf[idx]
'''
w = np.logspace(-3,2,100)
s = 1j*w
ft_vel = lambda x,y,z: -q/(rho*cp)*(v*np.sqrt(x**2 + y**2 + z**2) + (-x)*np.sqrt(4*a*s + v**2))*np.exp(-(v*(-x) + np.sqrt(4*a*s + v**2)*np.sqrt(x**2 + y**2 + z**2))/(2*a))/(4*np.pi*a**2*np.sqrt(4*a*s + v**2)*np.sqrt(x**2 + y**2 + z**2))
R = lambda x,y,z: np.sqrt(x**2+y**2+z**2)
D = lambda s: np.sqrt(4*a*s + v**2)
XX = lambda x,y,z,s: (v*R(x,y,z) - x*D(s))*np.exp(-R(x,y,z)*D(s)/(2*a))/D(s)
ft_vel2 = lambda x,y,z: -q/(rho*cp)*np.exp(v*x/(2*a))/(4*s*np.pi*a**2*R(x,y,z))*(XX(x,y,z,s)-XX(x,y,z,0))
ops = ((-.01,0,0),(.01,0,0),(.02,0,0),(.04,0,0),(0,-.01,0),(0,.01,0))
#U = np.fft.fft(X[:,2])
for i in range(len(ops)):
#H1 = U * np.fft.fft(X[:,i]) / U**2
plt.figure('Bode T/Vel ' + str(ops[i]))
control.bode_plot((control.frd(ft_vel2(*ops[i]), w),control.frd(ft_vel(*ops[i]), w)),w, dB=True)
plt.legend(('fdt2', 'fdt'))
plt.show()
wf = freqs*2*np.pi
idx = np.logspace(0,3,100).astype(int)
w = wf[idx]
s = 1j*w
ft_pot = lambda x,y,z: 1/(rho*cp)*np.exp(-(v*x+np.sqrt((x**2+y**2+z**2)*(4*a*s+v**2)))/(2*a))/(2*np.pi*a*np.sqrt(x**2+y**2+z**2))
Mf=None
af=None
frds_fdt=[]
frds_fem=[]
U = np.fft.fft(X[:,1])
for i in range(3,X.shape[1]):
H1 = U * np.fft.fft(X[:,i]) / U**2
plt.figure('Bode T/Pow ' + str(ops[i - 3]))
control.bode_plot((control.frd(H1[idx], w),control.frd(ft_pot(*ops[i - 3]), w)),w, dB=True)
plt.legend(('fem', 'fdt'))
# frds_fem.append(control.frd(H1[:N], wf))
# frds_fdt.append(control.frd(ft_pot(*ops[i - 3]), wf))
#
# H1 = H1[:N]
#
# Mf = np.vstack((Mf, 20*np.log10(abs(H1)))) if Mf is not None else 20*np.log10(abs(H1))
# # Mf = np.vstack((Mf, abs(H1))) if Mf is not None else abs(H1)
# af = np.vstack((af, np.angle(H1)*180/np.pi)) if af is not None else np.angle(H1)*180/np.pi
plt.figure('Magnitude T/Pow')
for i in range(X.shape[1]-3):
plt.semilogx(wf, Mf[i,:])
plt.legend(('x=-.01','x=.01','x=.02','x=.04','y=-.005','y=.005','y=.01','z=.005'))
