def Ftrminmax(lK, Rg, alpha, betas, beta, delta, omegat, pos): a = np.amin(Ftrxsum(lK, Rg, alpha, betas, beta, delta, omegat, pos)) b = np.amax(Ftrxsum(lK, Rg, alpha, betas, beta, delta, omegat, pos)) c = np.amin(Ftrysum(lK, Rg, alpha, betas, beta, delta, omegat, pos)) d = np.amax(Ftrysum(lK, Rg, alpha, betas, beta, delta, omegat, pos)) # print np.abs(a+b)+np.abs(c+d) return (np.abs(a + b) + np.abs(c + d))
def evaluate_design(self, x_denorm): with open('_eval/ACMConfig.h', 'r') as f: new_line = [] for line in f.readlines(): if '#define CURRENT_LOOP_BANDWIDTH' in line: new_line.append( f'#define CURRENT_LOOP_BANDWIDTH (2*M_PI*{x_denorm[0]:g})\n' ) # 15 elif '#define DELTA_THE_DAMPING_FACTOR' in line: new_line.append( f'#define DELTA_THE_DAMPING_FACTOR ({x_denorm[1]:g})\n' ) # 0.08 else: new_line.append(line) with open('_eval/ACMConfig.h', 'w') as f: f.writelines(new_line) if os.path.exists(self.work_dir + '/_eval/main.exe'): os.remove(self.work_dir + '/_eval/main.exe') subprocess.run([ "gcc", "_eval/main.c", "_eval/controller.c", "_eval/observer.c", "-o", "_eval/main" ]) while not os.path.exists(self.work_dir + '/_eval/main.exe'): time.sleep(0.1) print('sleep for main.exe') # subprocess.run( [self.work_dir+'/_eval/main.exe'] ) os.system('cd _eval && main') while not os.path.exists(self.work_dir + '/_eval/pi_opti.dat'): time.sleep(0.1) print('sleep for .dat') speed_profile = np.loadtxt('_eval/pi_opti.dat', skiprows=1) os.remove(self.work_dir + '/_eval/pi_opti.dat') TS = 0.000025 # ACMConfig.h print(max(speed_profile), 'rpm') print(min(speed_profile, key=lambda x: abs(x - 90)), 'rpm') # [rpm] print(np.abs(speed_profile - 90).argmin() * TS * 1000, 'ms') # [ms] rise_time = np.abs(speed_profile - 90).argmin() * TS * 1000 over_shoot = max(speed_profile) if over_shoot <= 100: over_shoot = 100 # modify this function to call matlab and eMach to evaluate the design with free variables as in x_denorm return over_shoot, rise_time
def plot_weightings(): """Plots all weighting functions defined in :module: splweighting.""" from scipy.signal import freqz from pylab import plt, np sample_rate = 48000 num_samples = 2 * 4096 fig, ax = plt.subplots() for name, weight_design in sorted(_weighting_coeff_design_funsd.items()): b, a = weight_design(sample_rate) w, H = freqz(b, a, worN=num_samples) freq = w * sample_rate / (2 * np.pi) ax.semilogx(freq, 20 * np.log10(np.abs(H) + 1e-20), label='{}-Weighting'.format(name)) plt.legend(loc='lower right') plt.xlabel('Frequency / Hz') plt.ylabel('Damping / dB') plt.grid(True) plt.axis([10, 20000, -80, 5]) return fig, ax
def plot_weightings(): """Plots all weighting functions defined in :module: splweighting.""" from scipy.signal import freqz from pylab import plt, np sample_rate = 48000 num_samples = 2*4096 fig, ax = plt.subplots() for name, weight_design in sorted( _weighting_coeff_design_funsd.items()): b, a = weight_design(sample_rate) w, H = freqz(b, a, worN=num_samples) freq = w*sample_rate / (2*np.pi) ax.semilogx(freq, 20*np.log10(np.abs(H)+1e-20), label='{}-Weighting'.format(name)) plt.legend(loc='lower right') plt.xlabel('Frequency / Hz') plt.ylabel('Damping / dB') plt.grid(True) plt.axis([10, 20000, -80, 5]) return fig, ax
def c2v_design(R, L, n_pp, J_s, KE, B=0, CLBW_Hz=1000, CL_TS=1 / 20e3, fignum=5): currentKp, currentKi = get_coeffs_dc_motor_current_regulator(R, L, CLBW_Hz) currentKiCode = currentKi * currentKp * CL_TS if True: # 这里打印的用于实验中CCS的debug窗口检查电流环PI系数 上位机电流KP = CLBW_Hz 上位机电流KI = 1000 iSMC_currentKp = 上位机电流KP * L * 2 * np.pi iSMC_currentKi = 上位机电流KI / 1000 * R / L iSMC_currentKiCode = iSMC_currentKi * CL_TS * iSMC_currentKp print(f'\tiSMC_currentKp={iSMC_currentKp:g}, \ iSMC_currentKi={iSMC_currentKi:g}, \ iSMC_currentKiCode={iSMC_currentKiCode:g}') print(f'\tSimC_currentKp={currentKp:g}, \ SimC_currentKi={currentKi:g}, \ SimC_currentKiCode={currentKiCode:g}') print(f'\t上位机电流KP={上位机电流KP:g}, \ 上位机电流KI={上位机电流KI:g}') Gi_closed = control.tf( [1], [L / currentKp, 1]) # current loop zero-pole cancelled already currentBandwidth_radPerSec = currentKp / L KT = 1.5 * n_pp * KE dc_motor_motion = control.tf([KT], [J_s / n_pp, B]) # [Apk] to [elec.rad/s] print(dc_motor_motion) # quit() # 注意,我们研究的开环传递函数是以电流给定为输入的,而不是以转速控制误差为输入,这样仿真和实验容易实现一点。 # Gw_open = dc_motor_motion * Gi_closed * speedPI c2v_tf = dc_motor_motion * Gi_closed # fig5 = plt.figure(fignum) # plt.title('Designed Current Ref. to Velocity Meas. Transfer Function') mag, phase, omega = control.bode_plot(c2v_tf, 2 * np.pi * np.logspace(0, 4, 500), dB=1, Hz=1, deg=1, lw='0.5', label=f'{CLBW_Hz:g} Hz') open_cutoff_frequency_HZ = omega[(np.abs(mag - 0.0)).argmin()] / 2 / np.pi # print('\tCut-off frequency (without speed PI regulator):', open_cutoff_frequency_HZ, 'Hz') return (currentKp, currentKi), \ (上位机电流KP, 上位机电流KI), \ (mag, phase, omega)
def find_nearest(array, value): array = np.asarray(array) idx = (np.abs(array - value)).argmin() return idx, array[idx]
def reflection1(xvals, parms): #position=p[0]; intensity=p[1]; slit=p[2]; stth=p[3]; background=p[4]; thickness=p[5]; absorption=p[6] x0 = parms[0] i0 = parms[1] w = parms[2] theta = parms[3] ib = parms[4] thick = parms[5] u = parms[6] th = np.deg2rad(theta) p = w * np.sin(th) Atot = w * w / np.sin(2.0 * th) out = [] ni = 5 irng = np.array(range(1, ni + 1)) for x in xvals: if x < (x0 - p): val = ib elif ((x0 - p) < x and x0 > x): l1 = x - (x0 - p) nrleft = int(np.ceil(l1 / (2 * p) * ni)) if nrleft < 1: nrleft = 1 irngleft = np.array(range(1, nrleft + 1)) dl1 = l1 / float(nrleft) dl = irngleft * dl1 triA = dl * dl / np.tan(th) #triangle areas secA = [triA[0] ] + [triA[i] - triA[i - 1] for i in range(1, nrleft)] #section areas secA = np.array(secA) m1 = np.linspace( x0 - p + dl1 / 2.0, x - dl1 / 2.0, nrleft ) #section midpoint position - path length calculated from this plen = np.abs(2 * m1 / np.sin(th)) val = ib + np.sum(i0 * secA * np.exp(-u * plen)) elif (x0 <= x) and (x0 + p >= x): l1 = p nrleft = int(np.ceil(l1 / (2 * p) * ni)) if nrleft < 1: nrleft = 1 irngleft = np.array(range(1, nrleft + 1)) dl1left = l1 / float(nrleft) dlleft = irngleft * dl1left triAleft = dlleft * dlleft / np.tan(th) #triangle areas secAleft = [triAleft[0]] + [ triAleft[i] - triAleft[i - 1] for i in range(1, nrleft) ] #section areas secAleft = np.array(secAleft) m1left = np.linspace(x0 - p + dl1left / 2.0, x0 - dl1left / 2.0, nrleft) + (x - x0) plenleft = np.abs(2 * m1left / np.sin(th)) valleft = np.sum(i0 * secAleft * np.exp(-u * plenleft)) l2 = x - x0 nrright = int(np.ceil(x - x0 / (2 * p) * ni)) if nrright < 1: nrright = 1 irngright = np.array(range(1, nrright + 1)) dl1right = l2 / float(nrright) dlright = p - np.append(0.0, dl1right * irngright) triAright = dlright * dlright / np.tan(th) secAright = [ triAright[i] - triAright[i + 1] for i in range(nrright) ] secAright = np.array(secAright) m1right = np.linspace(x - x0 - dl1right / 2.0, dl1right / 2.0, nrright) plenright = np.abs(2 * m1right / np.sin(th)) valright = np.sum(i0 * secAright * np.exp(-u * plenright)) val = ib + valleft + valright elif (x > x0 + p): l1 = p #nrleft = int(np.ceil(l1/(x-(x0-p))*ni)); nrleft = int(np.ceil(l1 / (2 * p) * ni)) if nrleft < 1: nrleft = 1 irngleft = np.array(range(1, nrleft + 1)) dl1left = l1 / float(nrleft) dlleft = irngleft * dl1left triAleft = dlleft * dlleft / np.tan(th) #triangle areas secAleft = [triAleft[0]] + [ triAleft[i] - triAleft[i - 1] for i in range(1, nrleft) ] #section areas secAleft = np.array(secAleft) m1left = np.linspace(x0 - p + dl1left / 2.0, x0 - dl1left / 2.0, nrleft) + (x - x0) plenleft = np.abs(2 * m1left / np.sin(th)) valleft = np.sum(i0 * secAleft * np.exp(-u * plenleft)) l2 = p nrright = int(np.ceil(l2 / (2 * p) * ni)) if nrright < 1: nrright = 1 irngright = np.array(range(1, nrright + 1)) dl1right = l2 / float(nrright) dlright = p - np.append(0.0, dl1right * irngright) triAright = dlright * dlright / np.tan(th) secAright = [ triAright[i] - triAright[i + 1] for i in range(nrright) ] secAright = np.array(secAright) m1right = np.linspace(dlright[0] - dl1right / 2.0, dlright[-1] + dl1right / 2.0, nrright) + (x - (x0 + p)) plenright = np.abs(2 * m1right / np.sin(th)) valright = np.sum(i0 * secAright * np.exp(-u * plenright)) val = ib + valleft + valright out = out + [val] return np.array(out)
def iterate_for_desired_bandwidth(delta, desired_VLBW_Hz, motor_dict, CLBW_Hz_initial=1000, CLBW_Hz_stepSize=100): # print('DEBUG tuner.py', motor_dict) R = motor_dict['Rs'] L = motor_dict['Ls'] J_s = motor_dict['J_s'] JLoadRatio = motor_dict['JLoadRatio'] n_pp = motor_dict['n_pp'] KE = motor_dict['KE'] CL_TS = motor_dict['CL_TS'] VL_TS = motor_dict['VL_TS'] J_total = J_s * (1 + JLoadRatio) CLBW_Hz = CLBW_Hz_initial #100 # Hz (initial) VLBW_Hz = -10 # Hz (initial) count = 0 while True: count += 1 if count > 20: msg = f'Loop count 20 is reached. Step size is {CLBW_Hz_stepSize} Hz.' print(msg) # raise Exception() break # Current loop (Tune its bandwidth to support required speed response) if abs(VLBW_Hz - desired_VLBW_Hz) <= 10: # Hz break else: if VLBW_Hz > desired_VLBW_Hz: CLBW_Hz -= CLBW_Hz_stepSize # Hz if CLBW_Hz <= 0: raise Exception( f'Negative CLBW_Hz. Maybe change the step size of "CLBW_Hz" ({CLBW_Hz_stepSize} Hz) and try again.' ) break else: CLBW_Hz += CLBW_Hz_stepSize # Hz # print(f'CLBW_Hz = {CLBW_Hz}') currentKp, currentKi = get_coeffs_dc_motor_current_regulator( R, L, CLBW_Hz) currentKiCode = currentKi * currentKp * CL_TS if True: # 这里打印的用于实验中CCS的debug窗口检查电流环PI系数 上位机电流KP = CLBW_Hz 上位机电流KI = 1000 iSMC_currentKp = 上位机电流KP * L * 2 * np.pi iSMC_currentKi = 上位机电流KI / 1000 * R / L iSMC_currentKiCode = iSMC_currentKi * CL_TS * iSMC_currentKp # print(f'\tiSMC_currentKp={iSMC_currentKp:g}, \ # iSMC_currentKi={iSMC_currentKi:g}, \ # iSMC_currentKiCode={iSMC_currentKiCode:g}') # print(f'\tSimC_currentKp={currentKp:g}, \ # SimC_currentKi={currentKi:g}, \ # SimC_currentKiCode={currentKiCode:g}') # print(f'\t上位机电流KP={上位机电流KP:g}, \ # 上位机电流KI={上位机电流KI:g}') Gi_closed = control.tf( [1], [L / currentKp, 1]) # current loop zero-pole cancelled already currentBandwidth_radPerSec = currentKp / L # Speed loop KT = 1.5 * n_pp * KE dc_motor_motion = control.tf([KT * n_pp / J_total], [1, 0]) speedKp, speedKi = get_coeffs_dc_motor_SPEED_regulator( J_total, n_pp, KE, delta, currentBandwidth_radPerSec) speedKiCode = speedKi * speedKp * VL_TS if True: # 这里打印的用于实验中TI的debug窗口检查系数 上位机速度KP, 上位机速度KI = 逆上位机速度PI系数转换CODE(speedKp, speedKiCode, VL_TS, J_total) iSMC_speedKp, iSMC_speedKi, iSMC_speedKiCode = 上位机速度PI系数转换CODE( 上位机速度KP, 上位机速度KI, VL_TS, J_total) # print(f'\tiSMC_speedKp={iSMC_speedKp:g}, \ # iSMC_speedKi={iSMC_speedKi:g}, \ # iSMC_speedKiCode={iSMC_speedKiCode:g}') # print(f'\tSimC_speedKp={speedKp:g}, \ # SimC_speedKi={speedKi:g}, \ # SimC_speedKiCode={speedKiCode:g}') # print(f'\t上位机速度KP={上位机速度KP:g}, \ # 上位机速度KI={上位机速度KI:g}') # 下面打印的用于仿真 # print(f'\tspeedKp = {speedKp:g}', f'speedKi = {speedKi:g}', \ # f'wzero = {speedKi/2/np.pi:g} Hz', \ # f'cutoff = {delta*speedKi/2/np.pi:g} Hz', \ # f'ipole = {currentKp/L/2/np.pi:g} Hz', sep=' | ') speedPI = control.tf([speedKp, speedKp * speedKi], [1, 0]) Gw_open = dc_motor_motion * Gi_closed * speedPI # C2C c2c_tf = Gi_closed mag, phase, omega = control.bode_plot(c2c_tf, 2 * np.pi * np.logspace(0, 4, 500), dB=1, Hz=1, deg=1, lw='0.5', label=f'{CLBW_Hz:g} Hz') CLBW_Hz = omega[(np.abs(mag - 0.707)).argmin()] / 2 / np.pi C2C_designedMagPhaseOmega = mag, phase, omega # C2V c2v_tf = dc_motor_motion * Gi_closed mag, phase, omega = control.bode_plot(c2v_tf, 2 * np.pi * np.logspace(0, 4, 500), dB=1, Hz=1, deg=1, lw='0.5', label=f'{CLBW_Hz:g} Hz') open_cutoff_frequency_HZ = omega[(np.abs(mag - 0.0)).argmin()] / 2 / np.pi C2V_designedMagPhaseOmega = mag, phase, omega # V2V Gw_closed = Gw_open / (1 + Gw_open) mag, phase, omega = control.bode_plot(Gw_closed, 2 * np.pi * np.logspace(0, 4, 500), dB=1, Hz=1, deg=1, lw='0.5', label=f'{delta:g}') VLBW_Hz = omega[(np.abs(mag - 0.707)).argmin()] / 2 / np.pi V2V_designedMagPhaseOmega = mag, phase, omega # print(Gw_closed) # print('\tSpeed loop bandwidth:', VLBW_Hz, 'Hz') return (currentKp, currentKi), \ (speedKp, speedKi), \ (上位机电流KP, 上位机电流KI), \ (上位机速度KP, 上位机速度KI), \ (C2C_designedMagPhaseOmega, C2V_designedMagPhaseOmega, V2V_designedMagPhaseOmega), \ (CLBW_Hz, VLBW_Hz, open_cutoff_frequency_HZ)
def Ftryminmax(lK, Rg, alpha, betas, beta, delta): pos = np.array([0, 0.5 * np.pi, np.pi, 1.5 * np.pi]) omegat = np.arange(0, 360, 1) c = np.amin(Ftrysum(lK, Rg, alpha, betas, beta, delta, omegat, pos)) d = np.amax(Ftrysum(lK, Rg, alpha, betas, beta, delta, omegat, pos)) return (np.abs(d - c))
def Ftrxminmax(lK, Rg, alpha, betas, beta, delta): pos = np.array([0, 0.5 * np.pi, np.pi, 1.5 * np.pi]) omegat = np.arange(0, 360, 1) a = np.amin(Ftrxsum(lK, Rg, alpha, betas, beta, delta, omegat, pos)) b = np.amax(Ftrxsum(lK, Rg, alpha, betas, beta, delta, omegat, pos)) return (np.abs(b - a))