def test_conversion(self):
# Check the conversion functions
s = TransferFunction([1, 0], [1, -1])
assert_(isinstance(s.to_ss(), StateSpace))
assert_(isinstance(s.to_tf(), TransferFunction))
assert_(isinstance(s.to_zpk(), ZerosPolesGain))
# Make sure copies work
assert_(TransferFunction(s) is not s)
assert_(s.to_tf() is not s)
def test_properties(self):
# Test setters/getters for cross class properties.
# This implicitly tests to_ss() and to_zpk()
# Getters
s = TransferFunction([1, 0], [1, -1], dt=0.05)
assert_equal(s.poles, [1])
assert_equal(s.zeros, [0])
with warnings.catch_warnings():
warnings.simplefilter("ignore", DeprecationWarning)
assert_equal(s.gain, 1)
assert_equal(s.A, 1)
assert_equal(s.B, 1)
assert_equal(s.C, 1)
assert_equal(s.D, 1)
# state space setters
s2 = TransferFunction([2, 3], [4, 5], dt=0.05)
with warnings.catch_warnings():
warnings.simplefilter("ignore", DeprecationWarning)
s2.A = 1
s2.B = 1
s2.C = 1
s2.D = 1
self._compare_systems(s, s2)
# zpk setters
s2 = TransferFunction([2, 3], [4, 5], dt=0.05)
with warnings.catch_warnings():
warnings.simplefilter("ignore", DeprecationWarning)
s2.poles = 1
s2.zeros = 0
s2.gain = 1
self._compare_systems(s, s2)
def test_full(self):
# Numerator and denominator same order
num = [2, 3, 4]
den = [5, 6, 7]
num2, den2 = TransferFunction._z_to_zinv(num, den)
assert_equal(num, num2)
assert_equal(den, den2)
num2, den2 = TransferFunction._zinv_to_z(num, den)
assert_equal(num, num2)
assert_equal(den, den2)
def test_numerator(self):
# Numerator lower order than denominator
num = [2, 3]
den = [5, 6, 7]
num2, den2 = TransferFunction._z_to_zinv(num, den)
assert_equal([0, 2, 3], num2)
assert_equal(den, den2)
num2, den2 = TransferFunction._zinv_to_z(num, den)
assert_equal([2, 3, 0], num2)
assert_equal(den, den2)
def test_denominator(self):
# Numerator higher order than denominator
num = [2, 3, 4]
den = [5, 6]
num2, den2 = TransferFunction._z_to_zinv(num, den)
assert_equal(num, num2)
assert_equal([0, 5, 6], den2)
num2, den2 = TransferFunction._zinv_to_z(num, den)
assert_equal(num, num2)
assert_equal([5, 6, 0], den2)
def test_properties(self):
# Test setters/getters for cross class properties.
# This implicitly tests to_ss() and to_zpk()
# Getters
s = TransferFunction([1, 0], [1, -1], dt=0.05)
assert_equal(s.poles, [1])
assert_equal(s.zeros, [0])
def test_pole_one(self):
# Test that freqresp() doesn't fail on a system with a pole at 0.
# integrator, pole at zero: H(s) = 1 / s
system = TransferFunction([1], [1, -1], dt=0.1)
with warnings.catch_warnings():
warnings.simplefilter("ignore", RuntimeWarning)
w, mag, phase = dbode(system, n=2)
assert_equal(w[0], 0.) # a fail would give not-a-number
def test_freq_range(self):
# Test that freqresp() finds a reasonable frequency range.
# 1st order low-pass filter: H(z) = 1 / (z - 0.2),
# Expected range is from 0.01 to 10.
system = TransferFunction(1, [1, -0.2], dt=0.1)
n = 10
expected_w = np.linspace(0, np.pi, 10, endpoint=False)
w, H = dfreqresp(system, n=n)
assert_almost_equal(w, expected_w)
def __init__(self, counter, denominator):
"""
An element is described by the counter and denominator of its transfer function. It has corresponding scipy lti and TransferFunction object as attributes.
counter and denominator are list from highest order term to lowest.
"""
self.counter = np.asarray(counter, dtype=np.float64)
self.denominator = np.asarray(denominator, dtype=np.float64)
self.sys = lti(counter, denominator)
self.tf = TransferFunction(counter, denominator)
def test_pole_one(self):
# Test that freqresp() doesn't fail on a system with a pole at 0.
# integrator, pole at zero: H(s) = 1 / s
system = TransferFunction([1], [1, -1], dt=0.1)
with suppress_warnings() as sup:
sup.filter(RuntimeWarning, message="divide by zero")
sup.filter(RuntimeWarning, message="invalid value encountered")
w, mag, phase = dbode(system, n=2)
assert_equal(w[0], 0.) # a fail would give not-a-number
def test_range(self):
# Test that bode() finds a reasonable frequency range.
# 1st order low-pass filter: H(s) = 0.3 / (z - 0.2),
dt = 0.1
system = TransferFunction(0.3, [1, -0.2], dt=0.1)
n = 10
# Expected range is from 0.01 to 10.
expected_w = np.linspace(0, np.pi, n, endpoint=False) / dt
w, mag, phase = dbode(system, n=n)
assert_almost_equal(w, expected_w)
def test_conversion(self):
# Check the conversion functions
s = TransferFunction([1, 0], [1, -1], dt=0.05)
assert_(isinstance(s.to_ss(), StateSpace))
assert_(isinstance(s.to_tf(), TransferFunction))
assert_(isinstance(s.to_zpk(), ZerosPolesGain))
# Make sure copies work
assert_(TransferFunction(s) is not s)
assert_(s.to_tf() is not s)
def test_manual(self):
# Test dfreqresp() real part calculation (manual sanity check).
# 1st order low-pass filter: H(z) = 1 / (z - 0.2),
system = TransferFunction(1, [1, -0.2], dt=0.1)
w = [0.1, 1, 10]
w, H = dfreqresp(system, w=w)
# test real
expected_re = [1.2383, 0.4130, -0.7553]
assert_almost_equal(H.real, expected_re, decimal=4)
# test imag
expected_im = [-0.1555, -1.0214, 0.3955]
assert_almost_equal(H.imag, expected_im, decimal=4)
def test_auto(self):
# Test bode() magnitude calculation.
# 1st order low-pass filter: H(s) = 0.3 / (z - 0.2),
system = TransferFunction(0.3, [1, -0.2], dt=0.1)
w = np.array([0.1, 0.5, 1, np.pi])
w2, mag, phase = dbode(system, w=w)
jw = np.exp(w * 1j)
y = np.polyval(system.num, jw) / np.polyval(system.den, jw)
# Test mag
expected_mag = 20.0 * np.log10(abs(y))
assert_almost_equal(mag, expected_mag)
# Test phase
expected_phase = np.rad2deg(np.angle(y))
assert_almost_equal(phase, expected_phase)
def test_auto(self):
# Test dfreqresp() real part calculation.
# 1st order low-pass filter: H(z) = 1 / (z - 0.2),
system = TransferFunction(1, [1, -0.2], dt=0.1)
w = [0.1, 1, 10, 100]
w, H = dfreqresp(system, w=w)
jw = np.exp(w * 1j)
y = np.polyval(system.num, jw) / np.polyval(system.den, jw)
# test real
expected_re = y.real
assert_almost_equal(H.real, expected_re)
# test imag
expected_im = y.imag
assert_almost_equal(H.imag, expected_im)
def test_manual(self):
# Test bode() magnitude calculation (manual sanity check).
# 1st order low-pass filter: H(s) = 0.3 / (z - 0.2),
dt = 0.1
system = TransferFunction(0.3, [1, -0.2], dt=dt)
w = [0.1, 0.5, 1, np.pi]
w2, mag, phase = dbode(system, w=w)
# Test mag
expected_mag = [-8.5329, -8.8396, -9.6162, -12.0412]
assert_almost_equal(mag, expected_mag, decimal=4)
# Test phase
expected_phase = [-7.1575, -35.2814, -67.9809, -180.0000]
assert_almost_equal(phase, expected_phase, decimal=4)
# Test frequency
assert_equal(np.array(w) / dt, w2)
ax2.semilogx (w/(2*np.pi), phase, 'r-', linewidth="1")
ax2.set_title('Phase')
plt.tight_layout()
plt.show()
###########################################
# Multiplico Transferencias para OpenLoop #
###########################################
c = lti_to_sympy(pid)
p = lti_to_sympy(planta)
ol = c * p
open_loop = sympy_to_lti(ol)
open_loop = TransferFunction(open_loop.num, open_loop.den) #normalizo
w, mag, phase = bode(open_loop, freq)
fig, (ax1, ax2) = plt.subplots(2,1)
ax1.semilogx (w/(2*np.pi), mag, 'b-', linewidth="1")
ax1.set_title('Open Loop Tf - Magnitude')
ax2.semilogx (w/(2*np.pi), phase, 'r-', linewidth="1")
ax2.set_title('Phase')
plt.tight_layout()
plt.show()
############################################
# Cierro el lazo y hago pruebas temporales #
##############################
# Convierto Planta a Digital #
# por Forward Euler #
# y por Tustin #
##############################
Fsampling = 2000
Tsampling = 1 / Fsampling
planta_dig_tustin_n, planta_dig_tustin_d, td = cont2discrete(
(planta.num, planta.den), Tsampling, method='tustin')
planta_dig_euler_n, planta_dig_euler_d, td = cont2discrete(
(planta.num, planta.den), Tsampling, method='euler')
#normalizo con TransferFunction
print("Planta en Digital")
planta_dig_tustin = TransferFunction(planta_dig_tustin_n,
planta_dig_tustin_d,
dt=td)
print(planta_dig_tustin)
planta_dig_euler = TransferFunction(planta_dig_euler_n,
planta_dig_euler_d,
dt=td)
print(planta_dig_euler)
#dbode devuelve w = pi / dt, 100 puntos
f_eval = np.arange(0, 0.5, 0.0001) #de 0 a 1 en saltos de 0.01 de fsampling
w_tustin, mag_tustin, phase_tustin = dbode(planta_dig_tustin, n=1000)
w_euler, mag_euler, phase_euler = dbode(planta_dig_euler, n=1000)
fig, (ax1, ax2) = plt.subplots(2, 1)
def test_initialization(self):
# Check that all initializations work
s = TransferFunction(1, 1)
s = TransferFunction([1], [2])
s = TransferFunction(np.array([1]), np.array([2]))
def simulate(args):
# Left side constants
kv_l = 0.83 # Kv
ka_l = 0.1 # Ka
kp_l = args['kp'] # Kp
ki_l = args['ki'] # Ki
kd_l = args['kd'] # Kd
kf_v_l = args['kf'] # position feedforward
kf_p_l = 0 # velocity feedforward
# Right side constants
kv_r = 0.85
ka_r = 0.11
kp_r = kp_l
ki_r = ki_l
kd_r = kd_l
kf_v_r = kf_v_l
kf_p_r = 0
# create system model
left = TransferFunction([kd_l + kf_v_l, kp_l + kf_p_l, ki_l],
[ka_l, kd_l + kv_l, kp_l, ki_l])
right = TransferFunction([kd_r + kf_v_r, kp_r + kf_p_r, ki_r],
[ka_r, kd_r + kv_r, kp_r, ki_r])
# read in profile files
left_profile = prepare_profile('demoLeft.csv')
right_profile = prepare_profile('demoRight.csv')
dt = left_profile[0, 2]
dt_sim = 0.001
t_rr = np.arange(0, left_profile.shape[0] * dt,
dt) # time on RoboRIO (time for setpoint updates)
t = np.linspace(0, t_rr[-1],
np.floor(1 / dt_sim * dt *
left_profile.shape[0])) # time for simulation
# staircased trajectories for using in the simulation
u_left = staircase(left_profile, t, dt)
u_right = staircase(right_profile, t, dt)
# interpolated trajectories for error analysis
u_left_c = np.interp(t, t_rr, left_profile[:, 0])
u_right_c = np.interp(t, t_rr, right_profile[:, 0])
tout_l, y_l, x_l = lsim(left, u_left, t)
tout_r, y_r, x_r = lsim(right, u_right, t)
err_pid_l = y_l - u_left # raw error (the one the PID sees)
err_pid_r = y_r - u_right
err_lerp_l = y_l - u_left_c # error based off of the interpolated trajectory
err_lerp_r = y_r - u_right_c
prof_traj = plot_mp(u_left_c, u_right_c, dt_sim) # path from profile
actual_traj = plot_mp(y_l, y_r, dt_sim) # actual path followed
dev = deviation(prof_traj, actual_traj)
# Create ColumnDataSources for each plot
u_time = ColumnDataSource(data=dict(t=t, l=u_left, r=u_right))
y_time = ColumnDataSource(data=dict(t=t, l=y_l, r=y_r))
err_time = ColumnDataSource(data=dict(t=t, l=err_lerp_l, r=err_lerp_r))
pt_data = ColumnDataSource(data=dict(lx=prof_traj[:, 1],
ly=prof_traj[:, 2],
rx=prof_traj[:, 3],
ry=prof_traj[:, 4]))
at_data = ColumnDataSource(data=dict(lx=actual_traj[:, 1],
ly=actual_traj[:, 2],
rx=actual_traj[:, 3],
ry=actual_traj[:, 4]))
dev_data = ColumnDataSource(data=dict(t=t, l=dev[0], r=dev[1]))
return u_time, y_time, err_time, pt_data, at_data, dev_data
# Desde aca utilizo ceros y polos que entrego sympy #
#####################################################
planta = sympy_to_lti(Plant_out_sim)
filter_sense = sympy_to_lti(Filter_out_sim)
#######################################
# Convierto Planta y Filtro a Digital #
# por Tustin #
#######################################
Fsampling = 24000
Tsampling = 1 / Fsampling
planta_dig_tustin_n, planta_dig_tustin_d, td = cont2discrete((planta.num, planta.den), Tsampling, method='tustin')
#normalizo con TransferFunction
print ("Planta Digital:")
planta_dig_tustin = TransferFunction(planta_dig_tustin_n, planta_dig_tustin_d, dt=td)
print (planta_dig_tustin)
filter_dig_tustin_n, filter_dig_tustin_d, td = cont2discrete((filter_sense.num, filter_sense.den), Tsampling, method='tustin')
#normalizo con TransferFunction
print ("Filter Digital:")
filter_dig_tustin = TransferFunction(filter_dig_tustin_n, filter_dig_tustin_d, dt=td)
print (filter_dig_tustin)
########################
# Ecuacion PID Digital #
########################
## Parametros analogicos del PID (d100w_salida01.py)
kp = 0.000318
ki = 0.25
print('Numerador Planta Sympy: ' + str(planta.num))
print('Denominador Planta Sympy: ' + str(planta.den))
##############################
# Convierto Planta a Digital #
# por Tustin #
##############################
Fsampling = 2000
Tsampling = 1 / Fsampling
planta_dig_tustin_n, planta_dig_tustin_d, td = cont2discrete(
(planta.num, planta.den), Tsampling, method='tustin')
#normalizo con TransferFunction
print("Planta en Digital")
planta_dig_tustin = TransferFunction(planta_dig_tustin_n,
planta_dig_tustin_d,
dt=td)
print(planta_dig_tustin)
########################
# Ecuacion PID Digital #
########################
# kp = 3.96 #ziegler-nichols
# ki = 3960
# kd = 0.001
kp = 2
ki = 1000
kd = 0
ki_dig = ki / Fsampling
kp_dig = kp - ki_dig / 2
kd_dig = kd * Fsampling
pid_poly = lti_to_sympy(controller)
print("Raices del controlador:")
print(roots(pid_poly))
# k1, k2, k3 = PID_analog_digital(
### Convierto Controlador por Forward Euler
Fsampling = 1500
Tsampling = 1 / Fsampling
cont_n, cont_d, td = cont2discrete((controller.num, controller.den),
Tsampling,
method='euler')
#normalizo con TransferFunction
print(cont_n)
print(cont_d)
controller_d = TransferFunction(cont_n, cont_d, dt=td)
print(controller_d)
#dbode devuelve w = pi / dt, 100 puntos
f_eval = np.arange(0, 0.5, 0.0001) #de 0 a 1 en saltos de 0.01 de fsampling
# w, mag, phase = dbode(controller_d, w=f_eval*np.pi)
w, mag, phase = dbode(controller_d, n=1000)
# w, mag, phase = dbode(controller_d)
# print (w)
fig, (ax1, ax2) = plt.subplots(2, 1)
ax1.semilogx(w / (np.pi), mag, 'b')
ax1.set_title('PID Euler')
ax1.set_ylabel('Amplitude P D2 [dB]', color='b')
import numpy as np
import matplotlib.pyplot as plt
from scipy.signal import lfilter, TransferFunction, dimpulse
a = np.array([1, 7 / 140, -6 / 130, 0, -1 / 150, 1 / 150])
b = np.array([0, -6 / 20, -4 / 20, 0, 6 / 20, -4 / 20])
samples = 30
x = TransferFunction(b, a, dt=True)
signal_in = dimpulse(x, n=samples)
plt.figure(num=1, figsize=(8, 6))
plt.stem(signal_in[0], signal_in[1][0])
plt.title("Графік вихідного сигналу", fontsize=14)
plt.xlabel('Номер відліку', fontsize=10)
plt.ylabel('Амплітуда, В', fontsize=10)
plt.minorticks_on()
plt.grid(which='major', linewidth=1.2)
plt.grid(which='minor', linewidth=.5)
plt.show()
def test_imaginary(self):
# bode() should not fail on a system with pure imaginary poles.
# The test passes if bode doesn't raise an exception.
system = TransferFunction([1], [1, 0, 100], dt=0.1)
dbode(system, n=2)
def tfcascade(tfa, tfb):
tfc = TransferFunction( np.polymul(tfa.num, tfb.num), np.polymul(tfa.den, tfb.den) )
return tfc
Exemplo de filtro passa baixa utilizando Tustin
a
H(s) = -----------
(s + a)
S -> Z
H[Z] = Y[Z] / X[Z]
"""
W0 = 2 * pi * 1000
f0 = W0 / (2 * pi) # F0 em HZ
# w0 = 2*pi*f0;
# Definindo os coeficientes
num = [W0, 0]
den = [W0, 1]
H = TransferFunction(num, den)
print(type(H))
bode(H)
Fs = 8000
Ts = 1 / Fs
# Transforma de continuo para discreto, utilizando metodo de Tustin
Hd = cont2discrete(H, Ts, 'bilinear')
# Plotar em frequencia (Hz)
[H, w] = freqz(Hd.Numerator[0, 0], Hd.Denominator[0, 0], fs=Fs / (2 * pi))
figure(2)
plot(w, 20 * log10(abs(H)))
grid()
ax2.semilogx (w/(2*np.pi), phase, 'b-', linewidth="1")
ax2.set_title('Phase')
plt.tight_layout()
plt.show()
###########################################
# Multiplico Transferencias para OpenLoop #
###########################################
c = lti_to_sympy(pid)
p = lti_to_sympy(planta)
ol = c * p
open_loop = sympy_to_lti(ol)
open_loop = TransferFunction(open_loop.num, open_loop.den) #normalizo ol
if show_open_loop_bode:
w, mag_ol, phase_ol = bode(open_loop, freq)
fig, (ax1, ax2) = plt.subplots(2,1)
ax1.semilogx(w/(2*np.pi), mag_ol, 'b')
ax1.set_title('Analog OpenLoop')
ax1.set_ylabel('Amplitude P D2 [dB]', color='b')
# ax1.set_ylim([-40, 40])
ax2.semilogx(w/(2*np.pi), phase_ol, 'b')
ax2.set_ylabel('Phase', color='b')
ax2.set_xlabel('Frequency [Hz]')
plt.tight_layout()
from scipy.signal import TransferFunction, lti
numerator = 1.0
denominator = [-22.0, 0, 21.0 * 9.8]
tf = TransferFunction(lti(numerator, denominator))
print("Transfer Function: {0}".format(tf))
print("Poles: {0}".format(tf.poles))
print("Zeros: {0}".format(tf.zeros))
zero_input = tf.output(0)
print(zero_input)
ax2.set_title('Phase')
plt.tight_layout()
plt.show()
#######################################################
# Multiplico Transferencias para OpenLoop y CloseLoop #
#######################################################
c = lti_to_sympy(controller)
p = lti_to_sympy(planta)
ol = c * p
cl = ol / (1 + ol)
open_loop = sympy_to_lti(ol)
open_loop = TransferFunction(open_loop.num, open_loop.den) #normalizo ol
close_loop = sympy_to_lti(cl)
close_loop = TransferFunction(close_loop.num, close_loop.den) #normalizo cl
w, mag_ol, phase_ol = bode(open_loop, freq)
w, mag_cl, phase_cl = bode(close_loop, freq)
fig, (ax1, ax2) = plt.subplots(2, 1)
ax1.semilogx(w / (2 * np.pi), mag_ol, 'b')
ax1.semilogx(w / (2 * np.pi), mag_cl, 'y')
ax1.set_title('Analog OpenLoop Blue, CloseLoop Yellow')
ax1.set_ylabel('Amplitude P D2 [dB]', color='b')
ax1.set_xlabel('Frequency [Hz]')
ax1.set_ylim([-40, 40])
ax2.semilogx(w / (2 * np.pi), phase_ol, 'b')
if Polos_Ceros_Analog == True:
plot_s_plane(planta_TF)
##################################################
# Convierto Planta por ZOH a una frecuencia alta #
# para que no afecte polos o ceros #
##################################################
Fsampling = 20
Tsampling = 1 / Fsampling
planta_dig_zoh_n, planta_dig_zoh_d, td = cont2discrete(
(planta_TF.num, planta_TF.den), Tsampling, method='zoh')
#normalizo con TransferFunction
print("Planta Digital Zoh:")
planta_dig_zoh = TransferFunction(planta_dig_zoh_n, planta_dig_zoh_d, dt=td)
print(planta_dig_zoh)
w, mag_zoh, phase_zoh = dbode(planta_dig_zoh, n=10000)
if Bode_Planta_Digital == True:
fig, (ax1, ax2) = plt.subplots(2, 1)
ax1.semilogx(w / (2 * np.pi), mag_zoh, 'y')
ax1.set_title('Digital ZOH')
ax1.set_ylabel('Amplitude P D2 [dB]', color='g')
ax1.set_xlabel('Frequency [Hz]')
ax2.semilogx(w / (2 * np.pi), phase_zoh, 'y')
ax2.set_ylabel('Phase', color='g')
ax2.set_xlabel('Frequency [Hz]')
def test_properties(self):
# Test setters/getters for cross class properties.
# This implicitly tests to_ss() and to_zpk()
# Getters
s = TransferFunction([1, 0], [1, -1], dt=0.05)
assert_equal(s.poles, [1])
assert_equal(s.zeros, [0])
with warnings.catch_warnings():
warnings.simplefilter("ignore", DeprecationWarning)
assert_equal(s.gain, 1)
assert_equal(s.A, 1)
assert_equal(s.B, 1)
assert_equal(s.C, 1)
assert_equal(s.D, 1)
# state space setters
s2 = TransferFunction([2, 3], [4, 5], dt=0.05)
with warnings.catch_warnings():
warnings.simplefilter("ignore", DeprecationWarning)
s2.A = 1
s2.B = 1
s2.C = 1
s2.D = 1
self._compare_systems(s, s2)
# zpk setters
s2 = TransferFunction([2, 3], [4, 5], dt=0.05)
with warnings.catch_warnings():
warnings.simplefilter("ignore", DeprecationWarning)
s2.poles = 1
s2.zeros = 0
s2.gain = 1
self._compare_systems(s, s2)
def tfadd(tfa, tfb):
tfc = TransferFunction( np.polyadd(np.polymul(tfa.num,tfb.den),np.polymul(tfa.den,tfb.num)),
np.polymul(tfa.den,tfb.den) )
return tfc
ax2.set_title('Phase')
plt.tight_layout()
plt.show()
### Convierto Planta por Forward Euler
Fsampling = 2000
Tsampling = 1 / Fsampling
planta_dig_tustin_n, planta_dig_tustin_d, td = cont2discrete(
(planta.num, planta.den), Tsampling, method='tustin')
planta_dig_euler_n, planta_dig_euler_d, td = cont2discrete(
(planta.num, planta.den), Tsampling, method='euler')
#normalizo con TransferFunction
planta_dig_tustin = TransferFunction(planta_dig_tustin_n,
planta_dig_tustin_d,
dt=td)
print(planta_dig_tustin)
planta_dig_euler = TransferFunction(planta_dig_euler_n,
planta_dig_euler_d,
dt=td)
print(planta_dig_euler)
#dbode devuelve w = pi / dt, 100 puntos
f_eval = np.arange(0, 0.5, 0.0001) #de 0 a 1 en saltos de 0.01 de fsampling
w_tustin, mag_tustin, phase_tustin = dbode(planta_dig_tustin, n=1000)
w_euler, mag_euler, phase_euler = dbode(planta_dig_euler, n=1000)
fig, (ax1, ax2) = plt.subplots(2, 1)
Ubicacion Polos y Ceros y Respuesta en frecuencia
"""
########################################################
# Control Digital Custom, elijo Ceros Polos y Ganancia #
########################################################
Fsampling = 24000
cont_zeros = [0.916, -0.89 + 0.29j, -0.89 - 0.29j]
cont_poles = [1.0]
cont_const = 0.01
cont_zpk_b, cont_zpk_a = zpk2tf(cont_zeros, cont_poles, cont_const)
td = 1 / Fsampling
controller_tf = TransferFunction(cont_zpk_b, cont_zpk_a, dt=td)
print("Digital Controller:")
print(controller_tf)
#####################################
# Polos y Ceros del Control Digital #
#####################################
plot_argand(controller_tf)
########################
# Bode Control Digital #
########################
w, mag, phase = dbode(controller_tf, n=10000)
fig, (ax1, ax2) = plt.subplots(2, 1)
ax1.semilogx(w / (2 * np.pi), mag, 'c')
wsd = 2 * pi * fsa / fs
pwpa = 2 * tan(wpd / 2)
pwsa = 2 * tan(wsd / 2)
N, wn = buttord(pwpa, pwsa, rp, rs, True)
#N,wn=cheb1ord(pwpa,pwsa,rp,rs,True)
#b,a=cheby1(N,rp,wn,'low',True)
b, a = butter(N, wn, 'low', True)
Fs = 1
num, den = bilinear(b, a, Fs)
sys = TransferFunction(b, a, dt=1)
w, h = freqz(num, den, 128)
plt.plot((w * fs / (2 * pi)), 20 * log10(abs(h)))
plt.grid()
plt.title('Frequency response')
plt.xlabel('Gain magnitude')
plt.ylabel('Amplitude')
plt.show()
y = lfilter(num, den, x1)
plt.stem(y)
plt.title('Output response')