def run(self,
fig_num,
is_noise=True,
noise_level=10**(-2),
output_csv=OUTPUT_CSV):
"""
run simulation and save result
:return:
"""
tout, self.y, x = signal.lsim2(self.system, self.u, self.t)
noise = np.zeros(len(self.t))
if is_noise:
noise = np.random.rand(len(self.t)) * noise_level * max(self.y)
self.y += noise
mycsv.save(self.t,
self.u,
self.y,
save_name=self.path + output_csv,
header=("t", "u", "y"))
myplot.plot(fig_num, self.t, self.u)
myplot.plot(fig_num, self.t, self.y)
myplot.save(fig_num,
label=("time [s]", "u/y []"),
save_name=self.path + "Simulation_Result",
leg=("u", "y"))
def plant_data(fig):
"""
:param fig:
:return:
"""
o, reg, img = mycsv.read_float(DATAPATH + "/example2_plant_frf.csv")
g = reg + 1.j * img
o_list = mynum.nsplit(o, NDATA)
g_list = mynum.nsplit(g, NDATA)
fig += 1
for oi, gi in zip(o_list, g_list):
myplot.bodeplot(fig,
oi / 2 / np.pi,
20 * np.log10(abs(gi)),
np.angle(gi, deg=True),
line_style="-",
xl=[10**0, 10**4])
myplot.save(fig,
save_name=DATAPATH + "/" + str(fig) + "_plant",
title="plant")
return fig, o, g
def plant_data(fig, datapath=DATA):
"""
return FRF
:return:
"""
s = symbols('s')
on = 2 * np.pi * np.array([0, 3950, 5400, 6100, 7100])
kappa = [1, -1, 0.4, -1.2, 0.9]
zeta = [0, 0.035, 0.015, 0.015, 0.06]
Kp = 3.7 * 10**7
ol = np.array([])
hl = np.array([])
fig += 1
for i in range(2):
for l in PERT:
if l == 0 and i > 0:
continue
P = 0
for o, k, z in zip(on, kappa, zeta):
o *= (1 + l * (i == 0))
k *= (1 + l * (i == 1))
z *= (1 + l * (i == 2))
P += k / (s**2 + 2 * z * o * s + o**2)
P *= Kp
# calc continous
p = mysignal.symbolic_to_tf(P, s, ts=0)
o = 2 * np.pi * np.linspace(10**0, 10**4, num=F)
o, mag, phase = signal.bode(p, w=o)
theta = phase / 180 * np.pi
a = 10**(mag / 20)
h = a * np.exp(1.j * theta)
# calc discrete with 3/10 delay
h = h * mysignal.zoh_w_delay(o, TS, TD)
myplot.bodeplot(fig,
o / 2 / np.pi,
20 * np.log10(abs(h)),
np.angle(h, deg=True),
line_style="-",
xl=[10**1, 10**4])
ol = np.append(ol, o)
hl = np.append(hl, h)
myplot.save(fig,
save_name=datapath + "/" + str(fig) + "_plant",
title="plant",
leg=["plant" + str(i) for i in range(NDATA)])
mycsv.save(ol,
np.real(hl),
np.imag(hl),
save_name=datapath + "/example1_plant_frf.csv",
header=("o (rad/s)", "real(FRF)", "imag(FRF)"))
assert len(ol) == len(hl)
return fig, np.array(ol), np.array(hl)
def plant(fig, datapath=DATA):
"""
return FRF
:return:
"""
s = symbols('s')
on = 2 * np.pi * np.array([0, 3950, 5400, 6100, 7100])
kappa = [1, -1, 0.4, -1.2, 0.9]
zeta = [0, 0.035, 0.015, 0.015, 0.06]
Kp = 3.7 * 10**7
ol = np.array([])
hl = np.array([])
fig += 1
P = 0
for o, k, z in zip(on, kappa, zeta):
P += k / (s**2 + 2 * z * o * s + o**2)
P *= Kp
# calc continous
p = mysignal.symbolic_to_tf(P, s, ts=0)
o = 2 * np.pi * np.linspace(10**0, 10**4, num=F)
o, mag, phase = signal.bode(p, w=o)
h = mysignal.magphase2resp(mag, phase)
# calc discrete with 3/10 delay
h = h * mysignal.zoh_w_delay(o, TS, TD)
mag, angle = mysignal.resp2magphase(h, deg=True)
myplot.bodeplot(fig,
o / 2 / np.pi,
mag,
angle,
line_style="-",
xl=[10**1, 10**4])
ol = np.append(ol, o)
hl = np.append(hl, h)
myplot.save(fig,
save_name=datapath + "/" + str(fig) + "_plant",
title="plant",
leg=["plant" + str(i) for i in range(NDATA)])
mycsv.save(ol,
np.real(hl),
np.imag(hl),
save_name=datapath + "/example1_plant_frf.csv",
header=("o (rad/s)", "real(FRF)", "imag(FRF)"))
assert len(ol) == len(hl)
return fig, np.array(ol), np.array(hl)
def plotall(fig, fbc, ndata=NDATA, datapath=DATA):
"""
:param fig:
:return:
"""
fbc.split(ndata)
olist = fbc.olist
o = olist[0]
F = len(o)
Llist = fbc.Llist
gcf = fbc.calc_gcf()
print("Gain Crossover Frequencies (Hz):", gcf)
print("\tMin. (Hz):", min(gcf))
print("\tAve. (Hz):", np.mean(gcf))
print("\tMax. (Hz):", max(gcf))
f_gc = min(gcf)
##########Plot1##########
fig += 1
for L in Llist:
# Nyquist
myplot.plot(fig, np.real(L), np.imag(L), lw=6, line_style="-")
_theta = np.linspace(0, 2 * np.pi, 100)
myplot.plot(fig, (np.cos(_theta)), (np.sin(_theta)),
line_style="r--",
lw=1) # r=1
myplot.plot(fig, (0, ), (0, ), line_style="r+") # origin
myplot.plot(fig, (-1, ), (0, ), line_style="r+") # critical
# Stanility Constraint
myplot.plot(fig, (fbc.rm * np.cos(_theta)) + fbc.sigma,
fbc.rm * np.sin(_theta),
line_style="y:") # r=1
myplot.plot(
fig, (-1 / fbc.g_dgm, -np.cos(fbc.theta_dpm), -np.cos(fbc.theta_dpm2)),
(0, -np.sin(fbc.theta_dpm), np.sin(fbc.theta_dpm2)),
line_style="yo")
myplot.save(fig,
xl=[-1, 1],
yl=[-1, 1],
leg=(*["Nyquist" + str(k) for k in range(ndata)], "r=1",
"Origin", "(-1,j0)", "Stb. Cond.", "Margins"),
label=("Re", "Im"),
save_name=datapath + "/" + str(fig) + "_nyquist_enlarged",
title="Optimized Gain-Crossover Frequency (Hz): " + str(f_gc))
myplot.save(fig,
xl=[-3, 3],
yl=[-3, 3],
leg=(*["Nyquist" + str(k) for k in range(ndata)], "r=1",
"Origin", "(-1,j0)", "Stb. Cond.", "Margins"),
label=("Re", "Im"),
save_name=datapath + "/" + str(fig) + "_nyquist",
title="Optimized Gain-Crossover Frequency (Hz): " + str(f_gc))
##########Plot2##########
fig += 1
# L(s)
for L in Llist:
myplot.bodeplot(fig,
o[:F] / 2 / np.pi,
20 * np.log10(abs(L)),
np.angle(L, deg=True),
line_style='-')
myplot.save(fig,
save_name=datapath + "/" + str(fig) + "_bode",
title="Openloop L(s)",
leg=["L" + str(k) for k in range(ndata)])
##########Plot3##########
fig += 1
try:
c = fbc.freqresp()[:F]
except:
c = fbc.C[:F]
myplot.bodeplot(fig,
o[:F] / 2 / np.pi,
20 * np.log10(abs(c)),
np.angle(c, deg=True),
line_style='-')
g = fbc.g[:F]
myplot.bodeplot(fig,
o[:F] / 2 / np.pi,
20 * np.log10(abs(g)),
np.angle(g, deg=True),
line_style='-')
myplot.save(fig,
title='C(s)',
leg=("Controller", "Plant"),
save_name=datapath + "/Controller")
##########Plot4##########
fig += 1
m = fbc.nominal_sensitivity / (-20)
x = (fbc.o_dgc / fbc.o[:F])**m
myplot.plot(fig,
o[:F] / 2 / np.pi,
-20 * np.log10(abs(x)),
line_style='k--',
plotfunc=plt.semilogx)
for L in Llist:
T = 1 / (1 + L)
S = 1 - T
myplot.plot(fig,
o[:F] / 2 / np.pi,
20 * np.log10(abs(T)),
line_style='b-',
plotfunc=plt.semilogx)
myplot.plot(fig,
o[:F] / 2 / np.pi,
20 * np.log10(abs(S)),
line_style='r-',
plotfunc=plt.semilogx)
myplot.save(fig,
save_name=datapath + "/" + str(fig) + "_ST",
title="S(s) (Blue) and T(s) (Red).",
leg=("Nominal:" + str(fbc.nominal_sensitivity) + " dB/dec", ),
yl=(-80, 10))
##########Plot5##########
fig += 1
try:
c = fbc.freqresp(obj="pid")[:F]
myplot.bodeplot(fig,
o[:F] / 2 / np.pi,
20 * np.log10(abs(c)),
np.angle(c, deg=True),
line_style='-')
except:
pass
try:
c = fbc.freqresp(obj="fir")[:F]
myplot.bodeplot(fig,
o[:F] / 2 / np.pi,
20 * np.log10(abs(c)),
np.angle(c, deg=True),
line_style='-')
except:
pass
myplot.save(fig, title='Controllers', leg=("PID", "FIR"))
return fig