#TF without constant s = Symbol('s') Plant_num = Final_value * wn**2 / Input_step_value Plant_den = s**2 + 2 * psi * wn * s + wn**2 Plant_out = Plant_num / Plant_den Plant_out_sim = Plant_out.simplify() # print ('Plant_out: ') # print (Plant_out_sim) ##################################################### # Desde aca utilizo ceros y polos que entrego sympy # ##################################################### # planta = sympy_to_lti(Plant_out_sim) sensado = sympy_to_lti(Plant_out_sim * sense_probe_alpha / 3.3) # print ("planta con sympy:") # print (planta) ########################################################## # Convierto Planta Digital - Sensado Digital en realidad # # por Tustin # ########################################################## Fsampling = 24000 Tsampling = 1 / Fsampling planta_dig_tustin_n, planta_dig_tustin_d, td = cont2discrete( (sensado.num, sensado.den), Tsampling, method='tustin') #normalizo con TransferFunction print("Planta Digital:") planta_dig_tustin = TransferFunction(planta_dig_tustin_n,
#TF equation Voltage and Lon output and voltage on Rsense s = Symbol('s') Plant_num = (1 + s * Cout * RCout) * (1 - s * Leq / Rload) Plant_den = 1 + s / (w0 * Q) + s**2 / w0**2 Plant_out = alpha * (Vi / (1 - Duty)**2) * (Plant_num / Plant_den) Plant_out_sim = Plant_out.simplify() print('Plant_out: ') print(Plant_out_sim) ##################################################### # Desde aca utilizo ceros y polos que entrego sympy # ##################################################### planta = sympy_to_lti(Plant_out_sim) print("planta con sympy:") print(planta) ######################################## # Respuesta en Frecuencia de la Planta # ######################################## freq = np.arange(1, 100000, 1) w, mag, phase = bode(planta, freq) fig, (ax1, ax2) = plt.subplots(2, 1) ax1.semilogx(w / (2 * np.pi), mag, 'b-', linewidth="1") ax1.set_title('Plant Tf - Magnitude') ax2.semilogx(w / (2 * np.pi), phase, 'r-', linewidth="1") ax2.set_title('Phase')
Tfiltro = 1 / (s * Cf * Rf + 1) # Iout = TY1 / Rsense # Iout_sim = Iout.simplify() Isense = TY1 * Tfiltro Isense_sim = Isense.simplify() # print ('Iout: ') # print (Iout_sim) print('Isense: ') print(Isense_sim) final_value = Isense_sim.subs(s, 0).evalf() print('Isense_sim Final value: ' + str(final_value)) planta = sympy_to_lti(Isense_sim) print('Numerador Planta Sympy: ' + str(planta.num)) print('Denominador Planta Sympy: ' + str(planta.den)) ### Desde aca utilizo ceros y polos que entrego sympy freq = np.arange(1, 10000, 0.01) w, mag, phase = bode(planta, freq) fig, (ax1, ax2) = plt.subplots(2, 1) ax1.semilogx(w / (2 * pi), mag, 'b-', linewidth="1") ax1.set_title('Magnitude') ax2.semilogx(w / (2 * pi), phase, 'b-', linewidth="1") ax2.set_title('Phase') plt.tight_layout()
Vin = 35 #etapa de potencia s = Symbol('s') TY1 = Rsense / (s * L + Rd + Rsense) #etapa de filtro Tfiltro = 1 / (s * Cf * Rf + 1) Iout = TY1 / Rsense Iout_sim = Iout.simplify() Isense = TY1 * Tfiltro Isense_sim = Isense.simplify() planta_out = sympy_to_lti(Iout_sim) planta_real = sympy_to_lti(Isense_sim) ### Pruebo step con d=0.9 t_linear = np.linspace(0, 0.01, num=2000) u = np.ones_like(t_linear) u = u * (0.9 * Vin - Vd) t, y, x = lsim(planta_real, T=t_linear, U=u) t, y1, x1 = lsim(planta_out, T=t_linear, U=u) # fig, ax = plt.subplots() # ax.set_title('Respuesta escalon') # ax.set_ylabel('Vsense') # ax.set_xlabel('Tiempo [s]') # ax.grid() # ax.plot(t, y, 'r-')
from tc_udemm import sympy_to_lti, lti_to_sympy from sympy import * s = Symbol('s') a = 0.5 - 0.7j b = 0.5 + 0.7j H = 1 / ((s + a) * (s + b)) Tsim = H.simplify( ) # Esta función me permite ver como queda acomodada la función transferencia de H*Gp print(Tsim) #convierto sympy a lti planta = sympy_to_lti(H) ### Pruebo step t = np.linspace(0, 20, num=2000) # u = np.ones_like(t) # tp, y, x = lsim(planta, T=t, U=u) tp, y = step(planta, T=t) # fig, ax = plt.subplots() # ax.set_title('Respuesta escalon') # ax.set_ylabel('V') # ax.set_xlabel('Tiempo [s]') # ax.grid() # ax.plot(tp, y, 'b-') # plt.tight_layout()
#resultados de la etapa de potencia s = Symbol('s') Iout = Vpwm / (s * L + R + Rsense) Vsense = Iout * Rsense Plant_out = Iout Plant_sense = Vsense * Aopamp Plant_out_sim = Plant_out.simplify() Plant_sense_sim = Plant_sense.simplify() print('Plant_out: Iout: ') print(Plant_out_sim) print('Plant_sense: Vsense in opamp: ') print(Plant_sense_sim) planta = sympy_to_lti(Plant_out_sim) print('Numerador Planta Sympy: ' + str(planta.num)) print('Denominador Planta Sympy: ' + str(planta.den)) ############################## # 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')
Vpwm = 1 #resultados de la etapa de potencia s = Symbol('s') Zout = Rload / (s**2 * 2*Lout*Cout*Rload + s*2*Lout + Rload) Plant_out = Vpwm * Zout Plant_out_sim = Plant_out.simplify() print ('Plant_out: ') print (Plant_out_sim) ##################################################### # Desde aca utilizo ceros y polos que entrego sympy # ##################################################### planta = sympy_to_lti(Plant_out_sim) print ("planta con sympy:") print (planta) freq = np.arange(1, 1000000, 1) w, mag, phase = bode(planta, freq) fig, (ax1, ax2) = plt.subplots(2,1) ax1.semilogx (w/6.28, mag, 'b-', linewidth="1") ax1.set_title('Magnitude') ax2.semilogx (w/6.28, phase, 'r-', linewidth="1") ax2.set_title('Phase') plt.tight_layout()
plt.show() #PID con kp ki kd k = 10 z1 = -100 z2 = -1000 #ojo si no tengo segundo zero cambia la ecuacion!!! kp = k * (-z2 - z1) ki = k * z2 * z1 kd = k s = Symbol('s') pid_poly = (s**2 * kd + s * kp + ki) / s print(pid_poly) pid_tf = sympy_to_lti(pid_poly) w, mag, phase = bode((pid_tf.num, pid_tf.den), 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() ### desde Kp Ki Kd a zpk ### z, p, k = tf2zpk(pid_tf.num, pid_tf.den)
Transf_Z3_Vinput = (Z2 * Z3 /(Z2 + Z3)) / (Z1 + (Z2 * Z3 /(Z2 + Z3))) * Vinput Filter_out_sim = Transf_Z3_Vinput.simplify() print (Filter_out_sim) # Opamp # print (f'Filtro T con Opamp en Rsense Vinput = {Vinput}V: ') # Transf_Z4_Vinput_Opamp = Transf_Z4_Vinput * Amp_gain # Filter_Opamp_out_sim = Transf_Z4_Vinput_Opamp.simplify() # print (Filter_Opamp_out_sim) ################################## # Some checks on the Analog part # ################################## filter_TF = sympy_to_lti(Filter_out_sim) if Bode_Analog_Plant == True: wfreq = np.arange(2*np.pi, 2*np.pi*100000, 1) w, mag_p, phase_p = bode(filter_TF, wfreq) fig, (ax1, ax2) = plt.subplots(2,1) ax1.semilogx (w/6.28, mag_p, 'b-', linewidth="1") # ax1.semilogx (w/6.28, mag_s, 'g-', linewidth="1") ax1.set_title(f'Magnitude Input to Output Filter Analog Vinput = {Vinput}V') ax2.semilogx (w/6.28, phase_p, 'b-', linewidth="1") # ax2.semilogx (w/6.28, phase_s, 'g-', linewidth="1") ax2.set_title('Phase') plt.tight_layout() plt.show()
Pd_out = kp + s*kd Pd_out_sim = Pd_out.simplify() Pid_out = kp + ki/s + s*kd Pid_out_sim = Pid_out.simplify() print ('PI_out: ') print (Pi_out_sim) print ('PD_out: ') print (Pd_out_sim) print ('PID_out: ') print (Pid_out_sim) ############################################## # Grafico de Bode con Polos y Ceros de sympy # ############################################## pi = sympy_to_lti(Pi_out_sim) pd = sympy_to_lti(Pd_out_sim) pid = sympy_to_lti(Pid_out_sim) print ("PI Tf:") print (pi) print ("PD Tf:") print (pd) print ("PID Tf:") print (pid) freq = np.arange(1, 1000000, 1) w, mag_pi, phase_pi = bode(pi, freq) w, mag_pd, phase_pd = bode(pd, freq) w, mag_pid, phase_pid = bode(pid, freq)
Zsense_sim = Zsense.simplify() print ("Zsense:") print (Zsense_sim) # #etapa de potencia Zeq = Zsense / (s * L + Rd + Zsense) print ("Zeq:") print (Zeq) ZVIsense = Zeq * 1 /(1 + s*Cf*Rf) print ("ZVIsense:") print (ZVIsense) Vin = Vpwm - Vd #saco aca factor d y que multiplique todo planta = sympy_to_lti(0.9* Vin * ZVIsense) print ('Numerador Planta Sympy: ' + str(planta.num)) print ('Denominador Planta Sympy: ' + str(planta.den)) # Bode - Open Loop - freq = np.arange(1, 10000, 0.01) w, mag, phase = bode(planta, freq) fig, (ax1, ax2) = plt.subplots(2,1) ax1.semilogx (w/(2*pi), mag, 'b-', linewidth="1") ax1.set_title('Magnitude') ax2.semilogx (w/(2*pi), phase, 'r-', linewidth="1") ax2.set_title('Phase')
################# kp = 0.01 ki = 1 kd = 0.000001 s = Symbol('s') Pid_out = kp + ki / s + s * kd Pid_out_sim = Pid_out.simplify() print('Pid_out: ') print(Pid_out_sim) ############################################## # Grafico de Bode con Polos y Ceros de sympy # ############################################## pid = sympy_to_lti(Pid_out_sim) print("PID con sympy:") print(pid) freq = np.arange(1, 1000000, 1) 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()
from scipy.signal import cont2discrete, freqz, ZerosPolesGain, TransferFunction from tc_udemm import sympy_to_lti, lti_to_sympy from math import pi, sin """ Desde la funcion en s busco ceros polos y ganancia, luego digitalizo """ #ecuacion transferencia en s s = Symbol('s') f_s = (s + 1) / ((s + 2) * (s + 3)) final_value = f_s.subs(s, 0).evalf() print('Final value: ' + str(final_value)) #convierto a funcion lti planta = sympy_to_lti(f_s) print('Numerador Planta Sympy: ' + str(planta.num)) print('Denominador Planta Sympy: ' + str(planta.den)) #busco zeros polos y ganancia z, p, k = tf2zpk(planta.num, planta.den) # print ('Ceros: ' + str(planta.zeros)) print('Ceros: ' + str(z)) # print ('Polos: ' + str(planta.poles)) print('Polos: ' + str(p)) print('K: ' + str(k)) ### Desde aca utilizo ceros y polos que entrego sympy ### Convierto a sistema Digital ### Convierto Forward Euler Fsampling = 5
# Complex variable s = Symbol('s') # integrador ganancia y polo num = (s + 62.8) den = s*(s + 628) gain = 800*1 plant_sympy = gain * num / den fmin = 0.1 fmax = 10000 tiempo_de_simulacion = 3.0 plant_tf = sympy_to_lti(plant_sympy) print (plant_tf) ###################### # Frequency Response # ###################### if Frequency_Response == True: wfreq = np.arange(fmin*2*np.pi, fmax*2*np.pi, fmin) w, mag_ol, phase_ol = bode(plant_tf, wfreq) fig, (ax1, ax2) = plt.subplots(2,1) ax1.semilogx (w/6.28, mag_ol, 'b-', linewidth="1") ax1.set_title('Magnitude') ax2.semilogx (w/6.28, phase_ol, 'b-', linewidth="1")
Rd = 2.93 #Alimentacion del PWM Vpwm = 35 #etapa de potencia s = Symbol('s') TY1 = Rsense / (s * L + Rd + Rsense) #etapa de filtro Tfiltro = 1 / (s * Cf * Rf + 1) Isense = TY1 * Tfiltro Isense_sim = Isense.simplify() planta_real = sympy_to_lti(Isense_sim) ### Pruebo step con d=0.9 t_linear = np.linspace(0, 0.01, num=2000) u = np.ones_like(t_linear) u = u * (0.9 * Vpwm - Vd) t, y, x = lsim(planta_real, T=t_linear, U=u) ### Desde aca sistema Digital ### Convierto bilinear Fsampling = 4800 Tsampling = 1 / Fsampling num_d1, den_d1, td = cont2discrete((planta_real.num, planta_real.den), Tsampling, method='bilinear')
print() psi = two_psi_wn / (2 * wn) print("psi:") print(psi) print() if psi < 1: overshoot = exp((-np.pi) * psi / (sqrt(1 - psi**2))) print("overshoot:") print(overshoot) else: print("no overshoot") print() snubberless_system = sympy_to_lti(snubberless_system_s) print(snubberless_system) ### respuesta escalon del diodo de salida t = np.linspace(0, 200e-9, 10000) # T, yout = step2(snubberless_system, T=t) T, yout = step(snubberless_system, T=t) fig, ax = plt.subplots() ax.plot(T, yout, 'r-', linewidth=2, label=r'$y=\sin(x)$', alpha=0.6) ax.legend(loc='upper right') # ax.set_yticks([-1, 0, 1]) ax.set_title('Test plot') plt.show() ### Cicuito con snubber
#resultados de la etapa de potencia s = Symbol('s') Iout = Vpwm / (s * L + R + Rsense) Vsense = Iout * Rsense Plant_out = Iout Plant_sense = Vsense * Aopamp Plant_out_sim = Plant_out.simplify() Plant_sense_sim = Plant_sense.simplify() print('Plant_out: Iout: ') print(Plant_out_sim) print('Plant_sense: Vsense in opamp: ') print(Plant_sense_sim) planta = sympy_to_lti(Plant_out_sim) print('Numerador Planta Sympy: ' + str(planta.num)) print('Denominador Planta Sympy: ' + str(planta.den)) z, p, k = tf2zpk(planta.num, planta.den) print('Planta Ceros: ' + str(planta.zeros)) print('Planta Polos: ' + str(planta.poles)) print('Planta K: ' + str(k)) # ### Muestro la respuesta escalon de la planta a lazo abierto # t = np.linspace(0, 0.1, num=2000) t, y = step2(planta, T=t) fig, ax = plt.subplots() ax.set_title('Respuesta de la Planta')
b = 75 F = 2250 #fuerza en N para desplazar 30m/s #### sistema #### # F = m . a # m . d2(x)/d2t = F - b . v # a = d(v)/dt # m . d(v)/dt + b . v = F # L[H(t)] = m . s Vs + b Vs = Fs # Vs = Fs /(m . s + b) Gpedal = F Gp = 1 / (m * s + b) # Gp = F / (m * s + b) print(Gp.simplify()) #convierto sympy a lti planta = sympy_to_lti(Gp * Gpedal) ### Pruebo step t = np.linspace(0, 120, num=2000) u = np.ones_like(t) fopen_loop = u * F tp, yopen_loop, x = lsim(planta, T=t, U=u) Kp = 0.2 Ki = 0.05 Gc = Kp + Ki / s controlador = sympy_to_lti(Gc) System = Gc * Gpedal * Gp / (1 + Gc * Gpedal * Gp) Force = Gc * Gpedal / (1 + Gc * Gpedal * Gp)
ax1.set_ylim([0, 65]) ax2.semilogx(w / (np.pi), phase, 'r') ax2.set_ylabel('Phase', color='r') ax2.set_xlabel('Frequency [Hz]') plt.tight_layout() plt.show() #Multiplico para OpenLoop c = lti_to_sympy(controller_d) p = lti_to_sympy(planta_d2) 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=1000) fig, (ax1, ax2) = plt.subplots(2, 1) ax1.semilogx(w / (np.pi), mag, 'b') ax1.set_title('Digital OpenLoop') ax1.set_ylabel('Amplitude P D2 [dB]', color='b') ax1.set_xlabel('Frequency [Hz]') ax2.semilogx(w / (np.pi), phase, 'r') ax2.set_ylabel('Phase', color='r') ax2.set_xlabel('Frequency [Hz]')
Bode_Sensor_OpenLoop_CloseLoop_Digital = False Polos_Ceros_CloseLoop_Digital = False Escalon_CloseLoop_Digital = False Escalon_CloseLoop_Original_Digital = True # TF s = Symbol('s') Gs = 1 /(s**2 + 6*s + 10) print ('Plant_out:') print (Gs) ##################################################### # Desde aca utilizo ceros y polos que entrego sympy # ##################################################### planta_TF = sympy_to_lti(Gs) print ("planta transfer function:") print (planta_TF) ##################### # Bode de la Planta # ##################### fmin = 0.01 fmax = 1000 wfreq = np.arange(2*np.pi*fmin, 2*np.pi*fmax, fmin) w, mag_p, phase_p = bode(planta_TF, wfreq) if Bode_Planta_Analog == True: fig, (ax1, ax2) = plt.subplots(2,1) ax1.semilogx (w/6.28, mag_p, 'b-', linewidth="1") ax1.set_title('Magnitude')
Plant_out = Vpwm * Zout_load Plant_no_vi_out = Zout_load Plant_out_sim = Plant_out.simplify() Plant_no_vi_out_sim = Plant_no_vi_out.simplify() print ('Plant_out: ') print (Plant_out_sim) print ('Plant_vi_out: ') print (Plant_no_vi_out_sim) ##################################################### # Desde aca utilizo ceros y polos que entrego sympy # ##################################################### planta = sympy_to_lti(Plant_out_sim) planta_no_vi = sympy_to_lti(Plant_no_vi_out_sim) #################################################### # Respuesta Escalon de la planta al Duty Propuesto # #################################################### if show_plant_step: t = np.linspace(0, 0.2, num=2000) u = np.ones(t.size) * Duty t, y, x = lsim(planta, T=t, U=u) fig, ax = plt.subplots() ax.set_title('Plant - Step Response') ax.set_ylabel('Vout') ax.set_xlabel('Tiempo [s]') ax.grid()
Bode_Sensor_OpenLoop_CloseLoop_Digital = True Polos_Ceros_CloseLoop_Digital = True Escalon_CloseLoop_Digital = False Escalon_CloseLoop_Original_Digital = True # TF s = Symbol('s') Gs = 1 / (s**2 + 6 * s + 10) print('Plant_out:') print(Gs) ##################################################### # Desde aca utilizo ceros y polos que entrego sympy # ##################################################### planta_TF = sympy_to_lti(Gs) print("planta transfer function:") print(planta_TF) ##################### # Bode de la Planta # ##################### fmin = 0.01 fmax = 1000 wfreq = np.arange(2 * np.pi * fmin, 2 * np.pi * fmax, fmin) w, mag_p, phase_p = bode(planta_TF, wfreq) if Bode_Planta_Analog == True: fig, (ax1, ax2) = plt.subplots(2, 1) ax1.semilogx(w / 6.28, mag_p, 'b-', linewidth="1") ax1.set_title('Magnitude')
#resultados de la etapa de potencia s = Symbol('s') Iout = Vpwm / (s * L + R + Rsense) Vout = Iout * Rsense Plant_out = Vout * Aopamp Iout_sim = Iout.simplify() Vout_sim = Vout.simplify() Plant_out_sim = Plant_out.simplify() print('Plant_out: ') print(Plant_out_sim) ### Desde aca utilizo ceros y polos que entrego sympy planta = sympy_to_lti(Plant_out_sim) print("planta con sympy:") print(planta) freq = np.arange(1, 10000, 0.01) w, mag, phase = bode(planta, freq) fig, (ax1, ax2) = plt.subplots(2, 1) ax1.semilogx(w / 6.28, mag, 'b-', linewidth="1") ax1.set_title('Magnitude') ax2.semilogx(w / 6.28, phase, 'r-', linewidth="1") ax2.set_title('Phase') plt.tight_layout()
#TF without constant s = Symbol('s') Plant_num = Final_value * wn**2 / Input_step_value Plant_den = s**2 + 2 * psi * wn * s + wn**2 Plant_out = Plant_num/Plant_den Plant_out_sim = Plant_out.simplify() print ('Plant_out: ') print (Plant_out_sim) ##################################################### # Desde aca utilizo ceros y polos que entrego sympy # ##################################################### planta = sympy_to_lti(Plant_out_sim) sensado = sympy_to_lti(Plant_out_sim * sense_probe_alpha) print ("planta con sympy:") print (planta) ############################################### # Respuesta escalon de la planta y el sensado # ############################################### tiempo_de_simulacion = 0.1 t = np.linspace(0, tiempo_de_simulacion, num=2000) t, y = step2(planta, T=t) yp = y * Input_step_value t, y = step2(sensado, T=t) ys = y * Input_step_value