if False:
_ = control.bode_plot(sysTaLinUnc[0, 0],
omega_limits=[0.01, 500],
Hz=True,
dB=True)
_ = control.bode_plot(sysTaLinUnc[1, 1],
omega_limits=[0.01, 500],
Hz=True,
dB=True)
_ = control.bode_plot(sysTaLinUnc[2, 2],
omega_limits=[0.01, 500],
Hz=True,
dB=True)
# Linear System Response
gainLaLinNom_mag, phaseLaLinNom_rad, _ = control.freqresp(sysLaLinNom,
omega=freqLin_rps)
LaLinNom = gainLaLinNom_mag * np.exp(1j * phaseLaLinNom_rad)
gainLaLinUnc_mag, phaseLaLinUnc_rad, _ = control.freqresp(sysLaLinUnc,
omega=freqLin_rps)
LaLinUnc = gainLaLinUnc_mag * np.exp(1j * phaseLaLinUnc_rad)
gainTaLinNom_mag, phaseTaLinNom_rad, _ = control.freqresp(sysTaLinNom,
omega=freqLin_rps)
TaLinNom = gainTaLinNom_mag * np.exp(1j * phaseTaLinNom_rad)
gainTaLinUnc_mag, phaseTaLinUnc_rad, _ = control.freqresp(sysTaLinUnc,
omega=freqLin_rps)
TaLinUnc = gainTaLinUnc_mag * np.exp(1j * phaseTaLinUnc_rad)
#%% Excitation
def test_freqresp_siso(ss_siso):
"""Test SISO frequency response"""
omega = np.linspace(10e-2, 10e2, 1000)
# test frequency response
ctrl.freqresp(ss_siso, omega)
def test_freqresp_mimo(ss_mimo):
"""Test MIMO frequency response calls"""
omega = np.linspace(10e-2, 10e2, 1000)
ctrl.freqresp(ss_mimo, omega)
tf_mimo = tf(ss_mimo)
ctrl.freqresp(tf_mimo, omega)
#
for ax in plt.gcf().axes:
if ax.get_label() == 'control-bode-magnitude':
break
ax.semilogx([1e-4, 1e3], 20 * np.log10([1, 1]), 'k-')
#
# Replot phase starting at -90 degrees
#
# Get the phase plot axes
for ax in plt.gcf().axes:
if ax.get_label() == 'control-bode-phase':
break
# Recreate the frequency response and shift the phase
mag, phase, w = ct.freqresp(L, np.logspace(-4, 3))
phase = phase - 360
# Replot the phase by hand
ax.semilogx([1e-4, 1e3], [-180, -180], 'k-')
ax.semilogx(w, np.squeeze(phase), 'b-')
ax.axis([1e-4, 1e3, -360, 0])
plt.xlabel('Frequency [deg]')
plt.ylabel('Phase [deg]')
# plt.set(gca, 'YTick', [-360, -270, -180, -90, 0])
# plt.set(gca, 'XTick', [10^-4, 10^-2, 1, 100])
#
# Nyquist plot for complete design
#
plt.figure(7)
def FreqResp(sys, freq_rps):
gain_mag, phase_rad, _ = control.freqresp(sys, omega=freq_rps)
T = gain_mag * np.exp(1j * phase_rad)
return T
t_cd, im_resp = con.impulse_response(sys_und_tst)
plt.figure()
plt.plot(t_cd, im_resp, '-o')
plt.title("Impulse Response")
plt.xlabel("Time [s]")
plt.ylabel("Magnitude")
plt.grid()
# plot frequency response log scale
if sys_und_tst.isdtime():
w = 2 * np.pi * np.logspace(-3, np.log10(0.5), nsamp_single)
else:
w = 2 * np.pi * np.logspace(-3, np.log10(fstop_c), nsamp_single)
mag, phase, w = con.freqresp(sys_und_tst, w)
# freq response returns mag and phase as [[[mag]]], [[[phase]]]
# squeeze reduces this to a one dimensional array, optionally can use mag[0][0]
mag = np.squeeze(mag)
phase = np.squeeze(phase)
plt.figure()
plt.subplot(2, 1, 1)
plt.semilogx(w / (2 * np.pi), hf.db(mag))
plt.grid()
plt.xlabel('Frequency [Hz]')
plt.ylabel('Magnitude [dB]')
plt.title('Frequency Response')
plt.subplot(2, 1, 2)
plt.semilogx(w / (2 * np.pi), np.unwrap(phase) * 180 / np.pi)
plt.grid()
# plt.xlabel("Time [s]")
# plt.ylabel("Magnitude")
# plt.grid()
# plot step response
tc_s, youtc_s = con.step_response(sysc)
# plt.figure()
# plt.plot(tc_s, youtc_s)
# plt.title("Step Response - Continuous System")
# plt.xlabel("Time [s]")
# plt.ylabel("Magnitude")
# plt.grid()
# plot frequency response
w = 2 * np.pi * np.logspace(-3, np.log10(0.5 * Fs), 100)
magc, phasec, w = con.freqresp(sysc, w)
# freq response returns mag and phase as [[[mag]]], [[[phase]]]
# squeeze reduces this to a one dimensional array, optionally can use mag[0][0]
magc = np.squeeze(magc)
phasec = np.squeeze(phasec)
# plt.figure()
# plt.semilogx(w / (2 * np.pi), hf.db(magc))
# plt.grid()
# plt.xlabel('Frequency [Hz]')
# plt.ylabel('Magnitude [dB]')
# plt.title('Frequency Response - Continuous System')
#######################################################################################
# # doing impulse invariance the long way...
# # partial fraction expansion
# [r, p, k] = sig.residue([1], [1, 2, 2, 0])
sysLoopIn = ConnectName([sysPlantDist, sysCtrl], connectNames, inKeep, outKeep)
# Closed-Loop (connect [uCntrl])
connectNames = sysLoopIn.OutputName[0:2]
inKeep = sysLoopIn.InputName[:2] + sysLoopIn.InputName[4:]
outKeep = sysLoopIn.OutputName
sysCL = ConnectName([sysLoopIn], connectNames, inKeep, outKeep)
# Loop Li - (uExc -> uCtrl)
sysLi = -sysLoopIn[
0:2, 4:
6] # Li = KG, u_e to u # FIXIT - YES Negative : sysLi.dcgain() = array([[0.5, 0.25], [0.125, 0.5]])
sysLi.InputName = sysLoopIn.InputName[4:6]
sysLi.OutputName = sysLoopIn.OutputName[0:2]
gainLiLinNom_mag, phaseLiLinNom_rad, _ = control.freqresp(sysLi,
omega=freqLin_rps)
LiLinNom = gainLiLinNom_mag * np.exp(1j * phaseLiLinNom_rad)
gainLiLinNom_dB = 20 * np.log10(gainLiLinNom_mag)
phaseLiLinNom_deg = np.unwrap(phaseLiLinNom_rad) * rad2deg
# Loop Ti - (uExc -> uCtrl)
sysTi = -sysCL[
0:2, 2:
4] # FIXIT - YES Negative : sysTi.dcgain() = array([[0.32394366, 0.11267606], [0.05633803, 0.32394366]])
sysTi.InputName = sysCL.InputName[2:4]
sysTi.OutputName = sysCL.OutputName[0:2]
gainTiLinNom_mag, phaseTiLinNom_rad, _ = control.freqresp(sysTi,
omega=freqLin_rps)
TiLinNom = gainTiLinNom_mag * np.exp(1j * phaseTiLinNom_rad)
gainTiLinNom_dB = 20 * np.log10(gainTiLinNom_mag)
]
outKeep = [
outName[i - 1] for i in [8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20]
]
sysCL = Systems.ConnectName([sysCtrl, sysPlant], connectName, inKeep, outKeep)
# Look at only the in-out of the OL
inList = [sysOL.InputName.index(s) for s in ['cmdP', 'cmdQ', 'cmdR']]
outList = [sysOL.OutputName.index(s) for s in ['fbP', 'fbQ', 'fbR']]
sysSimOL = sysOL[outList, :]
sysSimOL = sysOL[:, inList]
# Linear System Response
gainLin_mag, phaseLin_rad, _ = control.freqresp(sysSimOL, omega=freqLin_rps)
TxyLin = gainLin_mag * np.exp(1j * phaseLin_rad)
gainLin_dB = 20 * np.log10(gainLin_mag)
phaseLin_deg = np.unwrap(phaseLin_rad) * rad2deg
rCritLin_mag = np.abs(TxyLin - (-1 + 0j))
sigmaLin_mag, _ = FreqTrans.Sigma(TxyLin)
#%% Excitation
numExc = 3
numCycles = 1
ampInit = 4.0 * deg2rad
ampFinal = ampInit
freqMinDes_rps = 0.1 * hz2rps * np.ones(numExc)
#y_a = stepHeight * (1 + (np.exp(-zeta*omega_n*t)/np.sqrt(1-zeta**2))*(2*zeta*np.sin(omega_d*t) - np.sin(omega_d*t+np.arccos(zeta))));
y_a = stepHeight * (1 + (np.exp(-zeta * omega_n * t) * omega_n / omega_d) *
(2 * zeta * np.sin(omega_d * t) -
np.sin(omega_d * t + np.arccos(zeta))))
fileName = 'Step'
elif signalType == 3: # Ramp input
u = np.zeros(N)
slope = 1
u = slope * (t - dt * startIndex)
u[0:startIndex] = 0
y_a = slope * (t - np.exp(-zeta * omega_n * t) *
(1 / omega_d) * np.sin(omega_d * t))
fileName = 'Ramp'
elif signalType == 4: # Harmonic input
u = np.sin(omega_n * t)
[[[mag]]], phase, omega = control.freqresp(sys, [omega_n])
print('magnitude at omega_n =', mag)
control.damp(sys, doprint=True)
A = -mag * (np.exp(-c * t / (2 * m)) - 1)
# Calculate amplitude
#y_a = omega_n**2*((np.sqrt((zeta**2-1)*omega_n**2)*(zeta*omega_n*np.exp(t*(-np.sqrt((zeta**2-1)*omega_n**2)-zeta*omega_n))-zeta*omega_n*np.exp(t*(np.sqrt((zeta**2-1)*omega_n**2)-zeta*omega_n))+np.sqrt((zeta**2-1)*omega_n**2)*np.exp(t*(-np.sqrt((zeta**2-1)*omega_n**2)-zeta*omega_n))+np.sqrt((zeta**2-1)*omega_n**2)*np.exp(t*(np.sqrt((zeta**2-1)*omega_n**2)-zeta*omega_n))))/(4*zeta*(zeta**2-1)*omega_n**4)+(2*zeta*np.sin(t*omega_n)-np.cos(t*omega_n))/(2*zeta*omega_n**2));
fileName = 'Harmonic'
elif signalType == 5:
u = np.random.normal(0, 1, N)
# White noise
fileName = 'Stochastic'
print('Selected input: ' + fileName)
print('')
print('------------------------------------------------------')
