def convert_forward_euler(pa, Tsampling=0.01):
# if isintance(pa, TransferFunction):
if isinstance(pa, lti):
#reviso primero el valor final
s = Symbol('s')
pa_sympy = lti_to_sympy(pa)
final_value_analog = pa_sympy.subs(s, 0).evalf()
# print (' Final value: ' + str(final_value_analog))
#convierto backward euler
num_d, den_d, td = cont2discrete((pa.num, pa.den),
Tsampling,
method='euler')
zd, pd, kd = tf2zpk(num_d, den_d)
#agrego zeros infinitos
while (np.shape(zd) < np.shape(pd)):
zd = np.append(zd, [-1])
#normalizo los zeros
planta_d = ZerosPolesGain(zd, pd, kd)
planta_d = planta_d.to_tf()
zd, pd, kd = tf2zpk(planta_d.num, planta_d.den)
#convierto a sympy para evaluar el valor final y ajustarlo
planta_d_sympy = lti_to_sympy(planta_d)
z = Symbol('z')
planta_d_sympy = planta_d_sympy.subs(s, z)
final_value_d = planta_d_sympy.subs(z, 1).evalf()
#ahora ajusto la ganancia para que me coincidan los dos valores finales
kd = kd * final_value / final_value_d
# print ('Ceros digital: ' + str(zd))
# print ('Polos digital: ' + str(pd))
# print ('K digital: ' + str(kd))
#normalizo por ultima vez planta_d, ya agregue los zeros
#y ajuste la ganancia con los valores finales
planta_d = ZerosPolesGain(zd, pd, kd)
planta_d = planta_d.to_tf()
# print ('planta_d ' + str(planta_d))
#muestro el valor final
planta_d_sympy = lti_to_sympy(planta_d)
z = Symbol('z')
planta_d_sympy = planta_d_sympy.subs(s, z)
final_value_d = planta_d_sympy.subs(z, 1).evalf()
# print ('planta_d final value: ' + str(final_value_d))
#reconvierto planta_d a dlti
planta_d = dlti(planta_d.num, planta_d.den, dt=td)
return planta_d
else:
raise ValueError('planta_analog is not instance of TransferFunction!')
w, mag, phase = bode(pid, freq)
fig, (ax1, ax2) = plt.subplots(2,1)
ax1.semilogx (w/(2*np.pi), mag, 'b-', linewidth="1")
ax1.set_title('PID Tf - Magnitude')
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])
ax1.semilogx(w/(2*np.pi), mag, 'b')
ax1.set_title('PID Digital')
ax1.set_ylabel('Amplitude P D2 [dB]', color='b')
ax1.set_xlabel('Frequency [Hz]')
ax2.semilogx(w/(2*np.pi), phase, 'r')
ax2.set_ylabel('Phase', color='r')
ax2.set_xlabel('Frequency [Hz]')
plt.tight_layout()
plt.show()
############################################
# Multiplico Transferencias para OpenLoop #
############################################
c = lti_to_sympy(pid_dig)
p = lti_to_sympy(filter_dig_tustin)
ol = c * p
open_loop = sympy_to_lti(ol)
open_loop = TransferFunction(open_loop.num, open_loop.den, dt=td) #normalizo
w, mag, phase = dbode(open_loop, n = 10000)
fig, (ax1, ax2) = plt.subplots(2,1)
ax1.semilogx(w/(2*np.pi), mag, 'b')
ax1.set_title('Digital OpenLoop')
ax1.set_ylabel('Amplitude P D2 [dB]', color='b')
ax1.set_xlabel('Frequency [Hz]')
f_eval = np.arange(0.1, 1000, 0.5)
w, mag, phase = bode((b, a), w=f_eval * 2 * np.pi)
fig, (ax1, ax2) = plt.subplots(2, 1)
ax1.semilogx(w / (2 * np.pi), mag, 'b-', linewidth="1")
ax1.set_title('Magnitude')
ax2.semilogx(w / (2 * np.pi), phase, 'r-', linewidth="1")
ax2.set_title('Phase')
plt.tight_layout()
plt.show()
controller = lti(b, a) #normalizo
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)
w, mag, phase = bode(pid, freq)
fig, (ax1, ax2) = plt.subplots(2,1)
ax1.semilogx (w/(2*np.pi), mag, 'b-', linewidth="1")
ax1.set_title('PID Tf - Magnitude')
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')
print('Polos Digitales ' + str(planta_d1_zpk.poles))
print('Gain Digital ' + str(planta_d1_zpk.gain))
# convierto a planta_d2 ajusto al sistema digital 2 zeros en infinito y corrijo la ganancia
zd2, pd2, kd2 = tf2zpk(num_d1, den_d1)
while (np.shape(zd2) < np.shape(pd2)):
zd2 = np.append(zd2, [-1])
#normalizo
planta_d2 = ZerosPolesGain(zd2, pd2, kd2)
planta_d2 = planta_d2.to_tf()
zd2, pd2, kd2 = tf2zpk(planta_d2.num, planta_d2.den)
#convierto a sympy para evaluar el valor final y ajustarlo
planta_d2_sympy = lti_to_sympy(planta_d2)
z = Symbol('z')
planta_d2_sympy = planta_d2_sympy.subs(s, z)
final_value_d2 = planta_d2_sympy.subs(z, 1).evalf()
#ahora ajusto la ganancia para que me coincidan los dos valores finales
kd2 = kd2 * final_value / final_value_d2
print('Ceros digital: ' + str(zd2))
print('Polos digital: ' + str(pd2))
print('K digital: ' + str(kd2))
#normalizo por ultima vez planta_d2, ya agregue los zeros y ajuste la ganancia con los valores finales
planta_d2 = ZerosPolesGain(zd2, pd2, kd2)
planta_d2 = planta_d2.to_tf()
print('planta_d2 ' + str(planta_d2))
planta_d2_sympy = lti_to_sympy(planta_d2)
method='euler')
# num_d, den_d, td = cont2discrete((planta.num, planta.den), Tsampling, method='zoh')
# num_d, den_d, td = cont2discrete((planta.num, planta.den), Tsampling, method='backward_diff')
# num_d, den_d, td = cont2discrete((planta.num, planta.den), Tsampling, method='bilinear')
# p_digital = convert_forward_euler(planta, Tsampling)
# num_d = p_digital.num
# den_d = p_digital.den
# td = p_digital.dt
print('Numerador Digital planta out 1 ' + str(num_d))
print('Denominador Digital planta out 1 ' + str(den_d))
planta_d = dlti(num_d, den_d, dt=td)
#convierto a sympy para evaluar el valor final
planta_d_sympy = lti_to_sympy(planta_d)
z = Symbol('z')
planta_d_sympy = planta_d_sympy.subs(s, z)
final_value_d = planta_d_sympy.subs(z, 1).evalf()
print('planta_d final value: ' + str(final_value_d))
# en frecuencia
w, h = freqz(planta_d.num, planta_d.den)
fig, (ax1, ax2) = plt.subplots(2, 1)
ax1.semilogx(w / (2 * pi) * Fsampling, 20 * np.log10(abs(h)), 'b')
ax1.set_ylabel('Amplitude [dB]', color='b')
ax1.set_xlabel('Frequency [Hz]')
angles = np.unwrap(np.angle(h))
ax2.semilogx(w / (2 * pi) * Fsampling, angles * 180 / pi, 'b-', linewidth="1")
w, mag, phase = bode(controller, freq)
fig, (ax1, ax2) = plt.subplots(2, 1)
ax1.semilogx(w / (2 * np.pi), mag, 'b-', linewidth="1")
ax1.set_title('PID Tf - Magnitude')
ax2.semilogx(w / (2 * np.pi), phase, 'r-', linewidth="1")
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')
if Bode_Controlador_Digital == True:
fig, (ax1, ax2) = plt.subplots(2, 1)
ax1.semilogx(w / (2 * np.pi), mag, 'c')
ax1.set_title('Digital Controller')
ax1.set_ylabel('Amplitude P D2 [dB]', color='c')
ax1.set_xlabel('Frequency [Hz]')
ax2.semilogx(w / (2 * np.pi), phase, 'c')
ax2.set_ylabel('Phase', color='c')
ax2.set_xlabel('Frequency [Hz]')
plt.tight_layout()
plt.show()
if Bode_Sensor_OpenLoop_CloseLoop_Digital == True:
contr_dig = lti_to_sympy(controller_tf)
plant_dig = lti_to_sympy(planta_dig_zoh)
ol_dig = plant_dig * contr_dig
open_loop_dig = sympy_to_lti(ol_dig)
close_loop_dig = sympy_to_lti(ol_dig / (1 + ol_dig))
#normalizo con TransferFunction
open_loop_dig = TransferFunction(open_loop_dig.num,
open_loop_dig.den,
dt=td)
close_loop_dig = TransferFunction(close_loop_dig.num,
close_loop_dig.den,
dt=td)
w, mag_ol, phase_ol = dbode(open_loop_dig, n=10000)
w, mag, phase = bode(pid, freq)
fig, (ax1, ax2) = plt.subplots(2,1)
ax1.semilogx (w/(2*np.pi), mag, 'b-', linewidth="1")
ax1.set_title('PID Tf - Magnitude')
ax2.semilogx (w/(2*np.pi), phase, 'r-', linewidth="1")
ax2.set_title('Phase')
plt.tight_layout()
plt.show()
#######################################################
# Multiplico Transferencias para OpenLoop y CloseLoop #
#######################################################
c = lti_to_sympy(pid)
p = lti_to_sympy(sensado)
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('Sensado - Open Loop Tf - Magnitude')
ax2.semilogx (w/(2*np.pi), phase, 'r-', linewidth="1")
ax2.set_title('Phase')