from control.matlab import *
import matplotlib.pyplot as plt
import numpy as np
import sympy as sp
import function_chapt6 as func
# P = tf([0, 1], [1, 1, 1.5, 1])
P = tf([0, 1], [1, 2, 2, 1])
# get a frequency when the phase will delay for 180 deg
_, _, wpc, _ = margin(P)
t = np.arange(0, 30, 0.1)
u = np.sin(wpc * t)
y = 0 * u
fig, ax = plt.subplots(2, 2)
for i in range(2):
for j in range(2):
# feedback the output negatively, and generate new input at next time stamp
u = np.sin(wpc * t) - y
y, t, x0 = lsim(P, u, t, 0)
ax[i, j].plot(t, u, ls='--', label='u')
ax[i, j].plot(t, y, label='y')
func.plot_set(ax[i, j], 't', 'u, y')
fig.tight_layout()
plt.show()
LS2 = func.linestyle_generator()
fig, ax = plt.subplots(2, 1)
fig, ax2 = plt.subplots()
for i in range(len(kd)):
K = tf([kd[i], kp, ki], [1, 0]) # proportional + integral control
H = P * K # open loop system
gain, phase, w = bode(H, logspace(-1, 2), Plot=False)
# closed loop system
Gyr = feedback(P * K, 1)
y, t = step(Gyr, np.arange(0, 2, 0.01))
pltargs2 = {'ls': next(LS2), 'label': '$k_D$=' + str(kd[i])}
ax2.plot(t, y * ref, **pltargs2)
# bode diagram
pltargs = {'ls': next(LS), 'label': '$k_D$=' + str(kd[i])}
ax[0].semilogx(w, 20 * np.log10(gain), **pltargs)
ax[1].semilogx(w, phase * 180 / np.pi, **pltargs)
# gain margin, phase margen, phase cross frequency, gain cross frequency
print('kP = ', kp, 'kI = ', ki, 'kD = ', kd[i])
print('(GM,PM,wpc,wgc)')
print(margin(H))
print('--------------------')
ax2.axhline(ref, color="k", linewidth=0.5)
func.bodeplot_set(ax, 3)
func.plot_set(ax2, 't', 'y', 'best')
plt.show()
import numpy as np
import sympy as sp
import function_chapt6 as func
fig, ax = plt.subplots(1, 2)
fig, ax2 = plt.subplots(2, 1)
LS = func.linestyle_generator()
# the case that closed loop system will be unstable
P = tf([0, 1], [1, 1, 1.5, 1])
x, y, _ = nyquist(P, logspace(-3, 5, 1000), Plot=False)
ax[0].plot(x, y, color='k')
ax[0].plot(x, -y, ls='--', color='k')
ax[0].scatter(-1, 0, color='k')
func.plot_set(ax[0], 'Re', 'Im')
gain, phase, w = bode(P, logspace(-2, 2), Plot=False)
pltargs = {'ls': next(LS), 'label': 'P=' + str(P)}
ax2[0].semilogx(w, 20 * np.log10(gain), **pltargs)
ax2[1].semilogx(w, phase * 180 / np.pi, **pltargs)
# the case that closed loop system will be stable
P = tf([0, 1], [1, 2, 2, 1])
x, y, _ = nyquist(P, logspace(-3, 5, 1000), Plot=False)
ax[1].plot(x, y, color='k')
ax[1].plot(x, -y, ls='--', color='k')
ax[1].scatter(-1, 0, color='k')
func.plot_set(ax[1], 'Re', 'Im')
gain, phase, w = bode(P, logspace(-2, 2), Plot=False)