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 Controlador(X):
#Atribuição dos paramentros Kp, Ki e Kd
Kp = Kpi + X[0] / 100
Ki = Kii + X[1] / 100
Kd = Kdi + X[2] / 100
#Função Transferência dos termos do controlador
P = Kp
I = tf(Ki, [1, 0])
D = tf([Kd, 0], [0.1 * Kd, 1])
#União dos blocos PID
C = parallel(P, I, D)
# Função Transferência com o Controlador
F = series(C, G)
# Penalidade para o valor do sinal de Entrada na planta
# ou seja penalidade para o sinal de Controle alto
tc = feedback(C, G)
_, yc = step_response(tc, time)
if max(yc) > maxcontrolador:
#SE1 = np.square(np.subtract(1,yc))
ISE = tdiv + max(yc)
else:
# Realizando a Integral do erro quadrado
t1 = feedback(F, 1)
_, y1 = step_response(t1, time)
SE = np.square(np.subtract(1, y1))
ISE = np.sum(SE)
return (ISE)
def test_interconnect_docstring():
"""Test the examples from the interconnect() docstring"""
# MIMO interconnection (note: use [C, P] instead of [P, C] for state order)
P = ct.LinearIOSystem(
ct.rss(2, 2, 2, strictly_proper=True), name='P')
C = ct.LinearIOSystem(ct.rss(2, 2, 2), name='C')
T = ct.interconnect(
[C, P],
connections = [
['P.u[0]', 'C.y[0]'], ['P.u[1]', 'C.y[1]'],
['C.u[0]', '-P.y[0]'], ['C.u[1]', '-P.y[1]']],
inplist = ['C.u[0]', 'C.u[1]'],
outlist = ['P.y[0]', 'P.y[1]'],
)
T_ss = ct.feedback(P * C, ct.ss([], [], [], np.eye(2)))
np.testing.assert_almost_equal(T.A, T_ss.A)
np.testing.assert_almost_equal(T.B, T_ss.B)
np.testing.assert_almost_equal(T.C, T_ss.C)
np.testing.assert_almost_equal(T.D, T_ss.D)
# Implicit interconnection (note: use [C, P, sumblk] for proper state order)
P = ct.tf2io(ct.tf(1, [1, 0]), inputs='u', outputs='y')
C = ct.tf2io(ct.tf(10, [1, 1]), inputs='e', outputs='u')
sumblk = ct.summing_junction(inputs=['r', '-y'], output='e')
T = ct.interconnect([C, P, sumblk], inplist='r', outlist='y')
T_ss = ct.feedback(P * C, 1)
np.testing.assert_almost_equal(T.A, T_ss.A)
np.testing.assert_almost_equal(T.B, T_ss.B)
np.testing.assert_almost_equal(T.C, T_ss.C)
np.testing.assert_almost_equal(T.D, T_ss.D)
def get_negative_feedback(self, sys_tf):
"""
Calculates the transfer function of a close loop
:param sys_tf: control object, Transfer function to transform.
:return: control object, Result transfer function
"""
sys_feedback = None
if self.feedback_tf == -1:
sys_feedback = feedback(sys_tf, sign=-1)
elif self.feedback_tf == 1:
sys_feedback = feedback(sys_tf, self.feedback_tf, sign=1)
return sys_feedback
def test_lineariosys_statespace(self):
"""Make sure that a LinearIOSystem is also a StateSpace object"""
iosys_siso = ct.LinearIOSystem(self.siso_linsys)
self.assertTrue(isinstance(iosys_siso, ct.StateSpace))
# Make sure that state space functions work for LinearIOSystems
np.testing.assert_array_equal(
iosys_siso.pole(), self.siso_linsys.pole())
omega = np.logspace(.1, 10, 100)
mag_io, phase_io, omega_io = iosys_siso.freqresp(omega)
mag_ss, phase_ss, omega_ss = self.siso_linsys.freqresp(omega)
np.testing.assert_array_equal(mag_io, mag_ss)
np.testing.assert_array_equal(phase_io, phase_ss)
np.testing.assert_array_equal(omega_io, omega_ss)
# LinearIOSystem methods should override StateSpace methods
io_mul = iosys_siso * iosys_siso
self.assertTrue(isinstance(io_mul, ct.InputOutputSystem))
# But also retain linear structure
self.assertTrue(isinstance(io_mul, ct.StateSpace))
# And make sure the systems match
ss_series = self.siso_linsys * self.siso_linsys
np.testing.assert_array_equal(io_mul.A, ss_series.A)
np.testing.assert_array_equal(io_mul.B, ss_series.B)
np.testing.assert_array_equal(io_mul.C, ss_series.C)
np.testing.assert_array_equal(io_mul.D, ss_series.D)
# Make sure that series does the same thing
io_series = ct.series(iosys_siso, iosys_siso)
self.assertTrue(isinstance(io_series, ct.InputOutputSystem))
self.assertTrue(isinstance(io_series, ct.StateSpace))
np.testing.assert_array_equal(io_series.A, ss_series.A)
np.testing.assert_array_equal(io_series.B, ss_series.B)
np.testing.assert_array_equal(io_series.C, ss_series.C)
np.testing.assert_array_equal(io_series.D, ss_series.D)
# Test out feedback as well
io_feedback = ct.feedback(iosys_siso, iosys_siso)
self.assertTrue(isinstance(io_series, ct.InputOutputSystem))
# But also retain linear structure
self.assertTrue(isinstance(io_series, ct.StateSpace))
# And make sure the systems match
ss_feedback = ct.feedback(self.siso_linsys, self.siso_linsys)
np.testing.assert_array_equal(io_feedback.A, ss_feedback.A)
np.testing.assert_array_equal(io_feedback.B, ss_feedback.B)
np.testing.assert_array_equal(io_feedback.C, ss_feedback.C)
np.testing.assert_array_equal(io_feedback.D, ss_feedback.D)
def test_feedback(self):
# Set up parameters for simulation
T, U, X0 = self.T, self.U, self.X0
# Linear system with constant feedback (via "nonlinear" mapping)
ioslin = ios.LinearIOSystem(self.siso_linsys)
nlios = ios.NonlinearIOSystem(None, \
lambda t, x, u, params: u, inputs=1, outputs=1)
iosys = ct.feedback(ioslin, nlios)
linsys = ct.feedback(self.siso_linsys, 1)
ios_t, ios_y = ios.input_output_response(iosys, T, U, X0)
lti_t, lti_y, lti_x = ct.forced_response(linsys, T, U, X0)
np.testing.assert_array_almost_equal(ios_y, lti_y, decimal=3)
def getValuesPlot(self):
s = tf([1, 0], 1)
# the plant
g = 200 / (10 * s + 1) / (0.05 * s + 1) ** 2
# disturbance plant
gd = 100 / (10 * s + 1)
# first design
# sensitivity weighting
M = 1.5
wb = 10
A = 1e-4
ws1 = (s / M + wb) / (s + wb * A)
# KS weighting
wu = tf(1, 1)
k1, cl1, info1 = mixsyn(g, ws1, wu)
# sensitivity (S) and complementary sensitivity (T) functions for
# design 1
s1 = feedback(1, g * k1)
t1 = feedback(g * k1, 1)
# second design
# this weighting differs from the text, where A**0.5 is used; if you use that,
# the frequency response doesn't match the figure. The time responses
# are similar, though.
ws2 = (s / M ** 0.5 + wb) ** 2 / (s + wb * A) ** 2
# the KS weighting is the same as for the first design
k2, cl2, info2 = mixsyn(g, ws2, wu)
# S and T for design 2
s2 = feedback(1, g * k2)
t2 = feedback(g * k2, 1)
# frequency response
omega = np.logspace(-2, 2, 101)
ws1mag, _, _ = ws1.freqresp(omega)
s1mag, _, _ = s1.freqresp(omega)
ws2mag, _, _ = ws2.freqresp(omega)
s2mag, _, _ = s2.freqresp(omega)
plt.figure(1)
# text uses log-scaled absolute, but dB are probably more familiar to most control engineers
#value, ca sa pot extrage data din line2d creata de plt.semilogx
value,=plt.semilogx(omega, 20 * np.log10(s1mag.flat), label='$S_1$')
return value.get_data()
def multi_loop():
"""
Gives the transfer funtion of multi-loop system
Feedback[ R(s) -> (G1 - H2) -> G2 -> feedback(G3G4, H1) , H3]-> C(s)
"""
g1 = np.array([
[1], [1, 10]])
g2 = np.array([
[1],
[1, 1]]
)
g3 = np.array([
[1, 0, 1],
[1, 4, 4]
])
g4 = np.array([
[1, 1],
[1, 6]
])
h1 = np.array([
[1, 1],
[1, 2]
])
h2 = np.array([[2], 1])
h3 = np.array([[1], 1])
sys_g1 = get_sys(g1, sys_name='tf')
sys_g2 = get_sys(g2, 'tf')
sys_g3 = get_sys(g3, 'tf')
sys_g4 = get_sys(g4, 'tf')
sysh1 = get_sys(h1, 'tf')
sysh2 = get_sys(h2, 'tf')
sysh3 = get_sys(h3, 'tf')
td.ouput(title='Exercise One',
keys=['G1', 'G2', 'G3', 'G4', 'H1', 'H2', 'H3'],
values=[sys_g1, sys_g2, sys_g3, sys_g4, sysh1, sysh2, sysh3])
sys_t = ct.series(sys_g1 - sysh2, sys_g2)
sys_t = ct.series(sys_t,
ct.feedback(
ct.series(sys_g3, sys_g4), sysh1, sign=1))
tS = ct.feedback(sys_t, sysh3)
zeros, poles = ct.pzmap(tS, Plot=True, grid=True)
show_plot(zeros, poles)
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 pidplot1(nlg, dlg, nlh, dlh):
G = control.tf(nlg, dlg)
H = control.tf(nlh, dlh)
s = str(H)
s1 = "("
s2 = "("
s = s.split('\n')
s[1] = s[1].strip()
s[3] = s[3].strip()
s1 = s1 + s[1] + ')'
s2 = s2 + s[3] + ')'
v = control.feedback(G, H)
t2, y2 = control.step_response(v)
plt.plot(t2, y2, 'r', linewidth=1, label='G(s) with Feedback H(s)')
plt.grid(True)
plt.title('Time response of G(s) with feedback H(s)=G(s)/(1+G(s)H(s))')
plt.xlabel('Time(sec)')
plt.ylabel('Amplitude')
if not os.path.isdir('static'):
os.mkdir('static')
else:
# Remove old plot files
for filename in glob.glob(
os.path.join('static', 'Feedback', 'pidc1.png')):
os.remove(filename)
pidc1 = os.path.join('static', 'Feedback', 'pidc1.png')
plt.savefig(pidc1)
plt.clf()
plt.cla()
plt.close()
return pidc1
def __init__(self, plant, dt):
self.plant, self.dt = plant, dt
Ac, Bc = hcu.num_jacobian([0, 0, 0, 0], [0], plant.dyn_cont)
#print Ac
#print Bc
Cc, Dc = np.array([[0, 1, 0, 0]]), np.array([[0]]) # regulate theta
self.sysc = control.ss(Ac, Bc, Cc, Dc)
self.sysd = control.sample_system(self.sysc, dt)
plant_tfd = control.tf(self.sysd)
ctl_tfd = control.tf([-4.945, 8.862, -3.967], [1.000, -1.481, 0.4812],
dt)
gt = control.feedback(ctl_tfd * plant_tfd, 1)
cl_polesd = gt.pole()
cl_polesc = np.log(cl_polesd) / dt
#y, t = control.step(gt, np.arange(0, 5, dt))
#plt.plot(t, y[0])
#plt.show()
#self.il_ctl = self.synth_pid(0.5, 0.01, 0.05)
#pdb.set_trace()
if 1:
#self.il_ctl = DiscTimeFilter([-4.945, 8.862, -3.967], [1.000, -1.481, 0.4812], 1.05)
self.il_ctl = DiscTimeFilter([-4.945, 8.862, -3.967],
[1.000, -1.481, 0.4812], 1.25)
else:
self.il_ctl = self.synth_pid(0.4, 0.04, 0.1)
self.il_ctl.num = [-c for c in self.il_ctl.num]
self.il_ctl.gain = 1.
#self.ol_ctl = DiscTimeFilter([0.18856, -0.37209, 0.18354], [1.00000, -1.86046, 0.86046], 0.9)
self.ol_ctl = DiscTimeFilter([0.18856, -0.37209, 0.18354],
[1.00000, -1.86046, 0.86046], 0.4)
def solve(problem_spec):
""" solution of coprime decomposition
:param problem_spec: ProblemSpecification object
:return: solution_data: output value of the system and controller function
"""
s, t, T = sp.symbols("s, t, T")
transfer_func = problem_spec.transfer_func()
z_func, n_func = transfer_func.expand().as_numer_denom(
) # separate numerator and denominator
# tracking controller
# numerator and denominator of controller
cd_res = cd.coprime_decomposition(z_func, n_func, problem_spec.pol)
tf_k = (cd_res.f_func * z_func) / (cd_res.h_func * n_func) # open_loop
z_o, n_o = sp.simplify(tf_k).expand().as_numer_denom()
n_coeffs_o = [float(c) for c in st.coeffs(z_o, s)
] # coefficients of numerator of open_loop
d_coeffs_o = [float(c) for c in st.coeffs(n_o, s)
] # coefficients of denominator of open_loop
# feedback
close_loop = control.feedback(control.tf(n_coeffs_o, d_coeffs_o))
# simulate system with controller with initial error (190K instead of 200K).
y = control.forced_response(close_loop, problem_spec.tt, problem_spec.yr,
problem_spec.x0_1)
solution_data = SolutionData()
solution_data.yy = y[1]
solution_data.controller_n = cd_res.f_func
solution_data.controller_d = cd_res.h_func
solution_data.controller_ceoffs = cd_res.c_coeffs
return solution_data
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 execute(self):
# Realimentando a malha
sysFechada = con.feedback(self.sys, 1)
# Resposta ao degrau
[self.xout, self.yout] = con.step_response(sysFechada,
const.TEMPO_CALCULO)
# "Alterando" amplitude do degrau
self.yout = self.yout * const.SP
# Pegando as informações sobre o sistema
info = con.step_info(sysFechada, self.xout)
# Pegando o valor de estado estacionário
self.valorEstacionario = info['SteadyStateValue'] * const.SP
# Ponto de acomodação
self.tempo_acomodacao = info['SettlingTime']
self.valor_acomodacao = accommodationPoint(self.yout,
self.valorEstacionario)
# Overshoot
self.overshootX = info['PeakTime']
self.overshootY = info['Peak'] * const.SP
self.overshoot = info['Overshoot']
# Erro em regime permanente
self.erroRegimePermanente = errorCalculate(const.SP,
self.valorEstacionario)
def step_sys(sys, final_time, setpoint):
# open-loop system transfer function
try:
num, den = model(sys)
except:
# for error detection
print("Err: system in not defined")
return
Gs = control.tf(num, den)
#print(Gs)
# closed-loop unity-feedback transfer function
Ts = control.feedback(Gs, 1)
# simulation time parameters
initial_time = 0
nsteps = 40 * final_time # number of time steps
t = np.linspace(initial_time, final_time, round(nsteps))
output, t = matlab.step(Ts, t)
output = setpoint*output
# covert numpy arrays to lists
t = list(t)
output = list(output)
# round lists to 6 decimal digits
ndigits = 6
t = [round(num, ndigits) for num in t]
output = [round(num, ndigits) for num in output]
return t, output
def fitness_calc(self, Gp, Time, Input):
# Compute location of zeros
(self.Z1,
self.Z2) = np.roots([1, 2 * self.DP1 * self.WN1, self.WN1**2])
(self.Z3,
self.Z4) = np.roots([1, 2 * self.DP2 * self.WN2, self.WN2**2])
# Create PI controller
(self.Gc_num, self.Gc_den) = zpk2tf(
[self.Z1, self.Z2, self.Z3, self.Z4], [0],
self.K) # Controller with one pole in origin and 2 pair of zeros
self.Gc = ctrl.tf(self.Gc_num, self.Gc_den)
# Evaluate closed loop stability
self.gm, self.pm, self.Wcg, self.Wcp = ctrl.margin(self.Gc * Gp)
# Dischard solutions with no gain margin
if self.gm == None or self.gm <= 1:
self.fitness = 999
return
# Closed loop system
self.M = ctrl.feedback(self.Gc * Gp, 1)
# Closed loop step response
self.y, self.t, self.xout = ctrl.lsim(self.M, Input, Time)
# Evaluate fitness
self.fitness = evaluate(Input, self.y)
def calculating_params(self, populations):
calculatedParams = []
for population in populations:
kp = population[0]
ti = population[1]
td = population[2]
G = kp * control.tf([ti * td, ti, 1], [ti, 0])
F = control.tf(1, [1, 6, 11, 6, 0])
system = control.feedback(control.series(G, F), 1)
t = []
i = 0
while i < 100:
t.append(i)
i += 0.01
try:
systemInfo = control.step_info(system)
except IndexError:
calculatedParams.append([10000000, 1, 100, 200])
continue
T, output = control.step_response(system, T=t)
ISE = round(sum((output - 1)**2), 2)
timeRise = round(systemInfo['RiseTime'], 2)
timeSettle = round(systemInfo['SettlingTime'], 2)
overshoot = round(systemInfo['Overshoot'], 2)
calculatedParams.append([ISE, timeRise, timeSettle, overshoot])
return calculatedParams
def main():
m = 1500 # Mass. Gives it a bit delay in the beginning.
k = 450 # Static gain. Tune so end values are similar to experimental data.
c = 240 # Time constant. Higher is slower. Damping.
# Plant transfer function. Static gain is numerator. Denominator is c * s + 1.
plant = control.tf([k], [m, c, 1])
plant_ss = control.ss(plant)
Kp = 2
Ki = 0
Kd = 1
# PID controller transfer function. C(s) = Kp + Ki/s + Kd s = (Kd s^2 + Kp s + Ki) / s
controller = control.tf([Kd, Kp, Ki], [1, 0])
sys = control.feedback(plant, controller)
# Plot the step responses
setpoint = 150
n = 100 # seconds
fig, ax = plt.subplots(tight_layout=True)
T = np.arange(0, n, 1) # 0 -- n seconds, in steps of 1 second
u = np.full(
n, fill_value=setpoint) # input vector. Step response so single value.
T, y_out, x_out = control.forced_response(sys, T, u)
ax.plot(T, y_out, label=str(setpoint))
ax.plot(T, x_out[0], label='x0')
ax.plot(T, x_out[1], label='x1')
ax.legend()
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 fitnessFunction(Kp, Ti, Td):
G = control.tf([Ti * Td, Ti, 1], [Ti, 0])
F = control.tf(1, [1, 6, 11, 6, 0])
sys = control.feedback(control.series(G, F), 1)
y = control.step(sys)
yout = y[0]
t = y[-1]
# Integrated Square Error
error = 0
for val in y[0]:
error += (val - 1) * (val - 1)
# Overshoot
OS = (yout.max() / yout[-1] - 1) * 100
Tr = 0
Ts = 0
# Rising Time
for i in range(0, len(yout) - 1):
if yout[i] > yout[-1] * .90:
Tr = t[i] - t[0]
# Settling Time
for i in range(2, len(yout) - 1):
if abs(yout[-i] / yout[-1]) > 1.02:
Ts = t[len(yout) - i] - t[0]
return 1 / (OS + Tr + Ts + error * error * 10) * 100000
def fitnessFunction(Kp, Ti, Td):
G = control.tf([Ti*Td, Ti, 1], [Ti, 0])
F = control.tf(1,[1,6,11,6,0])
sys = control.feedback(control.series(G, F), 1)
y = control.step(sys)
yout = y[0]
t = y[-1]
# Integrated Square Error
error = 0
for val in y[0]:
error += (val - 1)*(val - 1)
# Overshoot
OS = (yout.max()/yout[-1]-1)*100
Tr = 0
Ts = 0
# Rising Time
for i in range(0, len(yout) - 1):
if yout[i] > yout[-1]*.90:
Tr = t[i] - t[0]
# Settling Time
for i in range(2, len(yout) - 1):
if abs(yout[-i]/yout[-1]) > 1.02:
Ts = t[len(yout) - i] - t[0]
return 1/(OS + Tr + Ts + error*error*10)*100000
def classic():
# PI controller
Kp = 1.5
Ki = 30.
P = tf([Kp], [1])
I = tf([Ki], [1, 0])
Gc = parallel(P, I)
# Power convertor
Gpc = synthesize_2pole_filter(50, 0.707)
# Plant
Gp = tf([50], [1, 0])
# Sensor
# если выбросить из петли становится лучше
# "remove sensor lag"
Gs = synthesize_1pole_filter(20)
# Openloop
Gol = series(series(Gc, Gpc), Gp)
# Closed loop
Gcl = feedback(Gol)
ts, xs = step_response( Gcl )
grid()
plot(ts, xs)
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 pidplot1(num, den, kp, ki, kd):
G = control.tf(num, den)
if ki != 0:
g2 = control.tf([kd, kp, ki], [1, 0])
else:
g2 = control.tf([kd, kp], [1])
k = control.series(G, g2)
v = control.feedback(k, 1)
t1, y1 = control.step_response(G)
t2, y2 = control.step_response(v)
plt.plot(t2, y2, 'r', linewidth=1, label='G(s) with PID control')
plt.grid(True)
s = '(Kp=' + str(kp) + ',Ki=' + str(ki) + ',Kd=' + str(kd) + ')'
plt.title('Time response of G(s) with PID control' + s)
plt.xlabel('Time(sec)')
plt.ylabel('Amplitude')
if not os.path.isdir('static'):
os.mkdir('static')
else:
# Remove old plot files
for filename in glob.glob(
os.path.join('static', 'pidcontrol', 'pidc1.png')):
os.remove(filename)
pidc1 = os.path.join('static', 'pidcontrol', 'pidc1.png')
plt.savefig(pidc1)
plt.clf()
plt.cla()
plt.close()
return pidc1
def assignment2(Kc=Kc, Ki=Ki): #Kc has to be a list; Ki has to be a float
# Defining the transfer functions
sysgp = control.tf(4, [500, 2]) #Process
sysgv = control.tf(2, [1, 10, 1]) #Valve
sysgt = control.tf(2, 1) # Sensor
syspi_list = []
# Loop to iterate among the different values of Kc
for i in Kc:
syspi = control.tf([i, i * Ki], [1, 0])
syspi_list.append(syspi)
# Plotting the figures with the different values of Kc
plt.figure()
for i in range(len(syspi_list)):
#Defining the closed loop
syslc = control.feedback(syspi_list[i] * sysgv * sysgp, sysgt)
consig = 1 / 2 # The setpoint is 0.5, because the sensor
# transfer function is equal to 2
t, y = control.step_response(syslc, 200)
error = np.abs(y - consig) # The error while achieving the setpoint
plt.plot(t, y, label=f'Kc= {Kc[i]:.2f}')
# plt.plot(t, error, label=f'Error para Kc={Kc[i]:.2f}')
plt.title(f'Ki= {Ki:.4f}')
plt.legend()
plt.xlabel('Time (s)')
plt.ylabel('Value')
plt.show()
return syspi_list
def Q2_perfFCN(Kp, Ti, Td): G = Kp * tf([Ti * Td, Ti, 1.0], [Ti, 0]) sys = feedback(series(G,F),1) res = step_response(sys, t) ISE = sum((val - 1)**2 for val in res[1]) return (ISE,) + stepinfo(res)
def position():
vmax = 2
kl = 3.1
lmax = (vmax / kl)**2
lrange = np.arange(-lmax, lmax, 0.0001)[np.newaxis].T
vrange = (np.sign(lrange) * kl * (np.abs(lrange))**0.5)
k1 = np.linalg.pinv(lrange) @ vrange
plt.plot(lrange, vrange)
plt.plot(lrange, k1 * lrange)
#print(k1)
#plt.show()
#plt.clf()
Hs = ct.tf([0, 0, k1], [1, 0, 0])
#print(Hs)
Hz = ct.matlab.c2d(Hs, 1, method='zoh')
#print(Hz)
p = [-0.5, .8] #pole locations
z = [-0.5, .999] #zero loations
gain = 2e-7 #gain
freq = 0.001 #at frequency
Fs0 = 1 #sample rate
a, b = fn.generic_biquad(p, z, gain, freq, Fs0)
print('compensator', a, b)
Kz = ct.tf(b, a, 1)
a, b = fn.biquad_lowpass(0.01, 0.5, 20)
print('lowpass', a, b)
a, b = fn.biquad_lowpass(0.005, 0.707, 1)
Fz = ct.tf(b, a, 1)
ct.bode_plot(Hz * Kz * Fz, np.logspace(-4, -1, 1000))
ct.bode_plot(Hz * Kz, np.logspace(-4, -1, 1000))
plt.show()
plt.clf()
sys = ct.feedback(Hz * Kz * Fz, 1)
y, t = ct.step(sys, np.arange(0, 10000, 1))
plt.plot(t, y.T)
sys = ct.feedback(Hz * Kz, 1)
y, t = ct.step(sys, np.arange(0, 10000, 1))
plt.plot(t, y.T)
plt.show()
'''
def test_bdalg_functions(self):
"""Test block diagram functions algebra on I/O systems"""
# Set up parameters for simulation
T = self.T
U = [np.sin(T), np.cos(T)]
X0 = 0
# Set up systems to be composed
linsys1 = self.mimo_linsys1
linio1 = ios.LinearIOSystem(linsys1)
linsys2 = self.mimo_linsys2
linio2 = ios.LinearIOSystem(linsys2)
# Series interconnection
linsys_series = ct.series(linsys1, linsys2)
iosys_series = ct.series(linio1, linio2)
lin_t, lin_y, lin_x = ct.forced_response(linsys_series, T, U, X0)
ios_t, ios_y = ios.input_output_response(iosys_series, T, U, X0)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
# Make sure that systems don't commute
linsys_series = ct.series(linsys2, linsys1)
lin_t, lin_y, lin_x = ct.forced_response(linsys_series, T, U, X0)
self.assertFalse((np.abs(lin_y - ios_y) < 1e-3).all())
# Parallel interconnection
linsys_parallel = ct.parallel(linsys1, linsys2)
iosys_parallel = ct.parallel(linio1, linio2)
lin_t, lin_y, lin_x = ct.forced_response(linsys_parallel, T, U, X0)
ios_t, ios_y = ios.input_output_response(iosys_parallel, T, U, X0)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
# Negation
linsys_negate = ct.negate(linsys1)
iosys_negate = ct.negate(linio1)
lin_t, lin_y, lin_x = ct.forced_response(linsys_negate, T, U, X0)
ios_t, ios_y = ios.input_output_response(iosys_negate, T, U, X0)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
# Feedback interconnection
linsys_feedback = ct.feedback(linsys1, linsys2)
iosys_feedback = ct.feedback(linio1, linio2)
lin_t, lin_y, lin_x = ct.forced_response(linsys_feedback, T, U, X0)
ios_t, ios_y = ios.input_output_response(iosys_feedback, T, U, X0)
np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
def test_feedback_args(self, tsys):
# Added 25 May 2019 to cover missing exception handling in feedback()
# If first argument is not LTI or convertable, generate an exception
args = ([1], tsys.sys2)
with pytest.raises(TypeError):
ctrl.feedback(*args)
# If second argument is not LTI or convertable, generate an exception
args = (tsys.sys1, 'hello world')
with pytest.raises(TypeError):
ctrl.feedback(*args)
# Convert first argument to FRD, if needed
h = TransferFunction([1], [1, 2, 2])
omega = np.logspace(-1, 2, 10)
frd = ctrl.FRD(h, omega)
sys = ctrl.feedback(1, frd)
assert isinstance(sys, ctrl.FRD)
def test_connect(self):
# Define a couple of (linear) systems to interconnection
linsys1 = self.siso_linsys
iosys1 = ios.LinearIOSystem(linsys1)
linsys2 = self.siso_linsys
iosys2 = ios.LinearIOSystem(linsys2)
# Connect systems in different ways and compare to StateSpace
linsys_series = linsys2 * linsys1
iosys_series = ios.InterconnectedSystem(
(iosys1, iosys2), # systems
((1, 0),), # interconnection (series)
0, # input = first system
1 # output = second system
)
# Run a simulation and compare to linear response
T, U = self.T, self.U
X0 = np.concatenate((self.X0, self.X0))
ios_t, ios_y, ios_x = ios.input_output_response(
iosys_series, T, U, X0, return_x=True)
lti_t, lti_y, lti_x = ct.forced_response(linsys_series, T, U, X0)
np.testing.assert_array_almost_equal(lti_t, ios_t)
np.testing.assert_array_almost_equal(lti_y, ios_y, decimal=3)
# Connect systems with different timebases
linsys2c = self.siso_linsys
linsys2c.dt = 0 # Reset the timebase
iosys2c = ios.LinearIOSystem(linsys2c)
iosys_series = ios.InterconnectedSystem(
(iosys1, iosys2c), # systems
((1, 0),), # interconnection (series)
0, # input = first system
1 # output = second system
)
self.assertTrue(ct.isctime(iosys_series, strict=True))
ios_t, ios_y, ios_x = ios.input_output_response(
iosys_series, T, U, X0, return_x=True)
lti_t, lti_y, lti_x = ct.forced_response(linsys_series, T, U, X0)
np.testing.assert_array_almost_equal(lti_t, ios_t)
np.testing.assert_array_almost_equal(lti_y, ios_y, decimal=3)
# Feedback interconnection
linsys_feedback = ct.feedback(linsys1, linsys2)
iosys_feedback = ios.InterconnectedSystem(
(iosys1, iosys2), # systems
((1, 0), # input of sys2 = output of sys1
(0, (1, 0, -1))), # input of sys1 = -output of sys2
0, # input = first system
0 # output = first system
)
ios_t, ios_y, ios_x = ios.input_output_response(
iosys_feedback, T, U, X0, return_x=True)
lti_t, lti_y, lti_x = ct.forced_response(linsys_feedback, T, U, X0)
np.testing.assert_array_almost_equal(lti_t, ios_t)
np.testing.assert_array_almost_equal(lti_y, ios_y, decimal=3)
def draw_step_response_feedback(K,sys):
# Defining feedback system
C = control.tf(K,1)
# Using feedback according to http://nl.mathworks.com/help/control/examples/using-feedback-to-close-feedback-loops.html
controled_sys = control.feedback(C*sys,1)
poles = control.pole(controled_sys)
y,t = control.step(controled_sys)
plt.plot(t,y)
plt.xlabel('t')
plt.ylabel('y(t)')
def compute_ise(Kp, Ti, Td):
g = Kp * TransferFunction([Ti * Td, Ti, 1], [Ti, 0])
sys = feedback(series(g, F), 1)
sys_info = step_info(sys)
_, y = step_response(sys, T)
ise = sum((y - 1)**2)
t_r = sys_info['RiseTime']
t_s = sys_info['SettlingTime']
m_p = sys_info['Overshoot']
return ise, t_r, t_s, m_p
def q1_perfFNC(Kp, Ti, Td):
G = Kp * TransferFunction([Ti * Td, Ti, 1], [Ti, 0])
sys = feedback(series(G, F), 1)
sysinf = step_info(sys)
T, y = step_response(sys, T=t)
# return ISE, t_r, t_s, M_p
return sum((
y -
1)**2), sysinf['RiseTime'], sysinf['SettlingTime'], sysinf['Overshoot']
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 ramp_response_BF(num, den, feedback=1, t=None):
if t == None:
t = np.linspace(0, 10, 101)
u = t
# calcul de la BF
sys = ctl.tf(num, den)
sys_BF = g = ctl.feedback(sys, feedback)
lti_BF = sys_BF.returnScipySignalLti()[0][0]
t, s, x = signal.lsim2(lti_BF, u, t)
return t, s
def step_response_BF_pert(num1, den1, num2, den2, feedback=1, pert=0.25, pert_time=1, t=None):
if t == None:
t = np.linspace(0, 10, 101)
sys1 = ctl.tf(num1, den1)
sys2 = ctl.tf(num2, den2)
sys_BO1 = ctl.series(sys1, sys2)
sys_BF1 = g = ctl.feedback(sys_BO1, feedback)
lti_BF1 = sys_BF1.returnScipySignalLti()[0][0]
sys_BF2 = g = ctl.feedback(sys2, sys1)
lti_BF2 = sys_BF2.returnScipySignalLti()[0][0]
u = pert * (t > pert_time)
t, s1 = signal.step2(lti_BF1, None, t)
t, s2, trash = signal.lsim2(lti_BF2, u, t)
s = s1 + s2
return t, s
def step_response_BF_cor(num1, den1, num2, den2, feedback=1, t=None):
if t == None:
t = np.linspace(0, 10, 101)
sys1 = ctl.tf(num1, den1)
sys2 = ctl.tf(num2, den2)
sys_BO = ctl.series(sys1, sys2)
sys_BF = g = ctl.feedback(sys_BO, feedback)
lti_BF = sys_BF.returnScipySignalLti()[0][0]
t, s = signal.step2(lti_BF, None, t)
return t, s
def test_feedback_args(self):
# Added 25 May 2019 to cover missing exception handling in feedback()
# If first argument is not LTI or convertable, generate an exception
args = ([1], self.sys2)
self.assertRaises(TypeError, ctrl.feedback, *args)
# If second argument is not LTI or convertable, generate an exception
args = (self.sys1, np.array([1]))
self.assertRaises(TypeError, ctrl.feedback, *args)
# Convert first argument to FRD, if needed
h = TransferFunction([1], [1, 2, 2])
omega = np.logspace(-1, 2, 10)
frd = ctrl.FRD(h, omega)
sys = ctrl.feedback(1, frd)
self.assertTrue(isinstance(sys, ctrl.FRD))
def pid_design(G, K_guess, d_tc, verbose=False, use_P=True, use_I=True, use_D=True):
"""
:param G: transfer function
:param K_guess: gain matrix guess
:param d_tc: time constant for derivative
:param use_P: use p gain in design
:param use_I: use i gain in design
:param use_D: use d gain in design
:return: (K, G_comp, Gc_comp)
K: gain matrix
G_comp: open loop compensated plant
Gc_comp: closed loop compensated plant
"""
# compensator transfer function
H = []
if use_P:
H += [control.tf(1, 1)]
if use_I:
H += [control.tf((1), (1, 0))]
if use_D:
H += [control.tf((1, 0), (d_tc, 1))]
H = np.array([H]).T
H_num = [[H[i][j].num[0][0] for i in range(H.shape[0])] for j in range(H.shape[1])]
H_den = [[H[i][j].den[0][0] for i in range(H.shape[0])] for j in range(H.shape[1])]
H = control.tf(H_num, H_den)
# print('G', G)
# print('H', H)
ss_open = control.tf2ss(G*H)
if verbose:
print('optimizing controller')
K = lqr_ofb_design(K_guess, ss_open, verbose)
if verbose:
print('done')
# print('K', K)
# print('H', H)
G_comp = control.series(G, H*K)
Gc_comp = control.feedback(G_comp, 1)
return K, G_comp, Gc_comp
def form_PI_cl(data):
A = np.array([[1.0]])
B = np.array([[1.0]])
for i in range(N):
C = np.array([[dt*data['Ki_fit'][i]]])
D = np.array([[data['Kp_fit'][i]]])
pi_block = ss(A, B, C, D)
bike_block = ss(data['A_cl'][i], data['B_cl'][i], data['C_cl'][i], 0)
pc = series(pi_block, bike_block)
cl = feedback(pc, 1, sign=-1)
data['yr_cl_evals'][i] = la.eigvals(cl.A)
assert(np.all(abs(data['yr_cl_evals'][i]) < 1.0))
data['A_yr_cl'][i] = cl.A
data['B_yr_cl'][i] = cl.B
data['C_yr_cl'][i] = cl.C
assert(cl.D == 0)
num, den = ss2tf(cl.A, cl.B, cl.C, cl.D)
data['w_psi_r_to_psi_dot'][i], y = freqz(num[0], den)
data['w_psi_r_to_psi_dot'][i] /= (dt * 2.0 * np.pi)
data['mag_psi_r_to_psi_dot'][i] = 20.0 * np.log10(abs(y))
data['phase_psi_r_to_psi_dot'][i] = np.unwrap(np.angle(y)) * 180.0 / np.pi
# Modelo de la planta P = ctrl.tf([1],[1,2,0]) # Modelo del Controlador Kp = 4; Kd = 2; Ki = .1; pd = 1 integ = ctrl.tf ([Ki],[1,0]) deriv = ctrl.tf ([Kd, 1], [0.1,1]) prop = Kp C = ctrl.parallel (deriv + prop, integ) Ac,Bc,Cc,Dc = ctrl.matlab.ssdata(C) print deriv, integ, C # Planta con realimentación unitaria PID l = ctrl.series (C,P) sys = ctrl.feedback(l,1) y,t = ctrl.matlab.step(sys, T = np.arange(0, Ttotal, DT) ) mpl.plot(t,y, label='C*Planta CL') #print y.shape, t.shape #mag, phase, omega = bode(C) #mpl.plot(omega,mag) #mpl.show() # Planta con realimentación unitaria sin controlador sys1 = ctrl.feedback(P,1) y,t = ctrl.matlab.step(sys1, T = np.arange(0, Ttotal, DT) ) mpl.plot(t,y, label='Planta CL'); mpl.legend(loc='lower right') # Planta con realimentación unitaria controlador P
gd = 100 / (10 * s + 1) # first design # sensitivity weighting M = 1.5 wb = 10 A = 1e-4 ws1 = (s / M + wb) / (s + wb * A) # KS weighting wu = tf(1, 1) k1, cl1, info1 = mixsyn(g, ws1, wu) # sensitivity (S) and complementary sensitivity (T) functions for # design 1 s1 = feedback(1, g * k1) t1 = feedback(g * k1, 1) # second design # this weighting differs from the text, where A**0.5 is used; if you use that, # the frequency response doesn't match the figure. The time responses # are similar, though. ws2 = (s / M ** 0.5 + wb) ** 2 / (s + wb * A) ** 2 # the KS weighting is the same as for the first design k2, cl2, info2 = mixsyn(g, ws2, wu) # S and T for design 2 s2 = feedback(1, g * k2) t2 = feedback(g * k2, 1)
from pid_libs import Controller, Analysis
import control as ctrl
# Spec para um controlador PI de Kp = 0.6 e Ki = 0.857:
pid_specs = {'Kc' : 0.6, 'Ti' : 0.7}
# Funcoes de transferencia:
G = ctrl.tf (2, [1, 1.15, 3.76]) # G(s) = 2 / (s^2 + 1.15s + 3.76)
C = Controller(pid_specs).tf # C(s) = 0.6 + 0.857 / s
L = ctrl.feedback (C * G) # Realimentacao em serie
# Respostas no dominio do tempo de da frequencia
Analysis.time (G, method = 'impulse') # Resposta ao impulso da malha aberta
Analysis.time (L, plot = True) # Resposta ao degrau da malha fechada
Analysis.bode (G, plot = True) # Resposta em frequencia da malha aberta
Analysis.bode (L, plot = True) # Dados de estabilidade marginal da malha fechada
def step_response_BF(num, den, feedback=1, t=None):
sys = ctl.tf(num, den)
sys_BF = g = ctl.feedback(sys, feedback)
lti_BF = sys_BF.returnScipySignalLti()[0][0]
t, s = signal.step2(lti_BF, None, t)
return t, s
# Modelo del Controlador Kp = 4; Kd = 2; Ki = .1 integ = ctrl.tf ([Ki],[1,0]) deriv = ctrl.tf ([Kd, 0], [0.1,1]) prop = Kp # Interconección del controlador C = ctrl.parallel (deriv + prop, integ) #derivPuro = ctrl.tf ([Kd, Kp], [0,1]) # Serie del Controlador y la Planta l = ctrl.series (C,P) # Realimentación Unitaria sys = ctrl.feedback(l,1) # Respuesta al escalón unitario y,t = ctrl.matlab.step(sys, T = np.arange(0, Ttotal, DT) ) # Gráfica del escalón unitario #mpl.plot(t,y) #mpl.show() #print y.shape, t.shape #mag, phase, omega = bode(C) #mpl.plot(omega,mag) #mpl.show()
