def closeLoop(tf,k_p,h_multiplier=1):
H=control.TransferFunction([h_multiplier],[1])
H=control.tf2io(H)
H.name='H'
H.set_inputs(['in'])
H.set_outputs(['out'])
D=control.TransferFunction([k_p],[1])
D=control.tf2io(D)
D.name='D'
D.set_inputs(['in'])
D.set_outputs(['out'])
system=control.tf2io(tf)
system.name='system'
system.set_inputs(['in'])
system.set_outputs(['out'])
# +
# in ---->o---->--D-->system----> out
# |- |
# -------H------------
system_MF = control.InterconnectedSystem([H,D,system] ,name='system_MF',
connections=[
['H.in','system.out'],
['D.in','-H.out'],
['system.in','D.out'],
],
inplist=['D.in'],
inputs=['in'],
outlist=['system.out','D.out','-H.out'],
outputs=['out','control','error'])
return system_MF
def init_system():
Qp = 400.0
Cp = 10E-9
Lp = 25.7E-6
Rp = (1.0 / Qp) * math.sqrt(Lp / Cp)
K = 0.4
Qs = 50.0
Ls = 14E-3
Cs = 15.8E-12
Rs = 1.0 / Qs * math.sqrt(Ls / Cs)
Lm = K * math.sqrt(Lp * Ls)
#three branches, Z1, Z2, Z3
Branch1 = control.TransferFunction([Cp * (Lp - Lm), Cp * Rp, 1.0],
[Cp, 0.0])
Branch2 = control.TransferFunction([Cs * (Ls - Lm), Cs * Rs, 1.0],
[Cs, 0.0])
Branch3 = control.TransferFunction([Lm, 0.0], [1.0])
BranchParallel = ((Branch3**-1) + Branch2**-1)**-1
System = (BranchParallel + Branch1)**-1
sys = System.returnScipySignalLTI()[0][0]
return sys
def main():
dt = 0.1
T = np.arange(0, 6, dt)
plt.figure(1)
plt.xticks(np.arange(min(T), max(T) + 1, 1.0))
plt.xlabel("Time (s)")
plt.ylabel("Step response")
tf = ct.TransferFunction(1, [1, -0.6], dt)
sim(tf, T, "Single pole in RHP")
tf = ct.TransferFunction(1, [1, 0.6], dt)
sim(tf, T, "Single pole in LHP")
if "--noninteractive" in sys.argv:
latex.savefig("z_oscillations_1p")
plt.figure(2)
plt.xlabel("Time (s)")
plt.ylabel("Step response")
den = [np.real(x) for x in conv([1, 0.6 + 0.6j], [1, 0.6 - 0.6j])]
tf = ct.TransferFunction(1, den, dt)
sim(tf, T, "Complex conjugate poles in LHP")
den = [np.real(x) for x in conv([1, -0.6 + 0.6j], [1, -0.6 - 0.6j])]
tf = ct.TransferFunction(1, den, dt)
sim(tf, T, "Complex conjugate poles in RHP")
if "--noninteractive" in sys.argv:
latex.savefig("z_oscillations_2p")
else:
plt.show()
def zero_bins_to_num(zeros):
exponents = [None, -2, -1, 0, 1]# powers of 10 corresponding to each frequency bin
if zeros[0] == 1:
G = control.TransferFunction([1,0],1)
elif zeros[0] == 2:
G = control.TransferFunction([1,0,0],1)
else:
G = 1
for z_i, exp_i in zip(zeros[1:], exponents[1:]):
if z_i == 0:
# skip
continue
freq_i = random_log_freq(exp_i)
w_i = 2.0*np.pi*freq_i
if z_i == 1:
G_i = control.TransferFunction([1,w_i],1)
elif z_i == 2:
z_i = 0.8*rand()
G_i = control.TransferFunction([1,2*z_i*w_i,w_i**2],1)
G *= G_i
if G == 1:
# This is the default value if zeros is a list of
# all zeros: [0,0,0,...,0]
return G
else:
return np.squeeze(G.num)
def main():
dt = 0.0001
T = np.arange(0, 0.25, dt)
# Make plant
J = 3.2284e-6 # kg-m^2
b = 3.5077e-6 # N-m-s
Ke = 0.0181 # V/rad/s
Kt = 0.0181 # N-m/Amp
K = Ke # Ke = Kt
R = 0.0902 # Ohms
L = 230e-6 # H
# Stable plant (L = 0)
# s((Js + b)R + K^2)
# s(JRs + bR + K^2)
# JRs^2 + bRs + K^2s
# JRs^2 + (bR + K^2)s
G = ct.TransferFunction(K, [J * R, b * R + K**2, 0])
ct.root_locus(G, grid=True)
plt.xlabel("Real Axis (seconds$^{-1}$)")
plt.ylabel("Imaginary Axis (seconds$^{-1}$)")
if "--noninteractive" in sys.argv:
latex.savefig("highfreq_stable_rlocus")
plt.figure(2)
plt.xlabel("Time ($s$)")
plt.ylabel("Position ($m$)")
sim(ct.TransferFunction(1, 1), T, "Reference")
Gcl = make_closed_loop_plant(G, 1)
sim(Gcl, T, "Step response")
if "--noninteractive" in sys.argv:
latex.savefig("highfreq_stable_step")
else:
plt.show()
def pole_bins_to_den(poles):
# powers of 10 corresponding to each frequency bin
exponents = [None, -2, -1, 0, 1]
if poles[0] == 1:
G = control.TransferFunction(1,[1,0])
elif poles[0] == 2:
G = control.TransferFunction(1,[1,0,0])
else:
G = 1
for p_i, exp_i in zip(poles[1:], exponents[1:]):
if p_i == 0:
# skip
continue
freq_i = random_log_freq(exp_i)
w_i = 2.0*np.pi*freq_i
if p_i == 1:
G_i = control.TransferFunction(1,[1,w_i])
elif p_i == 2:
z_i = 0.8*rand()
G_i = control.TransferFunction(1,[1,2*z_i*w_i,w_i**2])
G *= G_i
return np.squeeze(G.den)
def main():
dt = 0.0001
T = np.arange(0, 0.25, dt)
# Make plant
J = 3.2284e-6 # kg-m^2
b = 3.5077e-6 # N-m-s
Ke = 0.0274 # V/rad/s
Kt = 0.0274 # N-m/Amp
K = Ke # Ke = Kt
R = 4 # Ohms
L = 2.75e-6 # H
# Unstable plant
# s((Js + b)(Ls + R) + K^2)
# s(JLs^2 + JRs + bLs + bR + K^2)
# JLs^3 + JRs^2 + bLs^2 + bRs + K^2s
# JLs^3 + (JR + bL)s^2 + (bR + K^2)s
G = cnt.TransferFunction(K, [J * L, J * R + b * L, b * R + K**2, 0])
cnt.root_locus(G, grid=True)
plt.xlabel("Real Axis (seconds$^{-1}$)")
plt.ylabel("Imaginary Axis (seconds$^{-1}$)")
if "--noninteractive" in sys.argv:
latexutils.savefig("highfreq_unstable_rlocus")
plt.figure(2)
plt.xlabel("Time ($s$)")
plt.ylabel("Position ($m$)")
sim(cnt.TransferFunction(1, 1), T, "Reference")
Gcl = make_closed_loop_plant(G, 3)
sim(Gcl, T, "Step response")
if "--noninteractive" in sys.argv:
latexutils.savefig("highfreq_unstable_step")
else:
plt.show()
def main():
dt = 0.0001
T = np.arange(0, 6, dt)
plt.xlabel("Time ($s$)")
plt.ylabel("Position ($m$)")
# Make plant
G = ct.TransferFunction(1, conv([1, 5], [1, 0]))
sim(ct.TransferFunction(1, 1), T, "Setpoint")
K = ct.TransferFunction(120, 1)
Gcl = ct.feedback(G, K)
sim(Gcl, T, "Underdamped")
K = ct.TransferFunction(3, 1)
Gcl = ct.feedback(G, K)
sim(Gcl, T, "Overdamped")
K = ct.TransferFunction(6.268, 1)
Gcl = ct.feedback(G, K)
sim(Gcl, T, "Critically damped")
if "--noninteractive" in sys.argv:
latex.savefig("pid_responses")
else:
plt.show()
def make_statespace(self):
jm = self.parameters["DC-Motor"]["Jm"]
bm = self.parameters["DC-Motor"]["Bm"]
kme = self.parameters["DC-Motor"]["Kme"]
kmt = self.parameters["DC-Motor"]["Kmt"]
rm = self.parameters["DC-Motor"]["Rm"]
lm = self.parameters["DC-Motor"]["Lm"]
kdm = self.parameters["DC-Motor"]["Kdm"]
kpm = self.parameters["DC-Motor"]["Kpm"]
kim = self.parameters["DC-Motor"]["Kim"]
nm = self.parameters["DC-Motor"]["Nm"]
dc = control.TransferFunction(
[0, kmt], [jm * lm, bm * lm + jm * rm, bm * rm + kme * kmt])
pidm = control.TransferFunction(
[kpm + kdm * nm, kpm * nm + kim, kim * nm], [1, nm, 0])
ii = control.TransferFunction([1], [1, 0, 0])
agv = ii * control.feedback(dc * pidm, sign=-1)
# Laplace --> Z
agvz = control.sample_system(agv, lib.pt, method='zoh')
# Transferfunction --> StateSpace
ss = control.tf2ss(agvz)
lib.set_statespace(ss)
def pid(kp, ki, kd):
"""
:param kp:
:param ki:
:param kd:
:return:
"""
diff = ctrl.TransferFunction([1, 0], 1)
intgr = ctrl.TransferFunction(1, [1, 0])
pid_tf = kp + kd * diff + ki * intgr
return pid_tf
def sys_dict():
sdict = {}
sdict['ss'] = ct.StateSpace([[-1]], [[1]], [[1]], [[0]])
sdict['tf'] = ct.TransferFunction([1], [0.5, 1])
sdict['tfx'] = ct.TransferFunction([1, 1], [1]) # non-proper TF
sdict['frd'] = ct.frd([10 + 0j, 9 + 1j, 8 + 2j, 7 + 3j], [1, 2, 3, 4])
sdict['lio'] = ct.LinearIOSystem(ct.ss([[-1]], [[5]], [[5]], [[0]]))
sdict['ios'] = ct.NonlinearIOSystem(sdict['lio']._rhs,
sdict['lio']._out,
inputs=1,
outputs=1,
states=1)
sdict['arr'] = np.array([[2.0]])
sdict['flt'] = 3.
return sdict
def pid_by_frequency(g, po, ts, err_step=None, err_ramp=None, err_para=None):
s = ct.TransferFunction([1, 0], [1])
ki, ess = ss_error(g/s, err_step, err_ramp, err_para)
if abs(ess) == np.inf or abs(ess) == np.nan:
ess = None
if ess is None or ki is None:
assert "Inconsistent system"
ki = ki / np.real(ess)
print("ki=", ki)
log_po = np.log(100 / po)
psi = log_po / np.sqrt(np.pi ** 2 + log_po ** 2)
wn = 4 / psi / ts
print("wn=", wn, "psi=", psi)
pm = 100 * psi
wcp = wn
p_cut = ct.evalfr(g, wcp * 1j)
p_cut_mag = np.abs(p_cut)
p_cut_angle = np.angle(p_cut)
controller_mag = 1 / p_cut_mag
controller_angle = -np.pi + pm * np.pi / 180 - p_cut_angle
kp = controller_mag * np.cos(controller_angle)
kd = (controller_mag * np.sin(controller_angle) + ki / wcp) / wcp
print(f"pm= {pm}, ki= {ki}, kp= {kp}, kd= {kd}")
controller = kp + kd * s + ki / s
return controller
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 random_Bode_TF():
plist = assign_poles_to_bins()
zlist = assign_zeros_to_bins(plist)
den = pole_bins_to_den(plist)
num = zero_bins_to_num(zlist)
G = control.TransferFunction(num, den)
return G
def symbolic_transfer_function(
eq: Union[sp.Expr, int, float]) -> ct.TransferFunction:
"""
Transform a symbolic equation to a transfer function (sympy -> control)
:param eq: your symbolic sympy based equation
:return: a control Transfer Function class
"""
s = sp.var('s')
try:
if eq.is_real:
pass
except:
if not isinstance(eq, float) and not isinstance(eq, int):
used_symbols = [str(sym) for sym in eq.free_symbols]
if not len(used_symbols) == 1 or "s" not in used_symbols:
raise Exception(
"invalid equation, please use correct transfer function equation (e.g. 1/(s**2+3))"
)
n, d = sp.fraction(sp.factor(eq))
num = sp.Poly(sp.expand(n), s).all_coeffs()
den = sp.Poly(sp.expand(d), s).all_coeffs()
num: [float] = [float(v) for v in num]
den: [float] = [float(v) for v in den]
return ct.TransferFunction(num, den)
def feedback(tau: float, fb_gain: float) -> None:
"""Simulation of a negative feedback system, using the package 'control'.
'time', 'in_signal', 'num' and 'den' are taken from the global workspace
Parameters
----------
tau : time constant of a first order lag [sec]
fb_gain : feedback gain
"""
# First, define the feedforward transfer function
sys = control.TransferFunction(num, den)
print(sys) # 1 / (tau*s + 1)
# Simulate the response of the feedforward system
t_out, y_out, x_out = control.forced_response(sys, T=time, U=in_signal)
# Then define a feedback-loop, with gain fb_gain
sys_total = control.feedback(sys, fb_gain)
print(sys_total) # 1 / (tau*s + (1+k))
# Simulate the response of the feedback system
t_total, y_total, x_total = control.forced_response(sys_total,
T=time, U=in_signal)
# Show the signals
plt.plot(time, in_signal, '-', label='Input')
plt.plot(t_out, y_out, '--', label='Feedforward')
plt.plot(t_total, y_total, '-.', label='Feedback')
# Format the plot
plt.xlabel('Time [sec]')
plt.title(f"First order lag (tau={tau} sec)")
plt.text(15, 0.8, "Simulated with 'control' ", fontsize=12)
plt.legend()
def pid(kp, ki, kd):
"""
This function constructs a PID controller; returning its associated transfer function, based upon
the provided values for its gain components.
:param kp: The proportional gain value.
:param ki: The integral gain value.
:param kd: The differential gain value.
:return: The PID's transfer function based on the gain values provided.
"""
diff = ctrl.TransferFunction(
[1, 0], 1) # Defines the differential's transfer function.
intgr = ctrl.TransferFunction(
1, [1, 0]) # Defines the integral's transfer function.
pid_tf = kp + (kd * diff) + (
ki * intgr) # Determines the PID's overall transfer function.
return pid_tf
def plot(self):
G = control.TransferFunction((1, 1.5), (1, 11, 10, 0))
mag, phase, omega = control.bode(G)
plt.title('Bode Plots', y= 2.20)
plt.gcf().canvas.set_window_title('Bode Plots')
plt.grid()
plt.show()
def plot(self):
G = control.TransferFunction((1, 1.5), (1, 11, 10, 0))
nichols(G)
plt.title('Nichols Plot')
plt.gcf().canvas.set_window_title('Nichols Plot')
plt.grid()
plt.show()
def plotPolesAndZeros(tf):
num = [tf[0], tf[1], tf[2]]
den = [tf[3], tf[4], tf[5]]
sistema = cl.TransferFunction(num, den)
cl.pzmap(sistema, Plot=True)
plt.show()
return
def runIdentification(self):
n_steps = len(self.t)
n = self.sys_id_n_poles # order of the denominator (a_1,...,a_n)
m = self.sys_id_n_zeros # order of the numerator (b_0,...,b_m)
d = self.sys_id_delays # number of delays
tau = 60.0 # forgetting period
lbda = 1.0 - self.dt / tau
self.sysid = SystemIdentification(n, m, d)
self.sysid.lbda = lbda
(theta_hat, a_coeffs,
b_coeffs) = self.sysid.run(self.t, self.u, self.y)
self.plotStateVector(a_coeffs, b_coeffs)
self.is_system_identified = True
self.btn_run_sys_id.setEnabled(False)
dt = self.dt
# num = self.sysid.getNum()
# den = self.sysid.getDen()
# self.Gz_dot = ctrl.TransferFunction(num, den, dt)
# Uncomment below to add integrator
# self.sysid.addIntegrator()
self.num = self.sysid.getNum()
self.den = self.sysid.getDen()
self.Gz = ctrl.TransferFunction(self.num, self.den, dt)
self.updateTfDisplay(a_coeffs[:, -1], b_coeffs[:, -1])
self.plotPolesZeros()
self.replayInputData()
def f(x):
controler = clt.TransferFunction([x[2], x[0], x[1]],[1,0])
sys = clt.feedback(controler*H) #fechando a malha
#Aplica degrau
sys2 = sys.returnScipySignalLTI()[0][0]
t2,y2 = step(sys2,N = dots)
return abs(1-y2[-1]) #retorna o erro
def setPlanta(PlantaStr):
Planta = PlantaStr.split("/")
numerador = [float(i) for i in Planta[0].split(',')]
if len(Planta) >= 2:
denominador = [float(i) for i in Planta[1].split(',')]
else:
denominador = [1.0]
return control.TransferFunction(numerador, denominador)
def random_transfer_function():
"""Generate a random transfer function for root locus practice."""
poles = random_poles()
zeros = random_zeros(poles)
zeros2 = np.floor(np.array(zeros) * 2) * 0.5
num = np.poly(zeros2)
den = np.poly(poles)
G = control.TransferFunction(num, den)
return G
def plot(self):
G = control.TransferFunction((1), (1, 1))
real, imag, freq = control.nyquist(G)
plt.title('Nyquist Plot')
plt.gcf().canvas.set_window_title('Nyquist Plot')
plt.xlabel('Real')
plt.ylabel('Imaginary')
plt.grid()
plt.show()
def make_closed_loop_plant(G, Kp):
"""Returns a TransferFunction representing a plant in negative feedback with
a P controller that uses the given gain.
Keyword arguments:
G -- open-loop plant
Kp -- proportional gain
"""
K = cnt.TransferFunction(Kp, 1)
return cnt.feedback(G, K)
def main():
dt = 0.0001
T = np.arange(0, 6, dt)
plt.xlabel("Time ($s$)")
plt.ylabel("Position ($m$)")
# Make plant
G = cnt.TransferFunction(1, conv([1, 5], [1, 0]))
sim(cnt.TransferFunction(1, 1), T, "Reference")
Gcl = make_closed_loop_plant(G, 120)
sim(Gcl, T, "Underdamped")
Gcl = make_closed_loop_plant(G, 3)
sim(Gcl, T, "Overdamped")
Gcl = make_closed_loop_plant(G, 6.268)
sim(Gcl, T, "Critically damped")
plt.savefig("pid_responses.svg")
def random_TF_first_order_only(max_poles=5, max_zeros=None):
plist = assign_first_order_poles_to_bins(max_poles=max_poles)
while not np.any(plist):
# We will not allow a TF that has no poles
plist = assign_poles_to_bins(max_poles=max_poles)
zlist = assign_first_order_zeros_to_bins(plist, max_zeros=max_zeros)
den = pole_bins_to_den(plist)
num = zero_bins_to_num(zlist)
G = control.TransferFunction(num,den)
return G
def random_transfer_function(max_poles=5):
"""Generate a random transfer function for root locus practice."""
poles = random_poles(max_poles=max_poles)
zeros = random_zeros(poles)
zeros2 = np.floor(np.array(zeros) *
2) * 0.5 # I guess I am rounding them all down here....
poles3, zeros3 = eliminate_near_cancellations(poles, zeros2.tolist())
num = np.poly(zeros3)
den = np.poly(poles3)
G = control.TransferFunction(num, den)
return G
def _create_comp(self):
self.Gc = control.TransferFunction(self.numlist,
self.denlist) * self.gain
if hasattr(control, 'c2d'):
c2d = control.c2d
elif hasattr(control, 'matlab'):
c2d = control.matlab.c2d
self.Gd = c2d(self.Gc, dt, 'tustin')
self.numz = squeeze(self.Gd.num)
self.denz = squeeze(self.Gd.den)
self.dig_comp = Digital_Compensator(self.numz, self.denz)
