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_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 PDrootlocusPlot():
G = control.TransferFunction(L, (L, K2, -9.8 + K1))
rlist, klist = control.rlocus(G, grid=False)
plt.title("Root Locus diagram, K1=" + str(K1) + " and K2= " + str(K2))
plt.show()
def rlocus(name, sys, kvect, k=None):
if k is None:
k = kvect[-1]
sysc = control.feedback(sys * k, 1)
closed_loop_poles = control.pole(sysc)
res = control.rlocus(sys, kvect, Plot=False)
for locus in res[0].T:
plt.plot(np.real(locus), np.imag(locus))
p = control.pole(sys)
z = control.zero(sys)
marker_props = {
'markeredgecolor': 'r',
'markerfacecolor': 'none',
'markeredgewidth': 2,
'markersize': 10,
'linestyle': 'none'
}
plt.plot(np.real(p), np.imag(p), marker='x', label='pole', **marker_props)
plt.plot(np.real(z), np.imag(z), marker='o', label='zero', **marker_props)
plt.plot(np.real(closed_loop_poles),
np.imag(closed_loop_poles),
marker='s',
label='closed loop pole',
**marker_props)
plt.legend(loc='best')
plt.xlabel('real')
plt.ylabel('imag')
plt.grid(True)
plt.title(name + ' root locus')
def rootlocus(sys, kvect=None):
if kvect is None:
kvect = np.logspace(-3, 0, 1000)
rlist, klist = control.rlocus(sys, plot=False, kvect=kvect)
for root in rlist.T:
plt.plot(np.real(root), np.imag(root))
plt.plot(np.real(root[-1]), np.imag(root[-1]), 'bs')
for pole in control.pole(sys):
plt.plot(np.real(pole), np.imag(pole), 'rx')
for zero in control.zero(sys):
plt.plot(np.real(zero), np.imag(zero), 'go')
plt.grid()
plt.xlabel('real')
plt.ylabel('imag')
if __name__ == "__main__":
#open loop transfer function
num = [
1,
]
denum = [1, 3.50, 8]
k = 516.666
GH = k * transfer_function(num, denum)
#create a list of evenly spaced gains
gains = np.linspace(-0.5, 10.0, num=2000)
roots = compute_roots(GH, gains)
fig, ax = plot_root_locus(gains, roots)
# plot.show()
""" This is the end of the first code and beginning of the second
2nd code requires installation of control library. Must run Anaconda command
promt and enter this command:
conda install -c conda-forge control
Root locus function discription here
https://python-control.readthedocs.io/en/0.8.2
/generated/control.matlab.rlocus.html#control.matlab.rlocus
To set the
"""
G = k * cnt.tf(num, denum)
y = 5
x = -5, 2
cnt.rlocus(G, kvect=gains, xlim=x, ylim=[-y, y], grid=True)
t_inicial = 0
t_final = 10
passo_T = 0.001
T = 0.1
time_array = np.arange(t_inicial, t_final, passo_T)
tfs = control.TransferFunction([1500, 16000], [750, 1500, 16000])
time_array_disc = np.arange(t_inicial, t_final, T)
tfz = control.c2d(tfs, T)
print(tfz)
print('polos:', tfz.pole())
print()
roots, gains = control.rlocus(tfz,
xlim=[-20, 2],
kvect=np.arange(0, 20, 0.001),
ylim=[-5, 5],
Plot=False)
last_gain = None
for i in range(len(roots)):
gain = gains[i]
root1 = roots[i][0]
root2 = roots[i][1]
radius1 = getRootRadius(root1)
radius2 = getRootRadius(root2)
if radius1 <= 1 and radius2 <= 1:
last_gain = gain
else:
break
print('Hard tunning - last gain:', last_gain)
import control as co
import matplotlib.pyplot as plt
s = co.tf('s')
g1 = (s + 4) / ((s + 2) * (s + 1))
g2 = (s**2 + 3 * s + 4) / ((s + 2) * (s**2 + 1))
g3 = (2 * s + 1) / (s**2 + 4 * s + 8)
plt.figure(1)
g1_rlocus = co.rlocus(g1)
plt.xlim([-8, 0])
plt.figure(2)
g2_rlocus = co.rlocus(g2)
plt.xlim([-2, 1])
plt.figure(3)
g3_rlocus = co.rlocus(g3)
plt.xlim([-4, 0])
plt.show()
import numpy
import matplotlib
import control
from pylab import *
from numpy import *
from scipy import signal
G = control.TransferFunction([6], [1, 7, 6])
C1 = control.TransferFunction([1, 10], [1, 30.31])
K = 54.67
H1 = control.feedback(G * C1 * K)
C2 = control.TransferFunction([1, 0.13583], [1, 0.05])
H2 = control.feedback(G * C1 * C2 * K)
[rlist, klist] = control.rlocus(
H1, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True, PrintGain=True, grid=False)
# plt.show()
[rlist2, klist2] = control.rlocus(
H2, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True, PrintGain=True, grid=False)
# plt.show()
# Parte 2: Respuesta ante escalon
t = arange(0, 25, 0.01)
T1, yout1 = control.step_response(H1, t)
T2, yout2 = control.step_response(H2, t)
plot(T1, yout1, label='Sistema realimentado con C1')
plot(T2, yout2, label='Sistema realimentado con C1 y C2')
xlabel('Tiempo [s]')
ylabel('Salida')
title('Respuesta en el tiempo de una senal tipo escalon')
# plt.show()
# Tiempo establecimiento
return ca.Function('ss', [x, u, p], [A, B, C, D],
['x', 'u', 'p'], ['A', 'B', 'C', 'D'])
if __name__ == "__main__":
run()
code_generation()
x0, u0, p0 = do_trim(vt=100, gamma_deg=90, m_fuel=0.8)
lin = linearize()
import control
sys1 = control.ss(*lin(x0, u0, p0))
# get pitch rate from elevator
G = control.ss2tf(sys1[1, 2])
control.rlocus(G)
plt.show()
# next steps
# pitch rate pid design, use this to control pitch angle
# use flight path angle to control altitude
#print(x0, u0, p0)
#print(sys1)
# sys2 = do_trim(vt=50, gamma_deg=90, m_fuel=0.0)
# pid design to get gains, manually for now using root locus
# schedule gains
# gains = gain_schedule(gains1, gains2, t)
import numpy as np
import matplotlib.pyplot as plt
import control
#### Caso C1 #####
G = control.TransferFunction([3], [1, 7, 6])
C1 = control.TransferFunction([1, 10], [1, 30.31])
K = 109.34
H1 = control.feedback(G * C1 * K)
[rlist, klist] = control.rlocus(H1,
plotstr='-',
Plot=True,
PrintGain=True,
grid=False)
plt.show()
t = np.arange(0, 30, 0.05)
T1, yout1 = control.step_response(H1, t)
plt.plot(T1, yout1)
plt.xlabel('Tiempo [s]'), plt.ylabel('Salida')
plt.title('Respuesta a entrada en escalon - Compensador 1')
plt.show()
C2 = control.TransferFunction([1, 0.136], [1, 0.05])
H2 = control.feedback(G * C1 * C2 * K)
#[rlist2, klist2] = control.rlocus(
# H2, plotstr='-', Plot=True, PrintGain=True, grid=False)
#plt.show()
t = np.arange(0, 30, 0.05)
T2, yout2 = control.step_response(H2, t)
plt.plot(T2, yout2)
def rlocus(tf_list=[],
kvect=np.logspace(-2, 1.2, 1000),
xlim=None,
ylim=None,
show_grid=None):
"""Root locus plot
Calculate the root locus by finding the roots of 1+k*TF(s) where TF is self.num(s)/self.den(s) and each k is an element of kvect.
"""
xlabel = "Real Axis"
ylabel = "Imag Axis"
ylabel = "Magnitude (dB)"
hovertemplate = "<b>K</b>: %{text:.3f}<br><b>imag</b>: %{y:.3f}<br><b>real</b>: %{x:.3f}<br>m: %{customdata[0]:.3f}<br>wn: %{customdata[1]:.3f} rad/s"
data = []
xlim = []
ylim = []
for index, tf in enumerate(tf_list):
r_list, k_list = ctl.rlocus(tf,
kvect,
xlim=xlim,
ylim=ylim,
Plot=False)
#get ylim and xlim
xlim_max = np.max(np.real(np.ravel(r_list)))
xlim_min = np.min(np.real(np.ravel(r_list)))
ylim_max = np.max(np.imag(np.ravel(r_list)))
ylim_min = np.min(np.imag(np.ravel(r_list)))
xlim = 1.2 * np.array([xlim_min, xlim_max])
ylim = 1.2 * np.array([ylim_min, ylim_max])
tf_name = "tf {}".format(index + 1)
r_list = np.transpose(r_list)
first_point = dict(color='#555', width=1, dash="dot")
for index_r in range(len(r_list)):
r_temp = r_list[index_r]
#compute equivalent continuous m, wn
r_list_comp = r_list.astype(
complex
) # WTF: the python control "damp" function is buggy due to this missing cast !
r_list_continuous = np.log(r_list_comp) / tf.dt
wn_vect = np.abs(r_list_continuous)
m_vect = -np.real(r_list_continuous) / wn_vect
custom_data = np.dstack((m_vect, wn_vect))[0]
data.append({
"x": np.real(r_temp),
"y": np.imag(r_temp),
"name": tf_name,
"customdata": custom_data,
"hovertemplate": hovertemplate,
"text": k_list,
"showlegend": False
})
data.append({
"x": [np.real(r_temp[0])],
"y": [np.imag(r_temp[0])],
"line": first_point,
"mode": "markers",
"marker": {
"symbol": "x",
"size": 8
},
"showlegend": False
})
layout = default_layout("Real Axis", "Imag Axis", name=None)
fig = go.Figure(data, layout=layout)
return fig
num = [25.95]
den = [1,7.25,9.77]
numMpComp = [1,5.46]
denMpComp = [1,2.21]
numErrComp = [1,1]
denErrComp = [1,.00945]
sys = co.tf(num,den)
mpComp = 0.289*co.tf(numMpComp,denMpComp)
errComp = 1.1591*co.tf(numErrComp,denErrComp)
mpSys = mpComp*sys
errMpSys = errComp*mpSys
print('\n\nErro sem compesador:',1/(1+co.dcgain(mpSys)))
print('Erro com compensador:',1/(1+co.dcgain(errMpSys)))
co.rlocus(sys)
plt.title('LGR do sistema original')
plt.grid()
plt.figure(2)
co.rlocus(mpSys)
plt.title('LGR do sistema com overshoot compensado')
plt.grid()
plt.figure(3)
co.rlocus(errMpSys)
plt.title('LGR do sistema com overshoot e erro compensado')
plt.grid()
plt.show()
plt.title('Resposta ao impulso')
T = np.arange(0,50,.1)
Y,t = co.step_response(1,T)
# a
t_inicial=0
t_final=10
passo_T=0.001
T=0.1
time_array = np.arange(t_inicial, t_final,passo_T)
tfs = control.TransferFunction([.5],[.5,1,0])
time_array_disc = np.arange(t_inicial, t_final,T)
tfz = control.c2d(tfs, T)
print(tfz)
# b
roots,gains=control.rlocus(tfz,xlim=[-20,2], ylim=[-5,5])
plt.legend(loc='best')
plt.show()
# c
last_gain=None
for i in range(len(roots)):
gain=gains[i]
root1=roots[i][0]
root2=roots[i][1]
radius1=getRootRadius(root1)
radius2=getRootRadius(root2)
if radius1<=1 and radius2<=1:
last_gain=gain
else:
break
ABCDs.append([A,B,C,D])
sys.append(control.ss(A,B,C,D))
return sys
if __name__ == "__main__":
#run()
#code_generation()
plan_traj()
sys = plan_traj()
# get pitch rate from elevator
Gp1 = control.ss2tf(sys[0][1, 2])
plt.figure()
control.rlocus(Gp1)
plt.show()
Gp2 = control.ss2tf(sys[1][1,2])
plt.figure()
control.rlocus(Gp2)
plt.show()
# next steps
# pitch rate pid design, use this to control pitch angle
# use flight path angle to control altitude
#print(x0, u0, p0)
#print(sys1)
# sys2 = do_trim(vt=50, gamma_deg=90, m_fuel=0.0)
print("poles: ", G.pole())
print("zeros: ",G.zero())
number_asymptotes=len(G.pole())-len(G.zero())
print("number of asymptotes: ",number_asymptotes)
centroid=(G.pole().sum()-G.zero().sum())/number_asymptotes
print("centroid: ",centroid)
print("angle(s) of deperture:")
for i in range(number_asymptotes):
theta=(((2*i)+1)/number_asymptotes)*180
print(theta)
gm, pm, wg, wp = ctl.margin(G)
print("Gain margin:", gm)
print("Phase margin:", pm)
plt.figure()
ctl.rlocus(G)
plt.figure()
ctl.bode(G,dB=True)
plt.show()
plt.figure()
ctl.nyquist_plot(G)
plt.show()
import control as co
import numpy as np
import matplotlib.pyplot as plt
import sympy as sp
num = [-25.95]
den = [1, 7.25, 9.77]
sys = co.tf(num, den)
co.rlocus(sys)
plt.title('K->+inf')
plt.figure(2)
sys = co.tf(num, den)
co.rlocus(-sys)
plt.title('K->-inf')
plt.grid()
plt.show()
import numpy as np
from matplotlib import pyplot as plt
import control
from matplotlib.pyplot import style
# style.use('seaborn')
G = control.TransferFunction((25.05, 100.2), (1, 8, 16.05, 28.2))
rlist, klist = control.rlocus(G)
plt.annotate("Pole, P1", (-6.13, -2))
plt.annotate("Pole, P2", (-1, 3))
plt.annotate("Pole, P3", (-1, -3))
plt.annotate("Zero, Z1", (-4, 1))
plt.legend()
plt.show()
#import numpy as np
import matplotlib.pyplot as plt
import control as cnt
G1 = cnt.tf([1], [1, 4, 3, 0])
cnt.rlocus(G1, xlim=(-4, 1), ylim=(-3, 3))
plt.xlabel('real part of s')
plt.ylabel('imaginary part of s')
plt.grid()
plt.savefig('ee18btech11050.pdf')
plt.savefig('ee18btech11050.eps')
plt.show()
import numpy as np
import matplotlib.pyplot as plt
import control
#if using termux
import subprocess
import shlex
#end if
num = [6, 0] # Coefficients of Numerator of Tranfer Function
den = [1, 2, 5] # Coefficients of Denominator of Tranfer Function
G = control.TransferFunction(num, den) # Transfer Function
rlist, klist = control.rlocus(G) # Root Locus of Transfer Function
ans1 = np.roots(num) # Zeros of Transfer Fuunction
ans2 = np.roots(den) # Poles of Transfer Function
X1 = [x.real for x in ans1]
Y1 = [x.imag for x in ans1]
plt.scatter(X1, Y1, color='orange',
label='Zeros') # Plotting zeros of Transfer Function
X2 = [x.real for x in ans2]
Y2 = [x.imag for x in ans2]
plt.scatter(X2, Y2, marker='x', color='blue',
label='Poles') # Plotting poles of Transfer Function
plt.scatter(-np.sqrt(5), 0, color='red',
label='Breakaway Point') #Breakaway point
plt.xlabel('Real axis')
plt.ylabel('Imaginary axis')
plt.title('Root Locus Plot')
from matplotlib import pyplot as plt
def fEquation(roots):
x = symbols('x')
whole = 1
if len(roots) == 0:
return 0
for root in roots:
whole *= (x - root)
return whole.expand()
pole1 = complex(0, 0)
pole2 = complex(-25, 0)
pole3 = complex(-50, 10)
pole4 = complex(-50, -10)
print(pole3.real)
roots = [pole1, pole2, pole3, pole4]
denominator = fEquation(roots)
k = symbols('k')
equation = 1 / denominator
diffEquation = diff(equation)
breakAwayPoints = solve(diffEquation)
alist = Poly(denominator).all_coeffs()
G = co.TransferFunction(1, (1, 125, 5100, 65000, 0))
rlist, klist = co.rlocus(G)
plt.show()
import numpy as np import control import matplotlib.pyplot as plt # L = control.TransferFunction((1), (1, 6, 5, 0, 0)) # L = control.TransferFunction((1, 2), (1, 10, 0, 0)) L = control.TransferFunction((1, 1), (1, -3, 0)) rlist, klist = control.rlocus(L, kvect=np.linspace(100, -100, num=1000)) plt.show()
def main():
# construct gain scheduler object
gs = GainScheduler()
Kp_guess = 3
Ki_guess = 8
# Vars
gs.m.tf = Param(initialize=30)
gs.m.Kp = Var(initialize=Kp_guess, bounds=(1, 10))
gs.m.Ki = Var(initialize=Ki_guess, bounds=(0, 10))
gs.m.y1 = Var(gs.m.tau)
gs.m.y2 = Var(gs.m.tau)
gs.m.y3 = Var(gs.m.tau)
gs.m.u = Var(gs.m.tau)
gs.m.r = Var(gs.m.tau)
gs.m.J = Var(gs.m.tau)
gs.m.w180cross = Var(initialize=1, bounds=(0, 10))
gs.m.w0dBcross = Var(initialize=1.2, bounds=(0, 10))
# Derivative Vars
gs.m.dy1dt = DerivativeVar(gs.m.y1, wrt=gs.m.tau)
gs.m.dy2dt = DerivativeVar(gs.m.y2, wrt=gs.m.tau)
gs.m.dy3dt = DerivativeVar(gs.m.y3, wrt=gs.m.tau)
gs.m.dudt = DerivativeVar(gs.m.u, wrt=gs.m.tau)
gs.m.drdt = DerivativeVar(gs.m.r, wrt=gs.m.tau)
gs.m.dJdt = DerivativeVar(gs.m.J, wrt=gs.m.tau)
### Discretize problem (after m.tau and all variables are defined, before constraint defs defined)
discretizer = TransformationFactory('dae.collocation')
discretizer.apply_to(gs.m,
wrt=gs.m.tau,
nfe=75,
ncp=10,
scheme='LAGRANGE-RADAU')
#% Build model
# Phase and Gain crossing constraints
gs.m.w180crossConstraint = Constraint(rule=gs._w180cross)
gs.m.w0dBcrossConstraint = Constraint(rule=gs._w0dBcross)
# Standard constraints
gs.m.bcConstraint = ConstraintList(rule=gs.BCs)
gs.m.y1dotConstraint = Constraint(gs.m.tau, rule=gs._y1dot)
gs.m.y2dotConstraint = Constraint(gs.m.tau, rule=gs._y2dot)
gs.m.y3dotConstraint = Constraint(gs.m.tau, rule=gs._y3dot)
gs.m.udotConstraint = Constraint(gs.m.tau, rule=gs._udot)
gs.m.rdotConstraint = Constraint(gs.m.tau, rule=gs._rdot)
# Margin constraints
gs.m.gainMargindBConstraint = Constraint(rule=gs._gainMargindB)
gs.m.phaseMarginConstraint = Constraint(rule=gs._phaseMargin)
gs.m.JdotConstraint = Constraint(gs.m.tau, rule=gs._Jdot)
gs.m.obj = Objective(expr=gs.m.J[1], sense=minimize)
solver = SolverFactory('ipopt')
solver.options["halt_on_ampl_error"] = "yes"
solver.options["tol"] = 1e-6
solver.options["max_iter"] = 2000
solver.options["linear_solver"] = "ma27"
solver.options["ma27_pivtol"] = 1e-6
#solver.options["ma27_liw_init_factor"] = 5
solver.options["linear_scaling_on_demand"] = "yes"
solver.options["mu_init"] = 1
solver.solve(gs.m, tee=True)
results_dict = {}
var_list = [gs.m.Kp, gs.m.Ki]
for var in var_list:
print("Saving Variable: ", var) # doctest: +SKIP
results_dict[var.name] = [value(var[index]) for index in var]
#%% Plotting
mpl.rcParams['font.size'] = 14
mpl.rcParams['legend.fontsize'] = 14
mpl.rcParams["font.family"] = "Times New Roman"
csfont = {'fontname': 'Times New Roman'}
plt.rc('text', usetex=True)
plt.rc('font', family='serif')
#plt.close('all')
blue = '#1f77b4ff'
orange = '#ff7f0eff'
green = '#2ca02cff'
time = np.dot(gs.m.tau, gs.m.tf())
y1 = [gs.m.y1[t]() for t in gs.m.tau]
r = [gs.m.r[t]() for t in gs.m.tau]
plt.figure(1)
plt.plot(time, y1, '.-')
plt.plot(time, r, 'k')
plt.xlabel('t [s]')
plt.ylabel('y')
plt.grid()
plt.title([
'Kp:%6.2f' % gs.m.Kp(),
'Ki:%6.2f' % gs.m.Ki(),
'J = %6.2f' % gs.m.J[1]()
])
print('Kp = ', gs.m.Kp())
print('Ki = ', gs.m.Ki())
sysC = control.tf([gs.m.Kp(), gs.m.Ki()], [1, 0])
sysG = control.tf([1], [1, 4, 8, 8])
sys = sysC * sysG
rlist, klist = control.rlocus(sys, kvect=np.logspace(-10, 1, 500))
sysC = control.tf([gs.m.Kp(), gs.m.Ki()], [1, 0])
sysG = control.tf([1], [1, 4, 8, 8])
sys = sysC * sysG
r, k = control.rlocus(sys, [1])
plt.close()
#fig,ax = plt.subplots()
plt.scatter(r.real, r.imag, color='k')
sysC = control.tf([gs.m.Kp(), gs.m.Ki()], [1, 0])
sysG = control.tf([1], [1, 4, 8, 8])
sys = sysC * sysG
plt.figure()
mag, phase, omega = control.bode(sys, dB=True, margins=True)
#%% Check gain/phase
#K = m.K()
#s = complex(real=0, imag=m.w180cross())
#CG = (s+1)/(s*(s+2)*(s**2+2*s+4))
#L = K*CG
#magL = sqrt(L.real**2+L.imag**2)
#angleL = -180/pi*np.arctan2(L.imag,L.real)
w = sympy.Symbol('w', real=True)
s = sympy.I * w
C = gs.m.Kp() + gs.m.Ki() / s
G = 1 / ((s + 2) * (s**2 + 2 * s + 4))
L = C * G
real, imag = L.as_real_imag()
phase = -180 / pi * sympy.atan2(imag, real)
mag = sympy.sqrt(real**2 + imag**2)
_phase = sympy.lambdify((w), phase)
_mag = sympy.lambdify((w), mag)
print('w180cross = ', gs.m.w180cross(), 'rad/s')
print('w0dBcross = ', gs.m.w0dBcross(), 'rad/s')
print('magLdB at w0dBcross = ', 20 * log10(_mag(gs.m.w0dBcross())))
print('angleL at w180cross = ', _phase(gs.m.w180cross()))
return
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()
def rlocusfind(system_or_tf, *args, **kwargs):
kwargs["Plot"] = False
initial_pick = None
if "initial_pick" in kwargs:
initial_pick = kwargs["initial_pick"]
del kwargs["initial_pick"]
plot_str = kwargs["plotstr"] if "plotstr" in kwargs else "-"
rl, gains = control.rlocus(system_or_tf, *args, **kwargs)
x = numpy.concatenate(numpy.real(rl.T))
y = numpy.concatenate(numpy.imag(rl.T))
#loci_points = numpy.squeeze(numpy.dstack( (x, y) ))
#lu_tree = scipy.spatial.KDTree(loci_points, 5)
fig = plt.figure()
ax = fig.add_subplot(1,1,1)
def rlocus_result_at(pole_x, pole_y):
pole = complex(pole_x, pole_y)
gain = gain_at(system_or_tf, pole)
damping_coefficient = damping_coefficient_at(pole)
overshoot = overshoot_from_damping_coefficient(damping_coefficient)
natural_frequency = undamped_natural_frequency_at(pole)
return RootLocusResult(
pole, gain, damping_coefficient, overshoot, natural_frequency
)
def pick_handler(event):
try:
mouse_event = event.mouseevent
locus_line = event.artist
#dist, point_index = lu_tree.query(
# (mouse_event.xdata, mouse_event.ydata)
#)
pole_x = mouse_event.xdata #x[point_index]
pole_y = mouse_event.ydata #y[point_index]
marker.set_xdata([pole_x])
marker.set_ydata([pole_y])
fig.canvas.draw()
res = rlocus_result_at(pole_x, pole_y)
print "-"*25
print "System:\n{0!s}".format(system_or_tf)
print "Gain:", res.gain
print "Pole: {0!s}".format(res.pole)
print "Damping:", res.damping_coefficient
print "Overshoot (%):", res.overshoot
print "Frequency (rad/s):", res.frequency
print "-"*25
except Exception as e:
print "Error: {0!s}".format(e)
sys.stdout.flush()
if not initial_pick:
rlocus_result = None
marker, = ax.plot([x[0]], [y[0]], "ko")
else:
initial_x, initial_y = initial_pick
rlocus_result = rlocus_result_at(initial_x, initial_y)
marker, = ax.plot([initial_x], [initial_y], "ko")
SynthesizedEvent = collections.namedtuple(
"SynthesizedEvent", ["mouseevent", "artist"]
)
artificial_pick_event = SynthesizedEvent( initial_pick, None )
for col in rl.T:
ax.plot(numpy.real(col), numpy.imag(col), plot_str, picker=3)
ax.grid(True)
ax.set_xlabel('Real')
ax.set_ylabel('Imaginary')
fig.canvas.mpl_connect("pick_event", pick_handler)
return (fig, rlocus_result)
xe2 = v ue = n A11 = np.sin(xe1) * xe2 / g # Upper left A12 = (-np.cos(xe1) / g) - (ue * g / np.power(xe2, 2)) # Upper right A21 = -np.cos(xe1) / g # Lower left A22 = (2 * k1 * np.power(ue, 2) * g / np.power(xe2, 3)) - (2 * k2 * xe2 * g) B1 = g / xe2 B2 = -2 * k1 * ue * g / np.power(xe2, 2) X1_over_U_numerator = [B1, (A12 * B2 - A22 * B1)] X1_over_U_denominator = [1, -(A22 + A11), A22 * A11 - A12 * A21] H1 = cnt.tf(X1_over_U_numerator, X1_over_U_denominator) X2_over_U_numerator = [B2, (A21 * B1 - A11 * B2)] X2_over_U_denominator = [1, -(A22 + A11), A22 * A11 - A12 * A21] H2 = cnt.tf(X2_over_U_numerator, X2_over_U_denominator) A = 1 B = -(A22 + A11) C = A22 * A11 - A12 * A21 print((-B + np.sqrt(B**2 - 4*A*C)) / 2*A) print((-B - np.sqrt(B**2 - 4*A*C)) / 2*A) cnt.rlocus(H1 / (1 + H1)); plt.show() cnt.rlocus(H2 / (1 + H2)); plt.show()
