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')