diff --git a/ACMConfig.h b/ACMConfig.h new file mode 100644 index 0000000..cfb8a99 --- /dev/null +++ b/ACMConfig.h @@ -0,0 +1,40 @@ + +#ifndef ACMCONFIG_H +#define ACMCONFIG_H + +#define NULL_D_AXIS_CURRENT_CONTROL -1 +#define MTPA -2 // not supported + +#define NUMBER_OF_STEPS 150000 +#define DOWN_SAMPLE 1 +#define CONTROL_STRATEGY NULL_D_AXIS_CURRENT_CONTROL +#define SENSORLESS_CONTROL False +#define VOLTAGE_CURRENT_DECOUPLING_CIRCUIT False +#define SATURATED_MAGNETIC_CIRCUIT False +#define INVERTER_NONLINEARITY False +#define TS 0.00025 +#define TS_INVERSE 4000 +#define TS_UPSAMPLING_FREQ_EXE 0.5 +#define TS_UPSAMPLING_FREQ_EXE_INVERSE 2 +#define PMSM_NUMBER_OF_POLE_PAIRS 2 +#define PMSM_RESISTANCE 0.45 +#define PMSM_D_AXIS_INDUCTANCE 0.00415 +#define PMSM_Q_AXIS_INDUCTANCE 0.01674 +#define PMSM_PERMANENT_MAGNET_FLUX_LINKAGE 0.504 +#define PMSM_SHAFT_INERTIA 0.06 +#define SPEED_LOOP_PID_PROPORTIONAL_GAIN 0.5 +#define SPEED_LOOP_PID_INTEGRAL_TIME_CONSTANT 1.05 +#define SPEED_LOOP_PID_DIREVATIVE_TIME_CONSTANT 0 +#define SPEED_LOOP_LIMIT_NEWTON_METER 8 +#define SPEED_LOOP_CEILING 40 +#define CURRENT_LOOP_PID_PROPORTIONAL_GAIN 15 +#define CURRENT_LOOP_PID_INTEGRAL_TIME_CONSTANT 0.08 +#define CURRENT_LOOP_PID_DIREVATIVE_TIME_CONSTANT 0 +#define CURRENT_LOOP_LIMIT_VOLTS 400 +#define DATA_FILE_NAME "pmsm_eemf.dat" +#define PC_SIMULATION True + +#define MACHINE_TS (TS*TS_UPSAMPLING_FREQ_EXE) //1.25e-4 +#define MACHINE_TS_INVERSE (TS_INVERSE*TS_UPSAMPLING_FREQ_EXE_INVERSE) // 8000 + +#endif diff --git a/ACMConfig.json b/ACMConfig.json new file mode 100644 index 0000000..f11e5ee --- /dev/null +++ b/ACMConfig.json @@ -0,0 +1,182 @@ +{ + "#01 ACMSIMC PMSM EEMF": { + "NUMBER_OF_STEPS": 150000, + "DOWN_SAMPLE": 1, + "CONTROL_STRATEGY": "NULL_D_AXIS_CURRENT_CONTROL", + "SENSORLESS_CONTROL": false, + "VOLTAGE_CURRENT_DECOUPLING_CIRCUIT": false, + "SATURATED_MAGNETIC_CIRCUIT": false, + "INVERTER_NONLINEARITY": false, + "TS": 0.00025, + "TS_INVERSE": 4000, + "TS_UPSAMPLING_FREQ_EXE": 0.5, + "TS_UPSAMPLING_FREQ_EXE_INVERSE": 2, + "PMSM_NUMBER_OF_POLE_PAIRS": 2, + "PMSM_RESISTANCE": 0.45, + "PMSM_D_AXIS_INDUCTANCE": 0.00415, + "PMSM_Q_AXIS_INDUCTANCE": 0.01674, + "PMSM_PERMANENT_MAGNET_FLUX_LINKAGE": 0.504, + "PMSM_SHAFT_INERTIA": 0.06, + "SPEED_LOOP_PID_PROPORTIONAL_GAIN": 0.5, + "SPEED_LOOP_PID_INTEGRAL_TIME_CONSTANT": 1.05, + "SPEED_LOOP_PID_DIREVATIVE_TIME_CONSTANT": 0, + "SPEED_LOOP_LIMIT_NEWTON_METER": 8, + "SPEED_LOOP_CEILING": 40, + "CURRENT_LOOP_PID_PROPORTIONAL_GAIN": 15, + "CURRENT_LOOP_PID_INTEGRAL_TIME_CONSTANT": 0.08, + "CURRENT_LOOP_PID_DIREVATIVE_TIME_CONSTANT": 0, + "CURRENT_LOOP_LIMIT_VOLTS": 400, + "DATA_FILE_NAME": "\"pmsm_eemf.dat\"", + "PC_SIMULATION": true + }, + "#01 ACMSIMC PMSM EEMF - Variant Speed P 000": { + "NUMBER_OF_STEPS": 150000, + "DOWN_SAMPLE": 1, + "CONTROL_STRATEGY": "NULL_D_AXIS_CURRENT_CONTROL", + "SENSORLESS_CONTROL": false, + "VOLTAGE_CURRENT_DECOUPLING_CIRCUIT": false, + "SATURATED_MAGNETIC_CIRCUIT": false, + "INVERTER_NONLINEARITY": false, + "TS": 0.00025, + "TS_INVERSE": 4000, + "TS_UPSAMPLING_FREQ_EXE": 0.5, + "TS_UPSAMPLING_FREQ_EXE_INVERSE": 2, + "PMSM_NUMBER_OF_POLE_PAIRS": 2, + "PMSM_RESISTANCE": 0.45, + "PMSM_D_AXIS_INDUCTANCE": 0.00415, + "PMSM_Q_AXIS_INDUCTANCE": 0.01674, + "PMSM_PERMANENT_MAGNET_FLUX_LINKAGE": 0.504, + "PMSM_SHAFT_INERTIA": 0.06, + "SPEED_LOOP_PID_PROPORTIONAL_GAIN": 0.5, + "SPEED_LOOP_PID_INTEGRAL_TIME_CONSTANT": 1.05, + "SPEED_LOOP_PID_DIREVATIVE_TIME_CONSTANT": 0, + "SPEED_LOOP_LIMIT_NEWTON_METER": 8, + "SPEED_LOOP_CEILING": 40, + "CURRENT_LOOP_PID_PROPORTIONAL_GAIN": 15, + "CURRENT_LOOP_PID_INTEGRAL_TIME_CONSTANT": 0.08, + "CURRENT_LOOP_PID_DIREVATIVE_TIME_CONSTANT": 0, + "CURRENT_LOOP_LIMIT_VOLTS": 400, + "DATA_FILE_NAME": "\"pmsm_eemf_VSP000.dat\"", + "PC_SIMULATION": true + }, + "#01 ACMSIMC PMSM EEMF - Variant Speed P 001": { + "NUMBER_OF_STEPS": 150000, + "DOWN_SAMPLE": 1, + "CONTROL_STRATEGY": "NULL_D_AXIS_CURRENT_CONTROL", + "SENSORLESS_CONTROL": false, + "VOLTAGE_CURRENT_DECOUPLING_CIRCUIT": false, + "SATURATED_MAGNETIC_CIRCUIT": false, + "INVERTER_NONLINEARITY": false, + "TS": 0.00025, + "TS_INVERSE": 4000, + "TS_UPSAMPLING_FREQ_EXE": 0.5, + "TS_UPSAMPLING_FREQ_EXE_INVERSE": 2, + "PMSM_NUMBER_OF_POLE_PAIRS": 2, + "PMSM_RESISTANCE": 0.45, + "PMSM_D_AXIS_INDUCTANCE": 0.00415, + "PMSM_Q_AXIS_INDUCTANCE": 0.01674, + "PMSM_PERMANENT_MAGNET_FLUX_LINKAGE": 0.504, + "PMSM_SHAFT_INERTIA": 0.06, + "SPEED_LOOP_PID_PROPORTIONAL_GAIN": 1.5, + "SPEED_LOOP_PID_INTEGRAL_TIME_CONSTANT": 1.05, + "SPEED_LOOP_PID_DIREVATIVE_TIME_CONSTANT": 0, + "SPEED_LOOP_LIMIT_NEWTON_METER": 8, + "SPEED_LOOP_CEILING": 40, + "CURRENT_LOOP_PID_PROPORTIONAL_GAIN": 15, + "CURRENT_LOOP_PID_INTEGRAL_TIME_CONSTANT": 0.08, + "CURRENT_LOOP_PID_DIREVATIVE_TIME_CONSTANT": 0, + "CURRENT_LOOP_LIMIT_VOLTS": 400, + "DATA_FILE_NAME": "\"pmsm_eemf_VSP001.dat\"", + "PC_SIMULATION": true + }, + "#01 ACMSIMC PMSM EEMF - Variant Speed P 002": { + "NUMBER_OF_STEPS": 150000, + "DOWN_SAMPLE": 1, + "CONTROL_STRATEGY": "NULL_D_AXIS_CURRENT_CONTROL", + "SENSORLESS_CONTROL": false, + "VOLTAGE_CURRENT_DECOUPLING_CIRCUIT": false, + "SATURATED_MAGNETIC_CIRCUIT": false, + "INVERTER_NONLINEARITY": false, + "TS": 0.00025, + "TS_INVERSE": 4000, + "TS_UPSAMPLING_FREQ_EXE": 0.5, + "TS_UPSAMPLING_FREQ_EXE_INVERSE": 2, + "PMSM_NUMBER_OF_POLE_PAIRS": 2, + "PMSM_RESISTANCE": 0.45, + "PMSM_D_AXIS_INDUCTANCE": 0.00415, + "PMSM_Q_AXIS_INDUCTANCE": 0.01674, + "PMSM_PERMANENT_MAGNET_FLUX_LINKAGE": 0.504, + "PMSM_SHAFT_INERTIA": 0.06, + "SPEED_LOOP_PID_PROPORTIONAL_GAIN": 2.5, + "SPEED_LOOP_PID_INTEGRAL_TIME_CONSTANT": 1.05, + "SPEED_LOOP_PID_DIREVATIVE_TIME_CONSTANT": 0, + "SPEED_LOOP_LIMIT_NEWTON_METER": 8, + "SPEED_LOOP_CEILING": 40, + "CURRENT_LOOP_PID_PROPORTIONAL_GAIN": 15, + "CURRENT_LOOP_PID_INTEGRAL_TIME_CONSTANT": 0.08, + "CURRENT_LOOP_PID_DIREVATIVE_TIME_CONSTANT": 0, + "CURRENT_LOOP_LIMIT_VOLTS": 400, + "DATA_FILE_NAME": "\"pmsm_eemf_VSP002.dat\"", + "PC_SIMULATION": true + }, + "#01 ACMSIMC PMSM EEMF - Variant Speed P 003": { + "NUMBER_OF_STEPS": 150000, + "DOWN_SAMPLE": 1, + "CONTROL_STRATEGY": "NULL_D_AXIS_CURRENT_CONTROL", + "SENSORLESS_CONTROL": false, + "VOLTAGE_CURRENT_DECOUPLING_CIRCUIT": false, + "SATURATED_MAGNETIC_CIRCUIT": false, + "INVERTER_NONLINEARITY": false, + "TS": 0.00025, + "TS_INVERSE": 4000, + "TS_UPSAMPLING_FREQ_EXE": 0.5, + "TS_UPSAMPLING_FREQ_EXE_INVERSE": 2, + "PMSM_NUMBER_OF_POLE_PAIRS": 2, + "PMSM_RESISTANCE": 0.45, + "PMSM_D_AXIS_INDUCTANCE": 0.00415, + "PMSM_Q_AXIS_INDUCTANCE": 0.01674, + "PMSM_PERMANENT_MAGNET_FLUX_LINKAGE": 0.504, + "PMSM_SHAFT_INERTIA": 0.06, + "SPEED_LOOP_PID_PROPORTIONAL_GAIN": 3.5, + "SPEED_LOOP_PID_INTEGRAL_TIME_CONSTANT": 1.05, + "SPEED_LOOP_PID_DIREVATIVE_TIME_CONSTANT": 0, + "SPEED_LOOP_LIMIT_NEWTON_METER": 8, + "SPEED_LOOP_CEILING": 40, + "CURRENT_LOOP_PID_PROPORTIONAL_GAIN": 15, + "CURRENT_LOOP_PID_INTEGRAL_TIME_CONSTANT": 0.08, + "CURRENT_LOOP_PID_DIREVATIVE_TIME_CONSTANT": 0, + "CURRENT_LOOP_LIMIT_VOLTS": 400, + "DATA_FILE_NAME": "\"pmsm_eemf_VSP003.dat\"", + "PC_SIMULATION": true + }, + "#01 ACMSIMC PMSM EEMF - Variant Speed P 004": { + "NUMBER_OF_STEPS": 150000, + "DOWN_SAMPLE": 1, + "CONTROL_STRATEGY": "NULL_D_AXIS_CURRENT_CONTROL", + "SENSORLESS_CONTROL": false, + "VOLTAGE_CURRENT_DECOUPLING_CIRCUIT": false, + "SATURATED_MAGNETIC_CIRCUIT": false, + "INVERTER_NONLINEARITY": false, + "TS": 0.00025, + "TS_INVERSE": 4000, + "TS_UPSAMPLING_FREQ_EXE": 0.5, + "TS_UPSAMPLING_FREQ_EXE_INVERSE": 2, + "PMSM_NUMBER_OF_POLE_PAIRS": 2, + "PMSM_RESISTANCE": 0.45, + "PMSM_D_AXIS_INDUCTANCE": 0.00415, + "PMSM_Q_AXIS_INDUCTANCE": 0.01674, + "PMSM_PERMANENT_MAGNET_FLUX_LINKAGE": 0.504, + "PMSM_SHAFT_INERTIA": 0.06, + "SPEED_LOOP_PID_PROPORTIONAL_GAIN": 4.5, + "SPEED_LOOP_PID_INTEGRAL_TIME_CONSTANT": 1.05, + "SPEED_LOOP_PID_DIREVATIVE_TIME_CONSTANT": 0, + "SPEED_LOOP_LIMIT_NEWTON_METER": 8, + "SPEED_LOOP_CEILING": 40, + "CURRENT_LOOP_PID_PROPORTIONAL_GAIN": 15, + "CURRENT_LOOP_PID_INTEGRAL_TIME_CONSTANT": 0.08, + "CURRENT_LOOP_PID_DIREVATIVE_TIME_CONSTANT": 0, + "CURRENT_LOOP_LIMIT_VOLTS": 400, + "DATA_FILE_NAME": "\"pmsm_eemf_VSP004.dat\"", + "PC_SIMULATION": true + } +} \ No newline at end of file diff --git a/ACMConfig.py b/ACMConfig.py new file mode 100644 index 0000000..25a49a3 --- /dev/null +++ b/ACMConfig.py @@ -0,0 +1,121 @@ +import json +def load_json(fname='ACMConfig.json', bool_print_out=True): + # In case there are comments in your json file, option 1 is to try https://pypi.org/project/jsmin/, option 2 is to remove it with GetJsonFromFileWithComments + # def GetJsonFromFileWithComments(filePath): + # # https://stackoverflow.com/questions/29959191/how-to-parse-json-file-with-c-style-comments + # contents = "" + # fh = open(filePath) + # for line in fh: + # cleanedLine = line.split("//", 1)[0] + # if len(cleanedLine) > 0 and line.endswith("\n") and "\n" not in cleanedLine: + # cleanedLine += "\n" + # contents += cleanedLine + # fh.close + # while "/*" in contents: + # preComment, postComment = contents.split("/*", 1) + # contents = preComment + postComment.split("*/", 1)[1] + # return contents + + with open(fname, 'r') as f: + raw_config_dicts = json.load(f) + + def decode_raw_fea_configs(raw_config_dicts): + for key, val in raw_config_dicts.items(): + print('\n', key) + for ke, va in val.items(): + print('\t', ke+':', va) + if bool_print_out: + decode_raw_fea_configs(raw_config_dicts) + return raw_config_dicts + +def write_to_ACMConfig_header_file(config_pairs): + str_c_defines = '' + for key, val in config_pairs.items(): + str_c_defines += f'#define {key} {val}\n' + + bare_template = f''' +#ifndef ACMCONFIG_H +#define ACMCONFIG_H + +#define NULL_D_AXIS_CURRENT_CONTROL -1 +#define MTPA -2 // not supported + +{str_c_defines} +#define MACHINE_TS (TS*TS_UPSAMPLING_FREQ_EXE) //1.25e-4 +#define MACHINE_TS_INVERSE (TS_INVERSE*TS_UPSAMPLING_FREQ_EXE_INVERSE) // 8000 + +#endif +''' + + with open('ACMConfig.h', 'w') as f: + f.write(bare_template) + + +if __name__ == '__main__': + + raw_config_dicts = load_json(bool_print_out=False) + config_pairs = raw_config_dicts['#01 ACMSIMC PMSM EEMF'] + write_to_ACMConfig_header_file(config_pairs) + + + + + + + + + + + + + + + + + + + + + + +readme = ''' +"#01 ACMSIMC PMSM EEMF" + +"NUMBER_OF_STEPS": 150000, <--- How many steps to simulate. NUMBER_OF_STEPS * MACHINE_TS = simulation time +"DOWN_SAMPLE": 1, <--- For plot. Sometimes you may want to reduce the waveform sampling step to make the codes run faster. + +"CONTROL_STRATEGY": "NULL_D_AXIS_CURRENT_CONTROL", <--- Control method. + +"SENSORLESS_CONTROL": false, <--- Use reconstructed speed and position for feedback control. +"VOLTAGE_CURRENT_DECOUPLING_CIRCUIT": false, <--- Decouple between d-axis and q-axis current loops. +"SATURATED_MAGNETIC_CIRCUIT": false, <--- Saturation. +"INVERTER_NONLINEARITY": false, <--- Inverter nonlinearity. + +"TS": 2.5e-4, <--- Sampling time in DSP or controller. Make sure this TS matches your actual control codes execution frequency, e.g., PWM having a carrier frequency of 4 kHz (Additionally, if it interupts at both 波峰 and 波谷, you can have a interupt freuquency of 8 kHz). +"TS_INVERSE": 4000, <--- TS * TS_INVERSE = 1 +"TS_UPSAMPLING_FREQ_EXE": 0.5, <--- Up-sampling TS by a factor of this number to obtain MACHINE_TS. That is, the machine dynamics are executed in a higher frequency than your controller, to imitate a continuous time system +"TS_UPSAMPLING_FREQ_EXE_INVERSE": 2, <--- 1/TS_UPSAMPLING_FREQ_EXE + +"PMSM_NUMBER_OF_POLE_PAIRS": 2, +"PMSM_RESISTANCE": 0.45, +"PMSM_D_AXIS_INDUCTANCE": 0.00415, +"PMSM_Q_AXIS_INDUCTANCE": 0.01674, +"PMSM_PERMANENT_MAGNET_FLUX_LINKAGE": 0.504, +"PMSM_SHAFT_INERTIA": 0.06, + +"SPEED_LOOP_PID_PROPORTIONAL_GAIN": 0.5, +"SPEED_LOOP_PID_INTEGRAL_TIME_CONSTANT": 1.05, +"SPEED_LOOP_PID_DIREVATIVE_TIME_CONSTANT": 0, +"SPEED_LOOP_LIMIT_NEWTON_METER": 8, + +"SPEED_LOOP_CEILING": 40, <--- velocity control loop execution frequency is 40 times slower than current control loop execution frequency + +"CURRENT_LOOP_PID_PROPORTIONAL_GAIN": 15, +"CURRENT_LOOP_PID_INTEGRAL_TIME_CONSTANT": 0.08, +"CURRENT_LOOP_PID_DIREVATIVE_TIME_CONSTANT": 0, +"CURRENT_LOOP_LIMIT_VOLTS": 8, + +"PC_SIMULATION": true +''' +# print(readme) + diff --git a/ACMConfig_backup.json b/ACMConfig_backup.json new file mode 100644 index 0000000..bce6856 --- /dev/null +++ b/ACMConfig_backup.json @@ -0,0 +1,42 @@ +{ + "#01 ACMSIMC PMSM EEMF": { + + "NUMBER_OF_STEPS": 150000, + "DOWN_SAMPLE": 1, + + "CONTROL_STRATEGY": "NULL_D_AXIS_CURRENT_CONTROL", + + "SENSORLESS_CONTROL": false, + "VOLTAGE_CURRENT_DECOUPLING_CIRCUIT": false, + "SATURATED_MAGNETIC_CIRCUIT": false, + "INVERTER_NONLINEARITY": false, + + "TS": 2.5e-4, + "TS_INVERSE": 4000, + "TS_UPSAMPLING_FREQ_EXE": 0.5, + "TS_UPSAMPLING_FREQ_EXE_INVERSE": 2, + + "PMSM_NUMBER_OF_POLE_PAIRS": 2, + "PMSM_RESISTANCE": 0.45, + "PMSM_D_AXIS_INDUCTANCE": 0.00415, + "PMSM_Q_AXIS_INDUCTANCE": 0.01674, + "PMSM_PERMANENT_MAGNET_FLUX_LINKAGE": 0.504, + "PMSM_SHAFT_INERTIA": 0.06, + + "SPEED_LOOP_PID_PROPORTIONAL_GAIN": 0.5, + "SPEED_LOOP_PID_INTEGRAL_TIME_CONSTANT": 1.05, + "SPEED_LOOP_PID_DIREVATIVE_TIME_CONSTANT": 0, + "SPEED_LOOP_LIMIT_NEWTON_METER": 8, + + "SPEED_LOOP_CEILING": 40, + + "CURRENT_LOOP_PID_PROPORTIONAL_GAIN": 15, + "CURRENT_LOOP_PID_INTEGRAL_TIME_CONSTANT": 0.08, + "CURRENT_LOOP_PID_DIREVATIVE_TIME_CONSTANT": 0, + "CURRENT_LOOP_LIMIT_VOLTS": 400, + + "DATA_FILE_NAME": "\"pmsm_eemf.dat\"", + + "PC_SIMULATION": true + } +} diff --git a/ACMOptimization.py b/ACMOptimization.py new file mode 100644 index 0000000..7095a9a --- /dev/null +++ b/ACMOptimization.py @@ -0,0 +1,70 @@ +from ACMConfig import load_json, write_to_ACMConfig_header_file +import json, subprocess + + +def subprocess_cmd(command_string): + process = subprocess.Popen(command_string,stdout=subprocess.PIPE, shell=True) + streamdata = proc_stdout = process.communicate()[0].strip() + # print(proc_stdout) + return process + + # example: + # subprocess_cmd('echo a; echo b') + +if __name__ == '__main__': + + # select tempalte json config + select_config = '#01 ACMSIMC PMSM EEMF' + + # load template json config + raw_config_dicts = load_json('ACMConfig_backup.json', bool_print_out=False) + + # change json config for simulation variants + NUMBER_OF_RUNS = 5 + for i in range(0, NUMBER_OF_RUNS): + raw_config_dicts[select_config+f' - Variant Speed P {i:03d}'] = dict(raw_config_dicts[select_config]) # copy dict + raw_config_dicts[select_config+f' - Variant Speed P {i:03d}']['SPEED_LOOP_PID_PROPORTIONAL_GAIN'] = 0.5+1*i + raw_config_dicts[select_config+f' - Variant Speed P {i:03d}']['DATA_FILE_NAME'] = f"\"pmsm_eemf_VSP{i:03d}.dat\"" + + # dump json config's to file + with open('ACMConfig.json', 'w') as f: + json.dump(raw_config_dicts, f, indent=4) + + # with all those json config's, for each json config, re-write ACMConfig.h and compile main.c and run it. + if True: + for i in range(0, NUMBER_OF_RUNS): + write_to_ACMConfig_header_file(raw_config_dicts[select_config+f' - Variant Speed P {i:03d}']) + child = subprocess_cmd("gcc main.c controller.c observer.c -L. -o main && start cmd /c main") + rc = child.returncode + if rc == 0: + continue + else: + raise Exception('The C codes end with returncode %d'%(rc)) + + # now with a pile of .dat files, we are ready to plot + import ACMPlot + import pandas as pd + import numpy as np + from collections import OrderedDict as O + df_info = pd.read_csv(r"./info.dat", na_values = ['1.#QNAN', '-1#INF00', '-1#IND00']) + bool_initialized = False + for i in range(0, NUMBER_OF_RUNS): + + df_profiles = pd.read_csv(f"pmsm_eemf_VSP{i:03d}.dat", na_values = ['1.#QNAN', '-1#INF00', '-1#IND00']) + + if bool_initialized == False: + bool_initialized = True + no_samples = df_profiles.shape[0] + no_traces = df_profiles.shape[1] + time = np.arange(1, no_samples+1) * df_info['DOWN_SAMPLE'].values[0] * df_info['TS'].values[0] + ax_list = [] + for i in range(0, no_traces, 6): + ax_list += list(ACMPlot.get_axis((1,6))) + + for idx, key in enumerate(df_profiles.keys()): + ACMPlot.plot_it(ax_list[idx], key, O([ + (str(idx), df_profiles[key]), + ]), time) + from pylab import plt + plt.show() + quit() diff --git a/ACMPlot.py b/ACMPlot.py index 74ecbbc..437a2f1 100644 --- a/ACMPlot.py +++ b/ACMPlot.py @@ -6,7 +6,8 @@ from collections import OrderedDict as O import pandas as pd # plot style -style = np.random.choice(plt.style.available); print(style); plt.style.use('grayscale') # [u'dark_background', u'bmh', u'grayscale', u'ggplot', u'fivethirtyeight'] +style = np.random.choice(plt.style.available); print(style); +plt.style.use('ggplot') # ['grayscale', u'dark_background', u'bmh', u'grayscale', u'ggplot', u'fivethirtyeight'] # plot setting mpl.rcParams['mathtext.fontset'] = 'stix' mpl.rcParams['font.family'] = 'STIXGeneral' @@ -24,15 +25,6 @@ 'weight' : 'normal', 'size' : 11.5,} -df_profiles = pd.read_csv(r"./algorithm.dat", na_values = ['1.#QNAN', '-1#INF00', '-1#IND00']) -df_info = pd.read_csv(r"./info.dat", na_values = ['1.#QNAN', '-1#INF00', '-1#IND00']) - -no_samples = df_profiles.shape[0] -no_traces = df_profiles.shape[1] -print(df_info, 'Simulated time: %g s.'%(no_samples * df_info['TS'].values[0] * df_info['DOWN_SAMPLE'].values[0]), 'Key list:', sep='\n') -for key in df_profiles.keys(): - print('\t', key) - ###################### # Plotting def get_axis(cNr): @@ -49,7 +41,7 @@ def plot_key(ax, key, df): ax.plot(time, df[key].values, '-', lw=1) ax.set_ylabel(key, fontdict=font) -def plot_it(ax, ylabel, d): +def plot_it(ax, ylabel, d, time=None): count = 0 for k, v in d.items(): if count == 0: @@ -66,18 +58,31 @@ def plot_it(ax, ylabel, d): # ax.set_xlim(0,35) # shared x # ax.set_ylim(0.85,1.45) -time = np.arange(1, no_samples+1) * df_info['DOWN_SAMPLE'].values[0] * df_info['TS'].values[0] +if __name__ == '__main__': + + df_info = pd.read_csv(r"./info.dat", na_values = ['1.#QNAN', '-1#INF00', '-1#IND00']) + data_file_name = df_info['DATA_FILE_NAME'].values[0].strip() + print(data_file_name) + df_profiles = pd.read_csv(data_file_name, na_values = ['1.#QNAN', '-1#INF00', '-1#IND00']) + + no_samples = df_profiles.shape[0] + no_traces = df_profiles.shape[1] + print(df_info, 'Simulated time: %g s.'%(no_samples * df_info['TS'].values[0] * df_info['DOWN_SAMPLE'].values[0]), 'Key list:', sep='\n') + for key in df_profiles.keys(): + print('\t', key) + + time = np.arange(1, no_samples+1) * df_info['DOWN_SAMPLE'].values[0] * df_info['TS'].values[0] -ax_list = [] -for i in range(0, no_traces, 6): - ax_list += list(get_axis((1,6))) + ax_list = [] + for i in range(0, no_traces, 6): + ax_list += list(get_axis((1,6))) -for idx, key in enumerate(df_profiles.keys()): - plot_it(ax_list[idx], key, O([ - (str(idx), df_profiles[key]), - # (str(idx), df_profiles[key]), - ])) -plt.show() -quit() + for idx, key in enumerate(df_profiles.keys()): + plot_it(ax_list[idx], key, O([ + (str(idx), df_profiles[key]), + # (str(idx), df_profiles[key]), + ]), time) + plt.show() + quit() diff --git a/ACMSim.h b/ACMSim.h index 5643ee9..d07dbd2 100644 --- a/ACMSim.h +++ b/ACMSim.h @@ -1,54 +1,15 @@ #ifndef ACMSIM_H #define ACMSIM_H -#define INDUCTION_MACHINE 1 -#define SYNCHRONOUS_MACHINE 2 -#define MACHINE_TYPE INDUCTION_MACHINE - /* standard lib */ -#include // printf #include // bool for _Bool and true for 1 // #include // bool for _Bool and true for 1 +#include // printf #include // bool for _Bool and true for 1 #include //reqd. for system function prototype #include // for clrscr, and getch() #include "stdlib.h" // for rand() #include "math.h" #include "time.h" -#if MACHINE_TYPE == INDUCTION_MACHINE - #define VVVF_CONTROL 0 - #define IFOC 1 - #define DFOC 2 - #define CONTROL_STRATEGY IFOC - - #define TAJIMA96 0 - #define TOTALLY_ADAPTIVE_OBSERVER 1 // Flux-MRAS - #define OBSERVER_APPLIED 0 - - #define SENSORLESS_CONTROL false - #define VOLTAGE_CURRENT_DECOUPLING_CIRCUIT false - - #define SATURATED_MAGNETIC_CIRCUIT true - #define INVERTER_NONLINEARITY true - -#elif MACHINE_TYPE == SYNCHRONOUS_MACHINE - #define NULL_D_AXIS_CURRENT_CONTROL -1 - #define MTPA -2 // not supported - #define CONTROL_STRATEGY NULL_D_AXIS_CURRENT_CONTROL - - #define SENSORLESS_CONTROL false -#endif - -/* How long should I go? */ -#define NUMBER_OF_LINES (5*150000) - -#define MACHINE_TS 1.25e-4 -#define MACHINE_TS_INVERSE 8000 -#define DOWN_FREQ_EXE 2 -#define DOWN_FREQ_EXE_INVERSE 0.5 -#define TS (MACHINE_TS*DOWN_FREQ_EXE) //2.5e-4 -#define TS_INVERSE (MACHINE_TS_INVERSE*DOWN_FREQ_EXE_INVERSE) // 4000 -#define DOWN_SAMPLE 1 // 5 // 10 - /* Macro for Part transformation*/ #define AB2M(A, B, COS, SIN) ( (A)*COS + (B)*SIN ) #define AB2T(A, B, COS, SIN) ( (A)*-SIN + (B)*COS ) @@ -61,145 +22,94 @@ #define AB2W(A, B) ( 0.816496580927726 * ( A*-0.5 + B*-0.8660254037844385 ) ) /* General Constants */ -#define TRUE True +#define TRUE True #define FALSE False -#define True (1) -#define False (0) -#define true 1 -#define false 0 -#define lTpi 0.15915494309189535 // 1/(2*pi) +#define True true +#define False false +#define true (1) +#define false (0) +#define ONE_OVER_2PI 0.15915494309189535 // 1/(2*pi) #define TWO_PI_OVER_3 2.0943951023931953 #define SIN_2PI_SLASH_3 0.86602540378443871 // sin(2*pi/3) #define SIN_DASH_2PI_SLASH_3 -0.86602540378443871 // sin(-2*pi/3) #define SQRT_2_SLASH_3 0.81649658092772603 // sqrt(2.0/3.0) +#define abs use_fabs_instead_or_you_will_regret // #define RPM_2_RAD_PER_SEC 0.20943951023931953 // (2/60*Tpi) // #define RAD_PER_SEC_2_RPM 4.7746482927568605 // 1/(im.npp/60*Tpi) -#define abs use_fabs_instead_or_you_will_regret -#define RAD_PER_SEC_2_RPM (60.0/(2*M_PI*ACM.npp)) -#define RPM_2_RAD_PER_SEC ((2*M_PI*ACM.npp)/60.0) -// #define PI_D 3.1415926535897932384626433832795 /* double */ +#define RAD_PER_SEC_2_RPM ( 60.0/(2*M_PI*ACM.npp) ) +#define RPM_2_RAD_PER_SEC ( (2*M_PI*ACM.npp)/60.0 ) #define M_PI_OVER_180 0.017453292519943295 +// #define PI_D 3.1415926535897932384626433832795 /* double */ // 补充的宏，为了实现实验/仿真代码大一统 #define Uint32 unsigned long int #define Uint16 unsigned int -#define PHASE_NUMBER 3 - - - -#if MACHINE_TYPE == INDUCTION_MACHINE - struct InductionMachineSimulated{ - double ial; - double ibe; - double psi_al; - double psi_be; - - double ual; - double ube; - - double x[13]; //////////////////////////////// - double rpm; - double rpm_cmd; - double rpm_deriv_cmd; - double Tload; - double Tem; - - double Js; - double npp; - double mu_m; - double Ts; - - double Lsigma; - double rs; - double rreq; - double Lmu; - double Lmu_inv; - double alpha; - - double Lm; - double rr; - double Lls; - double Llr; - double Lm_slash_Lr; - double Lr_slash_Lm; - - double LSigmal; - - double izq; - double izd; - double iz; - - double psimq; - double psimd; - double psim; - double im; - - double iqs; - double ids; - double iqr; - double idr; - - double ual_c_dist; - double ube_c_dist; - double dist_al; - double dist_be; - }; - extern struct InductionMachineSimulated ACM; - - #define UAL_C_DIST ACM.ual_c_dist // alpha-component of the distorted phase voltage = sine voltage + distored component - #define UBE_C_DIST ACM.ube_c_dist - #define DIST_AL ACM.dist_al // alpha-component of the distorted component of the voltage - #define DIST_BE ACM.dist_be - -#elif MACHINE_TYPE == SYNCHRONOUS_MACHINE - struct SynchronousMachineSimulated{ - double x[5]; - double rpm; - double rpm_cmd; - double rpm_deriv_cmd; - double Tload; - double Tem; - - double R; - double Ld; - double Lq; - double KE; - double L0; - double L1; - - double Js; - double npp; - double mu_m; - double Ts; - - double id; - double iq; - - double ial; - double ibe; - - double ud; - double uq; - - double ual; - double ube; - - double theta_d; - }; - extern struct SynchronousMachineSimulated ACM; -#endif + +#define NUMBER_OF_STATES 4 // valid for PMSM +#define PHASE_NUMBER 3 // 3 phase machine + +// Everthing else is in here +#include "ACMConfig.h" + + + +struct SynchronousMachineSimulated{ + + double Ts; + + double x[NUMBER_OF_STATES]; + double x_dot[NUMBER_OF_STATES]; + + double omg_elec; + double rpm; + double rpm_cmd; + double rpm_deriv_cmd; + double Tload; + double Tem; + + double npp; + + double R; + double Ld; + double Lq; + double KE; // psi_PM + double Js; + + double mu_m; + double L0; + double L1; + + double ual; + double ube; + double ial; + double ibe; + + double theta_d; + double ud; + double uq; + double id; + double iq; + + double eemf_q; + double eemf_al; + double eemf_be; + double theta_d__eemf; +}; +extern struct SynchronousMachineSimulated ACM; + #include "controller.h" #include "observer.h" -#include "Add_TAAO.h" +// #include "Add_TAAO.h" // #include "utility.h" -#include "inverter.h" +// #include "inverter.h" + +// Saturation +// #include "satlut.h" +// #define ACMSIMC_DEBUG false -// saturation -#include "satlut.h" -#define ACMSIMC_DEBUG false /* Declaration of Utility Function */ void write_header_to_file(FILE *fw); diff --git a/Add_TAAO.h b/Add_TAAO.h deleted file mode 100644 index 707ecc6..0000000 --- a/Add_TAAO.h +++ /dev/null @@ -1,175 +0,0 @@ -/* - * Add_TAAO.h - * - * by hory chen - * Email: horychen@qq.com - * Created on: Nov. 26th 2016 - * - * Warning: Do not declare any variables in h files, use extern instead. - */ - -/* Consistent with DSP Programs Begin - * Except for type double */ -#ifndef ADD_TAAO_H -#define ADD_TAAO_H -#if OBSERVER_APPLIED == TOTALLY_ADAPTIVE_OBSERVER - - -#define TAAO_FLUX_COMMAND_VALUE (1.2) // 1.2 -#define TAAO_FLUX_COMMAND_ON (false) -#define TIME_DIVISION_MULTIPLEXING (false) - -#define KVM_VALUE (700) // (100) -#define KMEC_VALUE (0) - -/* 第三轮调试（无速度传感器运行、供稿IEMDC） - * */ -#define M1 (0.01*0) //0.01//(0.05) // 转速辨识大震荡的罪魁祸首竟然是中频注入？ -#define OMG1 (24*2*M_PI) //(0.5*2*M_PI) -#define M3 (0.01*0) //(0.02) -#define OMG3 (10*2*M_PI) /* 10Hz竟然可以？！低频注入仍然不行，只能猜测25Hz可能刚好被逆非补偿给削弱了？不！就是越高频稳态辨识误差越大！ */ -#define M2 (0.0) // 0.1 -#define OMG2 (6*2*M_PI) //(1*2*M_PI) - -/* 四参数辨识（有偏） */ -#define GROUP_M 0*0.7 -#define GROUP_T 20 -#define GAMMA_LSIGMA 0.0 //0.1 //0.5 -#define GAMMA_RS 0*200 //(GROUP_M+0*GROUP_T)*(215) // rs小12.26~12.275大rs@TL=6Nm, //13.5@TL=20Nm //1.5 //2.5 // 5 -#define GAMMA_LEQ 0*GROUP_M*2.5*10*1e1 //13 for only Leq Id. // 10, 14.01~15就太快了，（加入饱和特性后）转速暂态时导致系统不稳定！是在150rpm下不稳定，用gamma4=14.01可以体现出来，14.00也不行。 -#define GAMMA_RREQ 0*1000 //0*GROUP_T*122*1e1 //*2.5 -#define GAMMA_OMEGAR GROUP_T*20520 // 6e5 for decoupled IFOC??? // 6e4 for IOFLC 500rpm //6e3 for IFOC 50,80rpm //2e3 //(7e2) // 1e3 for no ripple; 5e4 for fast response; 5e5 for good simulation. With kVM=4000 -#define GAMMA_OMEGAR_P 0// (100) 稳态转速自振荡 // 后面的都是IFOC加入解耦控制之前的// 600 for 0.8 flux command; 100 for 1.2 flux command - - - -#define AB2M(A, B, COS, SIN) ( (A)*COS + (B)*SIN ) -#define AB2T(A, B, COS, SIN) ( (A)*-SIN + (B)*COS ) -#define MT2A(M, T, COS, SIN) ( (M)*COS - (T)*SIN ) -#define MT2B(M, T, COS, SIN) ( (M)*SIN + (T)*COS ) - - -/* Macro for External Access Interface */ -#define US(X) im.us[X] -#define IS(X) im.is[X] -#define US_C(X) im.us_curr[X] -#define IS_C(X) im.is_curr[X] -#define US_P(X) im.us_prev[X] -#define IS_P(X) im.is_prev[X] - -#define OB_FLUX(X) ob.xCM[X] //ob.xCM[0]*IM.Lr_slash_Lm; -#define OB_OMG ob.taao_omg -#define OB_ALPHA ob.taao_alpha -#define OB_LMU ob.taao_Leq -#define OB_LSIGMA ob.taao_Lsigma -#define OB_RS ob.taao_rs -#define OB_RREQ ob.taao_rreq - -#define OB_LMU_INV (1.0/OB_LMU) -#define OB_TLOAD IM.Tload - -struct InductionMachine{ - double us[2]; - double is[2]; - double us_curr[2]; - double is_curr[2]; - double us_prev[2]; - double is_prev[2]; - - double Js; - double Js_inv; - double Lm; - double Lm_inv; - - double Lls; - double Llr; - double Ls; - double Lr; - double sigma; - double rs; - double rr; - double Tr; - double alpha; - double Lsigma; - double Leq; - double Leq_inv; - double rreq; - - double npp; - double omg; - double omg_curr; - double omg_prev; - - double theta_r; -}; -extern struct InductionMachine im; - -struct ObserverControl{ - double k_VM; - double k_CM; - double k_mec; - - double xVM[2]; // rotor flux of VM - double xCM[2]; // rotor flux of CM - double statorFlux[2]; // stator flux of VM - double varPi; // cascaded mechanical model - double Tem; - - double mismatch[3]; - // double pmismatch[2]; - double mismatch_mod; - // double mismatch_phase; - double error[2]; - - double gamma1; // rs - double gamma2; // Lsigma - double gamma3; // rreq - double gamma4; // Leq - double gamma5; // omg - double gamma5P; // omg - - double timebase; - double Ts; - - double taao_b[2]; - double taao_rs; - double taao_Lsigma; - double taao_rreq; - double taao_Leq; - - double taao_alpha; - - double taao_omg; - double taao_omg_integralPart; - double taao_speed; - double taao_invJs; - double taao_TLslashJs; - - double omega_e; - - double taao_flux_cmd; - int taao_flux_cmd_on; - - double taao_VMFlux_fed; - double taao_CMHpfFlux_fed; - - double taao_thetaA; - double taao_cos_thetaA; - double taao_sin_thetaA; - - // double uMs; // for Tsuji's CM whose magnitude will never fail, which is worth a try. - // double uTs; - // double iMs; - // double iTs; -}; -extern struct ObserverControl ob; - -void acm_init(); -void ob_init(); -void observation(); -double TAAO_FluxModulusCommand(); - -#endif -#endif -/* Consistent with DSP Programs End */ - diff --git a/README.md b/README.md index 7bb7386..d5dc49a 100644 --- a/README.md +++ b/README.md @@ -10,17 +10,21 @@ The benefit is that you can direct reuse the codes in DSP based motor drive. The numerical integration method is currently RK4, which is quite enough. DoPri54 will be included in future version (including stiffness detection and variable step numerical integration). -## Introduction to Current Branches: -- [IM] master: combination of other branches, kept in a developing status in which I feel good. +## Introduction to Current Branches (in time order): - [IM] vvvf: the skeleton with induction motor simulation and VVVF control. - [IM] foc: field oriented control (direct/indirect) with basic sensorless control. -- [IM] animate: test the feature of waveform (results) animation. +- [IM] animate: test the feature of waveform animation like an oscilloscope. - [PMSM] pmsm: id=0 control for interior permanent magnet synchronous motor. - [IM] vi_decouple: add voltage-current decoupling circuit for improved control performance during high speed reversal. - [IM] mras: model reference adaptive system based sensorless control (my 2017-Chen.Huang-Online paper). - [IM] \_femm: (This branch does not really belong here but I don't want to create a new repository for it...) It is about the design of the induction motor using free softwares as well as fitting the design to the equivalent circuit parameters for further control simulation. - [IM] saturation: include iron core saturation effect into the induction motor model simulation. - [Both] inverter_model: simple inverter modeling based on the paper 1996-Choi.Sul-Inverter. +- [Both] master: contain all the features of the branches mentioned above. The master branch is not updated anymore from this point, because I realized that having both IM and PMSM codes in one place is a silly idea. +- [PMSM] commissioning_pmsm: (under developing) self-commissioning procedure for permanent magnet motor. +- [PMSM] eemf: (still debugging) sensorless control based on extended emf method proposed by Zhiqian Chen et al. (2003). Sensorless open loop works but closed-loop has problems. + +(I recommend to use GitKraken to view this repository, so you know clearly which branch is the newest one. Note that master is obsolete. Sorry about my poor codes management skills T-T.) ## Visualization - The plots are made using package matplotlib. @@ -41,8 +45,15 @@ DoPri54 will be included in future version (including stiffness detection and va ## Video Tutorials For unfamiliar users, I have been creating video tutorials. However, they are currently in Chinese. -In near future, tge Engligh version will be brought about. - -> If you speak Chinese, I have a dedicated tutorial video on how to make this thing work from the ground up. -> Please take a look at this link to [知乎](https://zhuanlan.zhihu.com/p/64445558). -> In fact, now you can check out [my personal page](https://horychen.github.io) for a list of tutorial videos. +> *See [my Bilibili space](https://space.bilibili.com/7132537) (ACTIVELY UPDATED).* +> +> Also take a look at this link to [知乎](https://zhuanlan.zhihu.com/p/64445558) (not acvtively updated). +> +> Also check out [my personal page](https://horychen.github.io) for a list of tutorial videos (not acvtively updated). + +In near future, the English version will be brought about. But I would like to do it in high quality so I procrastinate to do so. + + diff --git a/controller.c b/controller.c index f7bd4f8..79b566c 100644 --- a/controller.c +++ b/controller.c @@ -2,7 +2,7 @@ /* PI Control * */ -double PI(struct PI_Reg *r, double err){ +double PID(struct PID_Reg *r, double err){ #define I_STATE r->i_state #define I_LIMIT r->i_limit double output; @@ -24,191 +24,6 @@ double PI(struct PI_Reg *r, double err){ } -#if MACHINE_TYPE == INDUCTION_MACHINE -/* Initialization */ -struct ControllerForExperiment CTRL; -void CTRL_init(){ - int i=0,j=0; - - CTRL.timebase = 0.0; - - /* Parameter (including speed) Adaptation */ - CTRL.rs = ACM.rs; - CTRL.rreq = ACM.rreq; - CTRL.Lsigma = ACM.Lsigma; - CTRL.alpha = ACM.alpha; - CTRL.Lmu = ACM.Lmu; - CTRL.Lmu_inv = 1.0/ACM.Lmu; - CTRL.Js = ACM.Js; - CTRL.Js_inv = 1.0/ACM.Js; - - CTRL.ual = 0.0; - CTRL.ube = 0.0; - - CTRL.rpm_cmd = 0.0; - CTRL.rotor_flux_cmd = 0.5; - - CTRL.omg_ctrl_err = 0.0; - CTRL.speed_ctrl_err = 0.0; - - CTRL.omg_fb = 0.0; - CTRL.ial_fb = 0.0; - CTRL.ibe_fb = 0.0; - CTRL.psi_mu_al_fb = 0.0; - CTRL.psi_mu_be_fb = 0.0; - - CTRL.theta_M = 0.0; - CTRL.cosT = 0.0; - CTRL.sinT = 1; - - CTRL.uMs_cmd = 0.0; - CTRL.uTs_cmd = 0.0; - CTRL.iMs_cmd = 0.0; - CTRL.iTs_cmd = 0.0; - - CTRL.omega_syn = 0.0; - CTRL.omega_sl = 0.0; - - // ver. IEMDC - CTRL.pi_speed.Kp = 0.5; - CTRL.pi_speed.Ti = 5; - CTRL.pi_speed.Ki = (CTRL.pi_speed.Kp*4.77) / CTRL.pi_speed.Ti * (TS*VC_LOOP_CEILING*DOWN_FREQ_EXE_INVERSE); - CTRL.pi_speed.i_state = 0.0; - CTRL.pi_speed.i_limit = 8; - - printf("Kp_omg=%g, Ki_omg=%g\n", CTRL.pi_speed.Kp, CTRL.pi_speed.Ki); - - CTRL.pi_iMs.Kp = 15; // cutoff frequency of 1530 rad/s - CTRL.pi_iMs.Ti = 0.08; - CTRL.pi_iMs.Ki = CTRL.pi_iMs.Kp/CTRL.pi_iMs.Ti*TS; // =0.025 - CTRL.pi_iMs.i_state = 0.0; - CTRL.pi_iMs.i_limit = 350; //350.0; // unit: Volt - - CTRL.pi_iTs.Kp = 15; - CTRL.pi_iTs.Ti = 0.08; - CTRL.pi_iTs.Ki = CTRL.pi_iTs.Kp/CTRL.pi_iTs.Ti*TS; - CTRL.pi_iTs.i_state = 0.0; - CTRL.pi_iTs.i_limit = 650; // unit: Volt, 350V->max 1300rpm - - printf("Kp_cur=%g, Ki_cur=%g\n", CTRL.pi_iMs.Kp, CTRL.pi_iMs.Ki); -} -void control(double speed_cmd, double speed_cmd_dot){ - // OPEN LOOP CONTROL - #if CONTROL_STRATEGY == VVVF_CONTROL - #define VF_RATIO 18 //18.0 // 8 ~ 18 shows saturated phenomenon - double freq = 2; // 0.15 ~ 0.5 ~ 2 （0.1时电压李萨茹就变成一个圆了） - double volt = VF_RATIO*freq; - CTRL.ual = volt*cos(2*M_PI*freq*CTRL.timebase); - CTRL.ube = volt*sin(2*M_PI*freq*CTRL.timebase); - return; - #endif - - // Input 1 is feedback: estimated speed or measured speed - #if SENSORLESS_CONTROL - CTRL.omg_fb = OB_OMG; - // #if OBSERVER_APPLIED == TAJIMA96 - // CTRL.omega_syn = ob.tajima.omega_syn; - // CTRL.omega_sl = ob.tajima.omega_sl; - // #endif - #else - CTRL.omg_fb = im.omg; - #endif - // Input 2 is feedback: measured current - CTRL.ial_fb = IS_C(0); - CTRL.ibe_fb = IS_C(1); - // Input 3 differs for DFOC and IFOC - #if CONTROL_STRATEGY == DFOC - // DFOC: estimated flux components in alpha-beta frame - CTRL.psi_mu_al_fb = OB_FLUX(0); - CTRL.psi_mu_be_fb = OB_FLUX(1); - #elif CONTROL_STRATEGY == IFOC - // IFOC: estimated rotor resistance - CTRL.rreq = OB_RREQ; - #else - #endif - - // Flux (linkage) command - CTRL.rotor_flux_cmd = 1.5; // f(speed, dc bus voltage, last torque current command) - // CTRL.rotor_flux_cmd = 3; - // 1. speed is compared with the base speed to decide flux weakening or not - // 2. dc bus voltage is required for certain application - // 3. last torque current command is required for loss minimization - - // M-axis current command - CTRL.iMs_cmd = CTRL.rotor_flux_cmd*CTRL.Lmu_inv + M1*OMG1*cos(OMG1*CTRL.timebase) / CTRL.rreq; - // printf("%g, %g, %g\n", CTRL.Lmu_inv, CTRL.iMs_cmd, CTRL.iTs_cmd); - - // T-axis current command - static int vc_count = 0; - if(vc_count++==VC_LOOP_CEILING*DOWN_FREQ_EXE_INVERSE){ - vc_count = 0; - CTRL.omg_ctrl_err = CTRL.omg_fb - speed_cmd*RPM_2_RAD_PER_SEC; - CTRL.iTs_cmd = - PI(&CTRL.pi_speed, CTRL.omg_ctrl_err); - - CTRL.speed_ctrl_err = CTRL.omg_ctrl_err * RAD_PER_SEC_2_RPM; - } - - - #if CONTROL_STRATEGY == DFOC - // feedback field orientation - double modulus = sqrt(CTRL.psi_mu_al_fb*CTRL.psi_mu_al_fb + CTRL.psi_mu_be_fb*CTRL.psi_mu_be_fb); - if(modulus<1e-3){ - CTRL.cosT = 1; - CTRL.sinT = 0; - }else{ - CTRL.cosT = CTRL.psi_mu_al_fb / modulus; - CTRL.sinT = CTRL.psi_mu_be_fb / modulus; - } - #elif CONTROL_STRATEGY == IFOC - // Feed-forward field orientation - CTRL.theta_M += TS * CTRL.omega_syn; - - if(CTRL.theta_M > M_PI){ - CTRL.theta_M -= 2*M_PI; - }else if(CTRL.theta_M < -M_PI){ - CTRL.theta_M += 2*M_PI; // 反转！ - } - - #if VOLTAGE_CURRENT_DECOUPLING_CIRCUIT - CTRL.omega_sl = CTRL.rreq*CTRL.iTs / CTRL.rotor_flux_cmd; - #else - CTRL.omega_sl = CTRL.rreq*CTRL.iTs_cmd / CTRL.rotor_flux_cmd; - #endif - - CTRL.omega_syn = CTRL.omg_fb + CTRL.omega_sl; - - CTRL.cosT = cos(CTRL.theta_M); - CTRL.sinT = sin(CTRL.theta_M); - #endif - - // Measured current in M-T frame - CTRL.iMs = AB2M(CTRL.ial_fb, CTRL.ibe_fb, CTRL.cosT, CTRL.sinT); - CTRL.iTs = AB2T(CTRL.ial_fb, CTRL.ibe_fb, CTRL.cosT, CTRL.sinT); - - // Voltage command in M-T frame - double vM, vT; - vM = - PI(&CTRL.pi_iMs, CTRL.iMs-CTRL.iMs_cmd); - vT = - PI(&CTRL.pi_iTs, CTRL.iTs-CTRL.iTs_cmd); - - // Current loop decoupling (skipped, see Chen.Huang-Stable) - { // Steady state dynamics based decoupling circuits for current regulation - #if VOLTAGE_CURRENT_DECOUPLING_CIRCUIT == TRUE - CTRL.uMs_cmd = vM + (CTRL.Lsigma) *(-CTRL.omega_syn*CTRL.iTs); // Telford03/04 - CTRL.uTs_cmd = vT + CTRL.omega_syn*(CTRL.rotor_flux_cmd + CTRL.Lsigma*CTRL.iMs); // 这个行，但是无速度运行时，会导致M轴电流在转速暂态高频震荡。 - // CTRL.uMs_cmd = vM; - // CTRL.uTs_cmd = vT; - #else - CTRL.uMs_cmd = vM; - CTRL.uTs_cmd = vT; - #endif - } - - // Voltage command in alpha-beta frame - CTRL.ual = MT2A(CTRL.uMs_cmd, CTRL.uTs_cmd, CTRL.cosT, CTRL.sinT); - CTRL.ube = MT2B(CTRL.uMs_cmd, CTRL.uTs_cmd, CTRL.cosT, CTRL.sinT); -} - -#elif MACHINE_TYPE == SYNCHRONOUS_MACHINE /* Initialization */ struct ControllerForExperiment CTRL; void CTRL_init(){ @@ -230,126 +45,123 @@ void CTRL_init(){ CTRL.Js = ACM.Js; CTRL.Js_inv = 1.0 / CTRL.Js; - CTRL.omg_fb = 0.0; - CTRL.ial_fb = 0.0; - CTRL.ibe_fb = 0.0; - CTRL.psi_mu_al_fb = 0.0; - CTRL.psi_mu_be_fb = 0.0; + CTRL.omg__fb = 0.0; + CTRL.ial__fb = 0.0; + CTRL.ibe__fb = 0.0; + CTRL.psi_mu_al__fb = 0.0; + CTRL.psi_mu_be__fb = 0.0; CTRL.rotor_flux_cmd = 0.0; // id=0 control CTRL.omg_ctrl_err = 0.0; CTRL.speed_ctrl_err = 0.0; - CTRL.iMs = 0.0; - CTRL.iTs = 0.0; - - CTRL.theta_M = 0.0; CTRL.cosT = 1.0; CTRL.sinT = 0.0; CTRL.omega_syn = 0.0; - CTRL.uMs_cmd = 0.0; - CTRL.uTs_cmd = 0.0; - CTRL.iMs_cmd = 0.0; - CTRL.iTs_cmd = 0.0; + CTRL.theta_d__fb = 0.0; + CTRL.id__fb = 0.0; + CTRL.iq__fb = 0.0; + CTRL.ud_cmd = 0.0; + CTRL.uq_cmd = 0.0; + CTRL.id_cmd = 0.0; + CTRL.iq_cmd = 0.0; // ver. IEMDC - CTRL.pi_speed.Kp = 0.5; - CTRL.pi_speed.Ti = 5; - CTRL.pi_speed.Ki = (CTRL.pi_speed.Kp*4.77) / CTRL.pi_speed.Ti * (TS*VC_LOOP_CEILING*DOWN_FREQ_EXE_INVERSE); - CTRL.pi_speed.i_state = 0.0; - CTRL.pi_speed.i_limit = 8; - - printf("Kp_omg=%g, Ki_omg=%g\n", CTRL.pi_speed.Kp, CTRL.pi_speed.Ki); - - CTRL.pi_iMs.Kp = 15; // cutoff frequency of 1530 rad/s - CTRL.pi_iMs.Ti = 0.08; - CTRL.pi_iMs.Ki = CTRL.pi_iMs.Kp/CTRL.pi_iMs.Ti*TS; // =0.025 - CTRL.pi_iMs.i_state = 0.0; - CTRL.pi_iMs.i_limit = 650; //350.0; // unit: Volt - - CTRL.pi_iTs.Kp = 15; - CTRL.pi_iTs.Ti = 0.08; - CTRL.pi_iTs.Ki = CTRL.pi_iTs.Kp/CTRL.pi_iTs.Ti*TS; - CTRL.pi_iTs.i_state = 0.0; - CTRL.pi_iTs.i_limit = 650; // unit: Volt, 350V->max 1300rpm - - printf("Kp_cur=%g, Ki_cur=%g\n", CTRL.pi_iMs.Kp, CTRL.pi_iMs.Ki); + CTRL.PID_speed.Kp = SPEED_LOOP_PID_PROPORTIONAL_GAIN; + CTRL.PID_speed.Ti = SPEED_LOOP_PID_INTEGRAL_TIME_CONSTANT; + CTRL.PID_speed.Ki = CTRL.PID_speed.Kp / CTRL.PID_speed.Ti * (TS*SPEED_LOOP_CEILING); // 4.77 = 1 / (npp*1/60*2*pi) + CTRL.PID_speed.i_limit = SPEED_LOOP_LIMIT_NEWTON_METER; + CTRL.PID_speed.i_state = 0.0; + printf("Speed PID: Kp=%g, Ki=%g, limit=%g Nm\n", CTRL.PID_speed.Kp, CTRL.PID_speed.Ki/TS, CTRL.PID_speed.i_limit); + + CTRL.PID_id.Kp = CURRENT_LOOP_PID_PROPORTIONAL_GAIN; // cutoff frequency of 1530 rad/s + CTRL.PID_id.Ti = CURRENT_LOOP_PID_INTEGRAL_TIME_CONSTANT; + CTRL.PID_id.Ki = CTRL.PID_id.Kp/CTRL.PID_id.Ti*TS; // =0.025 + CTRL.PID_id.i_limit = CURRENT_LOOP_LIMIT_VOLTS; //350.0; // unit: Volt + CTRL.PID_id.i_state = 0.0; + printf("Current PID: Kp=%g, Ki=%g, limit=%g V\n", CTRL.PID_id.Kp, CTRL.PID_id.Ki/TS, CTRL.PID_id.i_limit); + + CTRL.PID_iq.Kp = CURRENT_LOOP_PID_PROPORTIONAL_GAIN; + CTRL.PID_iq.Ti = CURRENT_LOOP_PID_INTEGRAL_TIME_CONSTANT; + CTRL.PID_iq.Ki = CTRL.PID_iq.Kp/CTRL.PID_iq.Ti*TS; + CTRL.PID_iq.i_limit = CURRENT_LOOP_LIMIT_VOLTS; // unit: Volt, 350V->max 1300rpm + CTRL.PID_iq.i_state = 0.0; } void control(double speed_cmd, double speed_cmd_dot){ - // Input 1 is feedback: estimated speed or measured speed + // Input 1 is feedback: estimated speed/position or measured speed/position #if SENSORLESS_CONTROL - getch("Not Implemented"); - // CTRL.omg_fb ; + // getch("Not Implemented"); + // CTRL.omg__fb ; // CTRL.omega_syn ; + CTRL.omg__fb = OB_OMG; + CTRL.theta_d__fb = OB_POS; #else - CTRL.omg_fb = sm.omg; + // from measurement() in main.c + CTRL.omg__fb = sm.omg_elec; + CTRL.theta_d__fb = sm.theta_d; #endif + // Input 2 is feedback: measured current - CTRL.ial_fb = IS_C(0); - CTRL.ibe_fb = IS_C(1); - // Input 3 is the rotor d-axis position - #if SENSORLESS_CONTROL - getch("Not Implemented"); - #else - CTRL.theta_M = sm.theta_d; - #endif + CTRL.ial__fb = IS_C(0); + CTRL.ibe__fb = IS_C(1); + // Input 3 is the flux linkage command #if CONTROL_STRATEGY == NULL_D_AXIS_CURRENT_CONTROL - // Flux (linkage) command CTRL.rotor_flux_cmd = 0.0; + CTRL.cosT = cos(CTRL.theta_d__fb); + CTRL.sinT = sin(CTRL.theta_d__fb); + #else + getch("Not Implemented"); #endif - // M-axis current command - CTRL.iMs_cmd = CTRL.rotor_flux_cmd / CTRL.Ld; + // d-axis current command + CTRL.id_cmd = CTRL.rotor_flux_cmd / CTRL.Ld; - // T-axis current command + // q-axis current command static int vc_count = 0; - if(vc_count++==VC_LOOP_CEILING*DOWN_FREQ_EXE_INVERSE){ + if(vc_count++ == SPEED_LOOP_CEILING){ + // velocity control loop execution frequency is 40 times slower than current control loop execution frequency vc_count = 0; - CTRL.omg_ctrl_err = CTRL.omg_fb - speed_cmd*RPM_2_RAD_PER_SEC; - CTRL.iTs_cmd = - PI(&CTRL.pi_speed, CTRL.omg_ctrl_err); + CTRL.omg_ctrl_err = CTRL.omg__fb - speed_cmd*RPM_2_RAD_PER_SEC; + CTRL.iq_cmd = - PID(&CTRL.PID_speed, CTRL.omg_ctrl_err); + // for plot CTRL.speed_ctrl_err = CTRL.omg_ctrl_err * RAD_PER_SEC_2_RPM; } - #if CONTROL_STRATEGY == NULL_D_AXIS_CURRENT_CONTROL - CTRL.cosT = cos(CTRL.theta_M); - CTRL.sinT = sin(CTRL.theta_M); - #endif - - // Measured current in M-T frame - CTRL.iMs = AB2M(CTRL.ial_fb, CTRL.ibe_fb, CTRL.cosT, CTRL.sinT); - CTRL.iTs = AB2T(CTRL.ial_fb, CTRL.ibe_fb, CTRL.cosT, CTRL.sinT); + // Measured current in d-q frame + CTRL.id__fb = AB2M(CTRL.ial__fb, CTRL.ibe__fb, CTRL.cosT, CTRL.sinT); + CTRL.iq__fb = AB2T(CTRL.ial__fb, CTRL.ibe__fb, CTRL.cosT, CTRL.sinT); - // Voltage command in M-T frame - double vM, vT; - vM = - PI(&CTRL.pi_iMs, CTRL.iMs-CTRL.iMs_cmd); - vT = - PI(&CTRL.pi_iTs, CTRL.iTs-CTRL.iTs_cmd); + // Voltage command in d-q frame + double vd, vq; + vd = - PID(&CTRL.PID_id, CTRL.id__fb-CTRL.id_cmd); + vq = - PID(&CTRL.PID_iq, CTRL.iq__fb-CTRL.iq_cmd); // Current loop decoupling (skipped for now) - CTRL.uMs_cmd = vM; - CTRL.uTs_cmd = vT; + CTRL.ud_cmd = vd; + CTRL.uq_cmd = vq; // Voltage command in alpha-beta frame - CTRL.ual = MT2A(CTRL.uMs_cmd, CTRL.uTs_cmd, CTRL.cosT, CTRL.sinT); - CTRL.ube = MT2B(CTRL.uMs_cmd, CTRL.uTs_cmd, CTRL.cosT, CTRL.sinT); + CTRL.ual = MT2A(CTRL.ud_cmd, CTRL.uq_cmd, CTRL.cosT, CTRL.sinT); + CTRL.ube = MT2B(CTRL.ud_cmd, CTRL.uq_cmd, CTRL.cosT, CTRL.sinT); } -#endif - /* Command */ void cmd_fast_speed_reversal(double timebase, double instant, double interval, double rpm_cmd){ if(timebase > instant+2*interval){ - ACM.rpm_cmd = rpm_cmd; + ACM.rpm_cmd = 0*150 + rpm_cmd; }else if(timebase > instant+interval){ - ACM.rpm_cmd = -rpm_cmd; + ACM.rpm_cmd = 0*150 + -rpm_cmd; }else if(timebase > instant){ - ACM.rpm_cmd = rpm_cmd; + ACM.rpm_cmd = 0*150 + rpm_cmd; + }else{ + ACM.rpm_cmd = 50; // default initial command } } diff --git a/controller.h b/controller.h index 9100bd0..0e35aaf 100644 --- a/controller.h +++ b/controller.h @@ -2,69 +2,16 @@ #ifndef CONTROLLER_H #define CONTROLLER_H -#define VC_LOOP_CEILING 40 - -struct PI_Reg{ - double Kp; - double Ti; - double Ki; // Ki = Kp/Ti*TS - double i_state; - double i_limit; -}; -double PI(struct PI_Reg *r, double err); - -#if MACHINE_TYPE == INDUCTION_MACHINE -struct ControllerForExperiment{ - - double timebase; - - double ual; - double ube; - - double rs; - double rreq; - double Lsigma; - double alpha; - double Lmu; - double Lmu_inv; - - double Tload; - double rpm_cmd; - - double Js; - double Js_inv; - - double omg_fb; - double ial_fb; - double ibe_fb; - double psi_mu_al_fb; - double psi_mu_be_fb; - double rotor_flux_cmd; - - double omg_ctrl_err; - double speed_ctrl_err; - - double iMs; - double iTs; - - double theta_M; - double cosT; - double sinT; - - double omega_syn; - double omega_sl; - - double uMs_cmd; - double uTs_cmd; - double iMs_cmd; - double iTs_cmd; - - struct PI_Reg pi_speed; - struct PI_Reg pi_iMs; - struct PI_Reg pi_iTs; +struct PID_Reg{ + double Kp; // Proportional gain + double Ti; // Integral time constant + double Ki; // Ki = Kp/Ti*TS (note TS is included here for ease of discrete implementaion) + double i_state; // Integral internal state + double i_limit; // Output limit }; -#elif MACHINE_TYPE == SYNCHRONOUS_MACHINE +double PID(struct PID_Reg *r, double err); + struct ControllerForExperiment{ double timebase; @@ -83,36 +30,42 @@ struct ControllerForExperiment{ double Js; double Js_inv; - double omg_fb; - double ial_fb; - double ibe_fb; - double psi_mu_al_fb; - double psi_mu_be_fb; + double omg__fb; + double ial__fb; + double ibe__fb; + double psi_mu_al__fb; + double psi_mu_be__fb; double rotor_flux_cmd; double omg_ctrl_err; double speed_ctrl_err; - double iMs; - double iTs; - - double theta_M; double cosT; double sinT; double omega_syn; - double uMs_cmd; - double uTs_cmd; - double iMs_cmd; - double iTs_cmd; - - struct PI_Reg pi_speed; - struct PI_Reg pi_iMs; - struct PI_Reg pi_iTs; + double theta_d__fb; + double id__fb; + double iq__fb; + double ud_cmd; + double uq_cmd; + double id_cmd; + double iq_cmd; + + // double theta_M; + // double iMs; + // double iTs; + // double uMs_cmd; + // double uTs_cmd; + // double iMs_cmd; + // double iTs_cmd; + + struct PID_Reg PID_speed; + struct PID_Reg PID_id; + struct PID_Reg PID_iq; }; -#endif extern struct ControllerForExperiment CTRL; void CTRL_init(); diff --git a/main.c b/main.c index 59a33b5..3208f78 100644 --- a/main.c +++ b/main.c @@ -7,297 +7,132 @@ double fabs(double x){ return (x >= 0) ? x : -x; } -#if MACHINE_TYPE == INDUCTION_MACHINE - struct InductionMachineSimulated ACM; - void Machine_init(){ - int i; - for(i=0;i<5;++i){ - ACM.x[i] = 0.0; - } - ACM.rpm = 0.0; - ACM.rpm_cmd = 0.0; - ACM.rpm_deriv_cmd = 0.0; - ACM.Tload = 0.0; - ACM.Tem = 0.0; - - ACM.Lmu = 0.4482; - // Those parameters are introduced since branch saturation - ACM.Lls = 0.0126; - ACM.Llr = 0.0126; - ACM.Lm = 0.5*(ACM.Lmu + sqrt(ACM.Lmu*ACM.Lmu + 4*ACM.Llr*ACM.Lmu)); - ACM.Lm_slash_Lr = ACM.Lm/(ACM.Lm+ACM.Llr); - ACM.Lr_slash_Lm = (ACM.Lm+ACM.Llr)/ACM.Lm; // i_dreq = ACM.idr*ACM.Lr_slash_Lm - ACM.LSigmal = 1.0 / (1.0 / ACM.Lls + 1.0 / ACM.Llr); - ACM.Lsigma = ACM.Lm + ACM.Lls - ACM.Lmu; // = Ls * (1.0 - Lm*Lm/Ls/Lr); - printf("Validate: %g = %g?\n", ACM.Lsigma, (ACM.Lm+ACM.Lls) * (1.0 - ACM.Lm*ACM.Lm/(ACM.Lm+ACM.Lls)/(ACM.Lm+ACM.Llr)) ); - - ACM.rreq = 1.69; - ACM.rs = 3.04; - ACM.rr = ACM.rreq * ACM.Lr_slash_Lm*ACM.Lr_slash_Lm; - - ACM.alpha = ACM.rreq / (ACM.Lmu); - ACM.Lmu_inv= 1.0/ACM.Lmu; - - ACM.Js = 0.0636; // Awaya92 using im.omg - ACM.npp = 2; - ACM.mu_m = ACM.npp/ACM.Js; - - ACM.Ts = MACHINE_TS; - - ACM.ial = 0.0; - ACM.ibe = 0.0; - - ACM.ual = 0.0; - ACM.ube = 0.0; - - // Those variables are introduced since branch saturation - ACM.iqs = 0.0; - ACM.ids = 0.0; - ACM.iqr = 0.0; - ACM.idr = 0.0; - ACM.psimq = 0.0; - ACM.psimd = 0.0; - } -#elif MACHINE_TYPE == SYNCHRONOUS_MACHINE - struct SynchronousMachineSimulated ACM; - void Machine_init(){ - int i; - for(i=0;i<5;++i){ - ACM.x[i] = 0.0; - } - ACM.rpm = 0.0; - ACM.rpm_cmd = 0.0; - ACM.rpm_deriv_cmd = 0.0; - ACM.Tload = 0.0; - ACM.Tem = 0.0; - - ACM.R = 0.45; - ACM.Ld = 4.15*1e-3; - ACM.Lq = 16.74*1e-3; - ACM.KE = 0.504; // Vs/rad - ACM.L0 = 0.5*(ACM.Ld + ACM.Lq); - ACM.L1 = 0.5*(ACM.Ld - ACM.Lq); - - ACM.Js = 0.06; // Awaya92 using ACM.omg - ACM.npp = 2; - ACM.mu_m = ACM.npp/ACM.Js; - - ACM.Ts = MACHINE_TS; +struct SynchronousMachineSimulated ACM; +void Machine_init(){ - ACM.id = 0.0; - ACM.iq = 0.0; + ACM.Ts = MACHINE_TS; - ACM.ial = 0.0; - ACM.ibe = 0.0; - - ACM.ud = 0.0; - ACM.uq = 0.0; - - ACM.ual = 0.0; - ACM.ube = 0.0; - - ACM.theta_d = 0.0; - } -#endif - -/* Saturation Model (also considers inverter model) */ -void collectCurrents(double *x){ - // Generalised Current by Therrien2013 - ACM.izq = x[1]/ACM.Lls + x[3]/ACM.Llr; - ACM.izd = x[0]/ACM.Lls + x[2]/ACM.Llr; - ACM.iz = sqrt(ACM.izd*ACM.izd + ACM.izq*ACM.izq); - - if(ACM.iz>1e-8){ - #if SATURATED_MAGNETIC_CIRCUIT - ACM.psim = sat_lookup(ACM.iz, satLUT); - ACM.im = ACM.iz - ACM.psim/ACM.LSigmal; - { - ACM.Lm = ACM.psim/ACM.im; - ACM.Lmu = ACM.Lm*ACM.Lm/(ACM.Lm+ACM.Llr); - ACM.Lmu_inv = 1.0/ACM.Lmu; - ACM.alpha = ACM.rr/(ACM.Lm+ACM.Llr); - ACM.rreq = ACM.Lmu*ACM.alpha; - ACM.Lsigma = (ACM.Lls+ACM.Lm) - ACM.Lmu; - ACM.Lm_slash_Lr = ACM.Lm/(ACM.Lm+ACM.Llr); - ACM.Lr_slash_Lm = (ACM.Lm+ACM.Llr)/ACM.Lm; - } - #else - ACM.psim = 1.0/(1.0/ACM.Lm+1.0/ACM.Lls+1.0/ACM.Llr)*ACM.iz; - #endif - - ACM.psimq = ACM.psim/ACM.iz*ACM.izq; - ACM.psimd = ACM.psim/ACM.iz*ACM.izd; - }else{ - #if ACMSIMC_DEBUG - printf("how to handle zero iz?\n"); - #endif - ACM.psimq = 0; - ACM.psimd = 0; - } - - ACM.iqs = (x[1] - ACM.psimq) / ACM.Lls; - ACM.ids = (x[0] - ACM.psimd) / ACM.Lls; - ACM.iqr = (x[3] - ACM.psimq) / ACM.Llr; - ACM.idr = (x[2] - ACM.psimd) / ACM.Llr; - - // /* Direct compute is ir from psis psir */ - // ACM.iqs = (x[1] - (ACM.Lm/(ACM.Lm+ACM.Llr))*x[3])/ACM.Lsigma; - // ACM.ids = (x[0] - (ACM.Lm/(ACM.Lm+ACM.Llr))*x[2])/ACM.Lsigma; - // ACM.iqr = (x[3] - (ACM.Lm/(ACM.Lm+ACM.Lls))*x[1])/(ACM.Lm+ACM.Llr-ACM.Lm*ACM.Lm/(ACM.Lm+ACM.Lls)); - // ACM.idr = (x[2] - (ACM.Lm/(ACM.Lm+ACM.Lls))*x[0])/(ACM.Lm+ACM.Llr-ACM.Lm*ACM.Lm/(ACM.Lm+ACM.Lls)); -} -void rK5_satDynamics(double t, double *x, double *fx){ - /* argument t is omitted*/ - - /* STEP ZERO: collect all the currents: is, ir, iz */ - collectCurrents(x); - - /* STEP ONE: Inverter Nonlinearity Considered */ - ACM.ual; // not CTRL.ual - ACM.ube; // not CTRL.ube - - /* STEP TWO: electromagnetic model with full flux states in alpha-beta frame */ - fx[1] = ACM.ube - ACM.rs*ACM.iqs; // 这里是反过来的！Q轴在前面！ - fx[0] = ACM.ual - ACM.rs*ACM.ids; - fx[3] = - ACM.rr*ACM.iqr + x[4]*x[2]; // 这里是反过来的！Q轴在前面！ - fx[2] = - ACM.rr*ACM.idr - x[4]*x[3]; - - /* STEP THREE: mechanical model */ - // ACM.Tem = ACM.npp*(ACM.Lm/(ACM.Lm+ACM.Llr))*(ACM.iqs*x[2]-ACM.ids*x[3]); // this is not better - ACM.Tem = ACM.npp*(ACM.iqs*x[0]-ACM.ids*x[1]); - fx[4] = (ACM.Tem - ACM.Tload)*ACM.mu_m; -} -double one_over_six = 1.0/6.0; -void rK555_Sat(double t, double *x, double hs){ - double k1[5], k2[5], k3[5], k4[5], xk[5]; - double fx[5]; int i; - - rK5_satDynamics(t, x, fx); // timer.t, - for(i=0;i<5;++i){ - k1[i] = fx[i] * hs; - xk[i] = x[i] + k1[i]*0.5; - } - - rK5_satDynamics(t, xk, fx); // timer.t+hs/2., - for(i=0;i<5;++i){ - k2[i] = fx[i] * hs; - xk[i] = x[i] + k2[i]*0.5; + for(i=0;i M_PI){ - ACM.theta_d -= 2*M_PI; - }else if(ACM.theta_d < -M_PI){ - ACM.theta_d += 2*M_PI; // 反转！ - } - ACM.x[3] = ACM.theta_d; - - ACM.id = ACM.x[0]; - ACM.iq = ACM.x[1]; - ACM.ial = MT2A(ACM.id, ACM.iq, cos(ACM.theta_d), sin(ACM.theta_d)); - ACM.ibe = MT2B(ACM.id, ACM.iq, cos(ACM.theta_d), sin(ACM.theta_d)); - ACM.rpm = ACM.x[2] * 60 / (2 * M_PI * ACM.npp); - #endif + // solve for ACM.x with ACM.ud and ACM.uq as inputs + RK_Linear(CTRL.timebase, ACM.x, ACM.Ts); + // rotor position + ACM.theta_d = ACM.x[3]; + if(ACM.theta_d > M_PI){ + ACM.theta_d -= 2*M_PI; + }else if(ACM.theta_d < -M_PI){ + ACM.theta_d += 2*M_PI; // 反转！ + } + ACM.x[3] = ACM.theta_d; + + // currents + ACM.id = ACM.x[0]; + ACM.iq = ACM.x[1]; + ACM.ial = MT2A(ACM.id, ACM.iq, cos(ACM.theta_d), sin(ACM.theta_d)); + ACM.ibe = MT2B(ACM.id, ACM.iq, cos(ACM.theta_d), sin(ACM.theta_d)); + + // speed + ACM.omg_elec = ACM.x[2]; + ACM.rpm = ACM.x[2] * 60 / (2 * M_PI * ACM.npp); + + // extended emf + ACM.eemf_q = (ACM.Ld-ACM.Lq) * (ACM.omg_elec*ACM.id - ACM.x_dot[1]) + ACM.omg_elec*ACM.KE; + ACM.eemf_al = ACM.eemf_q * -sin(ACM.theta_d); + ACM.eemf_be = ACM.eemf_q * cos(ACM.theta_d); + // ACM.theta_d__eemf = atan2(-ACM.eemf_al, ACM.eemf_be); + ACM.theta_d__eemf = atan2(-ACM.eemf_al*sign(ACM.omg_elec), ACM.eemf_be*sign(ACM.omg_elec)); + + // detect bad simulation if(isNumber(ACM.rpm)){ return false; }else{ @@ -311,18 +146,12 @@ void measurement(){ US_P(0) = US_C(0); US_P(1) = US_C(1); - #if MACHINE_TYPE == INDUCTION_MACHINE - IS_C(0) = ACM.ial; - IS_C(1) = ACM.ibe; - im.omg = ACM.x[4]; - im.theta_r = ACM.x[5]; - #elif MACHINE_TYPE == SYNCHRONOUS_MACHINE - IS_C(0) = ACM.ial; - IS_C(1) = ACM.ibe; - sm.omg = ACM.x[2]; - sm.theta_d = ACM.x[3]; - sm.theta_r = sm.theta_d; - #endif + IS_C(0) = ACM.ial; + IS_C(1) = ACM.ibe; + sm.omg_elec = ACM.x[2]; + sm.omg_mech = sm.omg_elec * sm.npp_inv; + sm.theta_d = ACM.x[3]; + // sm.theta_r = sm.theta_d; } void inverter_model(){ @@ -340,12 +169,7 @@ void inverter_model(){ #else ACM.ual = CTRL.ual; ACM.ube = CTRL.ube; - #endif - - // 冗余变量赋值 - #if MACHINE_TYPE == INDUCTION_MACHINE - ; - #elif MACHINE_TYPE == SYNCHRONOUS_MACHINE + // 仿真是在永磁体磁场定向系下仿真的哦 ACM.ud = AB2M(ACM.ual, ACM.ube, cos(ACM.theta_d), sin(ACM.theta_d)); ACM.uq = AB2T(ACM.ual, ACM.ube, cos(ACM.theta_d), sin(ACM.theta_d)); #endif @@ -353,32 +177,42 @@ void inverter_model(){ int main(){ - printf("NUMBER_OF_LINES: %d\n\n", NUMBER_OF_LINES); + printf("NUMBER_OF_STEPS: %d\n\n", NUMBER_OF_STEPS); /* Initialization */ Machine_init(); CTRL_init(); - acm_init(); + sm_init(); ob_init(); FILE *fw; - fw = fopen("algorithm.dat", "w"); + fw = fopen(DATA_FILE_NAME, "w"); + printf("%s\n", DATA_FILE_NAME); + printf("%s\n", DATA_FILE_NAME); + printf("%s\n", DATA_FILE_NAME); write_header_to_file(fw); /* MAIN LOOP */ - clock_t begin, end; + clock_t begin, end; begin = clock(); int _; // _ for the outer iteration - int dfe=0; // dfe for down frequency execution - for(_=0;_10){ + // ACM.rpm_cmd = 2000; + // } - ACM.Tload = 0 * sign(ACM.rpm); // No-load test + /* Load Torque */ + // ACM.Tload = 0 * sign(ACM.rpm); // No-load test // ACM.Tload = ACM.Tem; // Blocked-rotor test + ACM.Tload = 2 * sign(ACM.rpm); /* Simulated ACM */ if(machine_simulation()){ @@ -386,10 +220,10 @@ int main(){ break; } - if(++dfe==DOWN_FREQ_EXE){ - dfe = 0; + if(++dfe_counter == TS_UPSAMPLING_FREQ_EXE_INVERSE){ + dfe_counter = 0; - /* Time */ + /* Time in DSP */ CTRL.timebase += TS; measurement(); @@ -416,22 +250,13 @@ int main(){ /* Utility */ void write_header_to_file(FILE *fw){ - #if MACHINE_TYPE == INDUCTION_MACHINE - // no space is allowed! - // fprintf(fw, "x0,x1,x2,x3,rpm,uMs_cmd,uTs_cmd,iMs_cmd,iMs,iTs_cmd,iTs,psi_mu_al,tajima_rpm\n"); - // fprintf(fw, "$x_0$,$x_1$,$x_2$,$x_3$,Speed [rpm],$u_{Ms}^*$,$u_{Ts}^*$,$i_{Ms}^*$,$i_{Ms}$,$i_{Ts}^*$,$i_{Ts}$,$\\psi_{\\alpha\\mu}$,tajima_rpm\n"); - // fprintf(fw, "ACM.x[0],ACM.x[1],ACM.x[2],ACM.x[3],ACM.x[4],ACM.Tem,CTRL.uMs_cmd,CTRL.uTs_cmd,CTRL.iMs_cmd,CTRL.iMs,CTRL.iTs_cmd,CTRL.iTs,ob.psi_mu_al,ob.tajima.omg*RAD_PER_SEC_2_RPM,ACM.rpm\n"); - fprintf(fw, "ACM.x[0],ACM.x[1],ACM.x[2],ACM.x[3],ACM.x[4],ACM.Tem,ACM.ual,ACM.ube,ACM.rpm_cmd,e_omega\n"); - #elif MACHINE_TYPE == SYNCHRONOUS_MACHINE - // no space is allowed! - fprintf(fw, "x0,x1,x2,x3,uMs_cmd,uTs_cmd,iMs_cmd,iMs,iTs_cmd,iTs\n"); - #endif - + // no space is allowed! + fprintf(fw, "x0(id)[A],x1(iq)[A],x2(speed)[rad/s],x3(position)[rad],ud_cmd[V],uq_cmd[V],id_cmd[A],id_err[A],iq_cmd[A],iq_err[A],|eemf|[V],eemf_be[V],theta_d[rad],theta_d__eemf[rad],mismatch[rad],sin(mismatch)[rad],OB_POS,sin(ER_POS),OB_EEMF_BE,error(OB_EEMF),ob.xEEMF_dummy[0],error(xEEMF_dummy),OB_OMG,er_omg\n"); { FILE *fw2; fw2 = fopen("info.dat", "w"); - fprintf(fw2, "TS,DOWN_SAMPLE\n"); - fprintf(fw2, "%g, %d\n", TS, DOWN_SAMPLE); + fprintf(fw2, "TS,DOWN_SAMPLE,DATA_FILE_NAME\n"); + fprintf(fw2, "%g, %d, %s\n", TS, DOWN_SAMPLE, DATA_FILE_NAME); fclose(fw2); } } @@ -444,18 +269,12 @@ void write_data_to_file(FILE *fw){ if(++j == DOWN_SAMPLE) { j=0; - #if MACHINE_TYPE == INDUCTION_MACHINE - // 数目必须对上，否则ACMAnimate会失效，但是不会影响ACMPlot - fprintf(fw, "%g,%g,%g,%g,%g,%g,%g,%g,%g,%g\n", - ACM.x[0],ACM.x[1],ACM.x[2],ACM.x[3],ACM.x[4],ACM.Tem, - ACM.ual,ACM.ube,ACM.rpm_cmd,ACM.rpm_cmd-ACM.x[4]*RAD_PER_SEC_2_RPM - ); - #elif MACHINE_TYPE == SYNCHRONOUS_MACHINE - fprintf(fw, "%g,%g,%g,%g,%g,%g,%g,%g,%g,%g\n", - ACM.x[0], ACM.x[1], ACM.x[2], ACM.x[3], - CTRL.uMs_cmd, CTRL.uTs_cmd, CTRL.iMs_cmd, CTRL.iMs, CTRL.iTs_cmd, CTRL.iTs - ); - #endif + fprintf(fw, "%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g\n", + ACM.x[0], ACM.x[1], ACM.x[2], ACM.x[3], CTRL.ud_cmd, CTRL.uq_cmd, + CTRL.id_cmd, CTRL.id__fb-CTRL.id_cmd, CTRL.iq_cmd, CTRL.iq__fb-CTRL.iq_cmd, ACM.eemf_q, ACM.eemf_be, + ACM.theta_d, ACM.theta_d__eemf,ACM.theta_d-ACM.theta_d__eemf,sin(ACM.theta_d-ACM.theta_d__eemf), + OB_POS, sin(ACM.theta_d-OB_POS), OB_EEMF_BE, ACM.eemf_be-OB_EEMF_BE, ob.xEEMF_dummy[1], OB_EEMF_BE-ob.xEEMF_dummy[1], OB_OMG, ACM.omg_elec-OB_OMG + ); } } @@ -468,7 +287,7 @@ void write_data_to_file(FILE *fw){ int isNumber(double x){ // This looks like it should always be true, - // but it's false if x is a NaN (1.#QNAN0). + // but it's false if x is an NaN (1.#QNAN0). return (x == x); // see https://www.johndcook.com/blog/IEEE_exceptions_in_cpp/ cb: https://stackoverflow.com/questions/347920/what-do-1-inf00-1-ind00-and-1-ind-mean } diff --git a/observer.c b/observer.c index f908fe4..47d0a23 100644 --- a/observer.c +++ b/observer.c @@ -1,91 +1,10 @@ #include "ACMSim.h" -#if MACHINE_TYPE == INDUCTION_MACHINE && OBSERVER_APPLIED == TAJIMA96 -struct InductionMachine im; -struct Observer ob; -void acm_init(){ - int i; - for(i=0; i<2; ++i){ - im.us[i] = 0; - im.is[i] = 0; - im.us_curr[i] = 0; - im.is_curr[i] = 0; - im.us_prev[i] = 0; - im.is_prev[i] = 0; - } - - im.Js = ACM.Js; - im.Js_inv = 1./im.Js; - im.rs = ACM.rs; - im.rreq = ACM.rreq; - im.alpha = ACM.alpha; - im.Lsigma = ACM.Lsigma; - im.Lsigma_inv = 1/im.Lsigma; - im.Lmu = ACM.Lmu; - im.Lmu_inv = ACM.Lmu_inv; - im.npp = ACM.npp; - im.omg = 0; - im.theta_r = 0.0; -} -void ob_init(){ - - ob.Js = im.Js; - ob.Js_inv = im.Js_inv; - ob.rs = im.rs; - ob.rreq = im.rreq; - ob.alpha = im.alpha; - ob.Lsigma = im.Lsigma; - ob.Lsigma_inv = im.Lsigma_inv; - ob.Lmu = im.Lmu; - ob.Lmu_inv = im.Lmu_inv; - ob.npp = im.npp; - ob.omg = im.omg; - im.theta_r = 0.0; - im.theta_d = 0.0; - - ob.psi_mu_al = 0.0; - ob.psi_mu_be = 0.0; -} -void observation(){ - double rotor_flux_cmd, iMs, iTs, uMs_cmd, uTs_cmd; - - rotor_flux_cmd = CTRL.rotor_flux_cmd; - iMs = CTRL.iMs; - iTs = CTRL.iTs; - uMs_cmd = CTRL.uMs_cmd; - uTs_cmd = CTRL.uTs_cmd; - - // Speed estimation: Tajima1996 - ob.tajima.K_PEM = 0.1; // 0.1 ~ 2 - ob.tajima.omega_syn = (uTs_cmd - ob.rs*iTs) / (rotor_flux_cmd + ob.Lsigma*iMs); - { - ob.tajima.e_M = uMs_cmd - ob.rs*iMs + ob.tajima.omega_syn*ob.Lsigma*iTs; - ob.tajima.omega_syn -= ob.tajima.K_PEM * ob.tajima.e_M; - } - ob.tajima.omega_sl = ob.rreq*iTs / rotor_flux_cmd; - ob.tajima.omg = ob.tajima.omega_syn - ob.tajima.omega_sl; // Instantaneous Velocity Computation - - // Flux estimation 1: Voltage model (implemented by shitty Euler method for now) - double deriv_psi_s_al; - double deriv_psi_s_be; - static double psi_s_al = 0.0; - static double psi_s_be = 0.0; - deriv_psi_s_al = US_C(0) - ob.rs*IS_C(0); - deriv_psi_s_be = US_C(1) - ob.rs*IS_C(1); - psi_s_al += TS*deriv_psi_s_al; - psi_s_be += TS*deriv_psi_s_be; - ob.psi_mu_al = psi_s_al - ob.Lsigma*IS(0); - ob.psi_mu_be = psi_s_be - ob.Lsigma*IS(1); - - // Flux estimation 2: Current model (this is a bad flux estimator) -} - - -#elif MACHINE_TYPE == SYNCHRONOUS_MACHINE struct SynchronousMachine sm; struct Observer ob; -void acm_init(){ + +void sm_init(){ int i; for(i=0; i<2; ++i){ sm.us[i] = 0; @@ -96,34 +15,201 @@ void acm_init(){ sm.is_prev[i] = 0; } - sm.Js = ACM.Js; - sm.Js_inv = 1./sm.Js; + sm.npp = PMSM_NUMBER_OF_POLE_PAIRS; + sm.npp_inv = 1.0/sm.npp; - sm.R = ACM.R; - sm.KE = ACM.KE; - sm.Ld = ACM.Ld; - sm.Lq = ACM.Lq; + sm.R = PMSM_RESISTANCE; + sm.Ld = PMSM_D_AXIS_INDUCTANCE; + sm.Ld_inv = 1/sm.Ld; + sm.Lq = PMSM_Q_AXIS_INDUCTANCE; + sm.KE = PMSM_PERMANENT_MAGNET_FLUX_LINKAGE; // Vs/rad - sm.npp = ACM.npp; - sm.omg = 0; + sm.Js = PMSM_SHAFT_INERTIA; + sm.Js_inv = 1./sm.Js; + + sm.omg_elec = 0.0; + sm.omg_mech = sm.omg_elec * sm.npp_inv; + sm.theta_d = 0.0; } + void ob_init(){ - ob.Js = ACM.Js; - ob.Js_inv = 1.0/ob.Js; + ob.R = PMSM_RESISTANCE; + ob.Ld = PMSM_D_AXIS_INDUCTANCE; + ob.Lq = PMSM_Q_AXIS_INDUCTANCE; + ob.KE = PMSM_PERMANENT_MAGNET_FLUX_LINKAGE; // Vs/rad - ob.R = ACM.R; - ob.KE = ACM.KE; - ob.Ld = ACM.Ld; - ob.Lq = ACM.Lq; + ob.Js = PMSM_SHAFT_INERTIA; + ob.Js_inv = 1.0/ob.Js; - ob.npp = ACM.npp; - ob.omg = 0.0; + ob.omg_elec = 0.0; + ob.omg_mech = ob.omg_elec * sm.npp_inv; + ob.theta_d = 0.0; ob.eemf_al = 0.0; ob.eemf_be = 0.0; + ob.eemf_q = 0.0; + + ob.xXi[0] = 0.0; + ob.xXi[1] = 0.0; + ob.xEEMF_dummy[0] = 0.0; + ob.xEEMF_dummy[1] = 0.0; + ob.xOmg = 0.0; + + ob.pll_constructed_eemf_error[0] = 0.0; + ob.pll_constructed_eemf_error[1] = 0.0; + + // ob.DeltaL = (ob.Ld - ob.Lq) / 2; + // ob.SigmaL = (ob.Ld + ob.Lq) / 2; + + ob.g1 = OB_COEF_G1; + ob.g2 = OB_COEF_G2; + ob.ell = OB_COEF_ELL; + ob.gamma = OB_COEF_GAMMA; +} + +#define NUMBER_OF_EEMF_STATES 5 +void rhs_func_eemf(double *xxn, double *xXi, double *xEEMF_dummy, double xOmg, double hs){ + + #define R ob.R + #define LD sm.Ld + #define LD_INV sm.Ld_inv + #define LQ sm.Lq + #define g1 ob.g1 // Diagonal gain for eemf observer + #define g2 ob.g2 // Off-diagonal gain for eemf observer (=0) + #define ell ob.ell // Gain for speed adaptive Luenberger observer + #define gamma ob.gamma // Gain for speed update rule + // #define OMG_USED sm.omg_elec + #define OMG_USED xOmg + + // EEMF (from reduced order observer) as reference model + ob.eemf_al = xXi[0] + g1*IS(0); + ob.eemf_be = xXi[1] + g1*IS(1); + + // Compute mismatch + ob.pll_constructed_eemf_error[0] = ob.eemf_al - xEEMF_dummy[0]; + ob.pll_constructed_eemf_error[1] = ob.eemf_be - xEEMF_dummy[1]; + + // f: + // f[0], f[1] are the derivatives of xXi. + // f[2], f[3] are the derivatives of xEEMF_dummy. + // f[4] is the derivative of xOmg. + double f[NUMBER_OF_EEMF_STATES]; + double g1_deriv = 0; + + // xXi: Reduced-order eemf observer with intermediate state xXi + f[0] = (g1*LD_INV)*xXi[0] + OMG_USED*-xXi[1] - g1*LD_INV*US(0) + ( R*LD_INV*g1 + g1*g1*LD_INV - g1_deriv)*IS(0) + ( LQ*LD_INV * OMG_USED*g1 ) *-IS(1); + f[1] = (g1*LD_INV)*xXi[1] + OMG_USED* xXi[0] - g1*LD_INV*US(1) + ( R*LD_INV*g1 + g1*g1*LD_INV - g1_deriv)*IS(1) + ( LQ*LD_INV * OMG_USED*g1 ) * IS(0); + + // xEEMF_dummy: Speed adaptive Luenberger observer or PLL (extract frequency info from the reconstructed eemf/xXi) + f[2] = xOmg*-xEEMF_dummy[1] + ell * ob.pll_constructed_eemf_error[0]; + f[3] = xOmg* xEEMF_dummy[0] + ell * ob.pll_constructed_eemf_error[1]; + + // xOmg: Speed update rule + f[4] = gamma * (-ob.pll_constructed_eemf_error[0]*xEEMF_dummy[1] + ob.pll_constructed_eemf_error[1]*xEEMF_dummy[0]); + + xxn[0] = ( f[0] )*hs; + xxn[1] = ( f[1] )*hs; + xxn[2] = ( f[2] )*hs; + xxn[3] = ( f[3] )*hs; + xxn[4] = ( f[4] )*hs; + + #undef R + #undef LD + #undef LD_INV + #undef DeltaL + #undef SigmaL + #undef g1 + #undef g2 + #undef gamma + #undef ell } +void rk4_eemf(double hs){ + static double xx1[NUMBER_OF_EEMF_STATES]; + static double xx2[NUMBER_OF_EEMF_STATES]; + static double xx3[NUMBER_OF_EEMF_STATES]; + static double xx4[NUMBER_OF_EEMF_STATES]; + static double x_temp[NUMBER_OF_EEMF_STATES]; + static double *p_x_temp=x_temp; + + /* Theoritically speaking, rhs_func should be time-varing like rhs_func(.,t). + To apply codes in DSP, we do time-varing updating of IS(0) and IS(1) outside rhs_func(.) to save time. */ + + /* + * Begin RK4 + * */ + // time instant t + US(0) = US_P(0); + US(1) = US_P(1); + IS(0) = IS_P(0); + IS(1) = IS_P(1); + rhs_func_eemf( xx1, ob.xXi, ob.xEEMF_dummy, ob.xOmg, hs ); + x_temp[0] = ob.xXi[0] + xx1[0]*0.5; + x_temp[1] = ob.xXi[1] + xx1[1]*0.5; + x_temp[2] = ob.xEEMF_dummy[0] + xx1[2]*0.5; + x_temp[3] = ob.xEEMF_dummy[1] + xx1[3]*0.5; + x_temp[4] = ob.xOmg + xx1[4]*0.5; + + // time instant t+hs/2 + IS(0) = 0.5*(IS_P(0)+IS_C(0)); + IS(1) = 0.5*(IS_P(1)+IS_C(1)); + rhs_func_eemf( xx2, p_x_temp, p_x_temp+2, *(p_x_temp+4), hs ); + x_temp[0] = ob.xXi[0] + xx2[0]*0.5; + x_temp[1] = ob.xXi[1] + xx2[1]*0.5; + x_temp[2] = ob.xEEMF_dummy[0] + xx2[2]*0.5; + x_temp[3] = ob.xEEMF_dummy[1] + xx2[3]*0.5; + x_temp[4] = ob.xOmg + xx2[4]*0.5; + + // time instant t+hs/2 + rhs_func_eemf( xx3, p_x_temp, p_x_temp+2, *(p_x_temp+4), hs ); + x_temp[0] = ob.xXi[0] + xx3[0]; + x_temp[1] = ob.xXi[1] + xx3[1]; + x_temp[2] = ob.xEEMF_dummy[0] + xx3[2]; + x_temp[3] = ob.xEEMF_dummy[1] + xx3[3]; + x_temp[4] = ob.xOmg + xx3[4]; + + // time instant t+hs + IS(0) = IS_C(0); + IS(1) = IS_C(1); + rhs_func_eemf( xx4, p_x_temp, p_x_temp+2, *(p_x_temp+4), hs ); + // \+=[^\n]*1\[(\d+)\][^\n]*2\[(\d+)\][^\n]*3\[(\d+)\][^\n]*4\[(\d+)\][^\n]*/ ([\d]+) + // += (xx1[$5] + 2*(xx2[$5] + xx3[$5]) + xx4[$5])*0.166666666666667; // $5 + ob.xXi[0] += (xx1[0] + 2*(xx2[0] + xx3[0]) + xx4[0])*0.166666666666667; // 0 + ob.xXi[1] += (xx1[1] + 2*(xx2[1] + xx3[1]) + xx4[1])*0.166666666666667; // 1 + ob.xEEMF_dummy[0] += (xx1[2] + 2*(xx2[2] + xx3[2]) + xx4[2])*0.166666666666667; // 2 + ob.xEEMF_dummy[1] += (xx1[3] + 2*(xx2[3] + xx3[3]) + xx4[3])*0.166666666666667; // 3 + ob.xOmg += (xx1[4] + 2*(xx2[4] + xx3[4]) + xx4[4])*0.166666666666667; // 4 + + // EEMF + ob.eemf_al = ob.xXi[0] + ob.g1*IS(0); + ob.eemf_be = ob.xXi[1] + ob.g1*IS(1); + ob.omg_elec = ob.xOmg; + ob.omg_mech = ob.omg_elec * sm.npp_inv; + // ob.theta_d = atan2(-ob.eemf_al, + // ob.eemf_be); // 180 deg shift when speed is negative + // ob.theta_d = atan2(-ob.eemf_al*sign(ob.omg_elec), + // ob.eemf_be*sign(ob.omg_elec)); + ob.theta_d = atan2(-ob.eemf_al*sign(ob.xOmg), + ob.eemf_be*sign(ob.xOmg)); +} + void observation(){ + + ob.g1 = OB_COEF_G1; + // if(CTRL.timebase>2){ + // ob.g1 = -fabs(OB_OMG)*sm.Ld*2; + // } + OB_OMG = sm.omg_elec; + + /* OBSERVATION */ + rk4_eemf(TS); + + /* 备份这个采样点的数据供下次使用。所以，观测的和实际的相比，是延迟一个采样周期的。 */ + // 2017年1月20日，将控制器放到了观测器的后面。 + // * 所以，上一步电压US_P的更新也要延后了。 + // US_P(0) = US_C(0); + // US_P(1) = US_C(1); + IS_P(0) = IS_C(0); + IS_P(1) = IS_C(1); } -#endif diff --git a/observer.h b/observer.h index f9e3530..6bd1010 100644 --- a/observer.h +++ b/observer.h @@ -1,135 +1,89 @@ #ifndef ADD_OBSERVER_H #define ADD_OBSERVER_H - -#if MACHINE_TYPE == INDUCTION_MACHINE && OBSERVER_APPLIED == TAJIMA96 - #define M1 0 - #define OMG1 2 - - /* Macro for External Access Interface */ - #define US(X) im.us[X] - #define IS(X) im.is[X] - #define US_C(X) im.us_curr[X] - #define IS_C(X) im.is_curr[X] - #define US_P(X) im.us_prev[X] - #define IS_P(X) im.is_prev[X] - - #define OB_RREQ ob.rreq - #define OB_OMG ob.tajima.omg - #define OB_FLUX(X) ob.psi_mu_al[X] - - struct Tajima{ - double K_PEM; - double omega_syn; - double e_M; - double omega_sl; - double omg; - }; - - struct InductionMachine{ - double us[2]; - double is[2]; - double us_curr[2]; - double is_curr[2]; - double us_prev[2]; - double is_prev[2]; - - double Js; - double Js_inv; - - double rs; - double rreq; - - double alpha; - double Lsigma; - double Lsigma_inv; - double Lmu; - double Lmu_inv; - - double npp; - double omg; - double theta_r; - double theta_d; - }; - extern struct InductionMachine im; - - struct Observer{ - double Js; - double Js_inv; - - double rs; - double rreq; - - double alpha; - double Lsigma; - double Lsigma_inv; - double Lmu; - double Lmu_inv; - - double npp; - double omg; - - double psi_mu_al; - double psi_mu_be; - - struct Tajima tajima; - }; - extern struct Observer ob; - -#elif MACHINE_TYPE == SYNCHRONOUS_MACHINE - - /* Macro for External Access Interface */ - #define US(X) sm.us[X] - #define IS(X) sm.is[X] - #define US_C(X) sm.us_curr[X] - #define IS_C(X) sm.is_curr[X] - #define US_P(X) sm.us_prev[X] - #define IS_P(X) sm.is_prev[X] - - struct SynchronousMachine{ - double us[2]; - double is[2]; - double us_curr[2]; - double is_curr[2]; - double us_prev[2]; - double is_prev[2]; - - double Js; - double Js_inv; - - double R; - double KE; - double Ld; - double Lq; - - double npp; - double omg; - double theta_r; - double theta_d; - }; - extern struct SynchronousMachine sm; - - - struct Observer{ - double Js; - double Js_inv; - - double R; - double KE; - double Ld; - double Lq; - - double npp; - double omg; - - double eemf_al; - double eemf_be; - }; - extern struct Observer ob; - -#endif - -void acm_init(); +#define OB_COEF_G1 (-fabs(sm.omg_elec)*sm.Ld*10) // Initial value +#define OB_COEF_G2 0 +#define OB_COEF_ELL 1000 // 100 //2000 +#define OB_COEF_GAMMA 5000 // 0.2 //500 + +/* Macro for External Access Interface */ +#define US(X) sm.us[X] +#define IS(X) sm.is[X] +#define US_C(X) sm.us_curr[X] +#define IS_C(X) sm.is_curr[X] +#define US_P(X) sm.us_prev[X] +#define IS_P(X) sm.is_prev[X] + +#define OB_EEMF_AL ob.eemf_al +#define OB_EEMF_BE ob.eemf_be +#define OB_POS ob.theta_d +#define OB_OMG ob.xOmg +#define OB_LD ob.Ld +#define OB_LQ ob.Lq +#define OB_R ob.R + +struct SynchronousMachine{ + double us[2]; + double is[2]; + double us_curr[2]; + double is_curr[2]; + double us_prev[2]; + double is_prev[2]; + + double npp; + double npp_inv; + + double R; + double Ld; + double Ld_inv; + double Lq; + double KE; + + double Js; + double Js_inv; + + double omg_elec; // omg_elec = npp * omg_mech + double omg_mech; + double theta_d; +}; +extern struct SynchronousMachine sm; + +struct Observer{ + + double R; + double Ld; + double Lq; + double KE; // psi_PM; + + double Js; + double Js_inv; + + double omg_elec; // omg_elec = npp * omg_mech + double omg_mech; + double theta_d; + + double eemf_al; + double eemf_be; + double eemf_q; + + double xXi[2]; + double xEEMF_dummy[2]; + double xOmg; + + double pll_constructed_eemf_error[2]; + + double DeltaL; + double SigmaL; + + double g1; + double g2; + double ell; + double gamma; +}; +extern struct Observer ob; + + +void sm_init(); void ob_init(); void observation(); diff --git a/observerTAAO.c b/observerTAAO.c deleted file mode 100644 index 546abdb..0000000 --- a/observerTAAO.c +++ /dev/null @@ -1,282 +0,0 @@ -#include "ACMSim.h" - -#if OBSERVER_APPLIED == TOTALLY_ADAPTIVE_OBSERVER - -struct InductionMachine im; -struct ObserverControl ob; - -/* Total Adaptation - * */ -double deriv_rs; -double deriv_Lsigma; -double deriv_rreq; -double deriv_Leq; -double deriv_omg; -double deriv_invJs; -double deriv_TLslashJs; -void rhs_func_14th(double *xxn, double *xVM, double *xCM, double *xB, - double xRs, double xLsigma, double xRreq, double xLeq, double xOmg, - double xVarPi, double xInvJs, double xTLslashJs, double hs){ - - // Compute mismatch - ob.xVM[0] = (xVM[0] - xLsigma * IS(0)); // VM rotor flux = VM stator flux - Lsigma*i_s - ob.xVM[1] = (xVM[1] - xLsigma * IS(1)); - ob.mismatch[0] = ob.xVM[0] - xCM[0]; - ob.mismatch[1] = ob.xVM[1] - xCM[1]; - #ifdef PC_SIMULATION - ob.error[0] = IM.x[2]*IM.Lm_slash_Lr - xCM[0]; //(STEADY_FLUX_LINKAGE_MODULUS + ob.extraModulus)*ob.cosT - xCM[0]; - ob.error[1] = IM.x[3]*IM.Lm_slash_Lr - xCM[1]; //(STEADY_FLUX_LINKAGE_MODULUS + ob.extraModulus)*ob.sinT - xCM[1]; - #endif - - /* Voltage model for stator flux */ - xxn[0] = ( US(0) - xRs*IS(0) - ob.k_VM*ob.mismatch[0] )*hs; - xxn[1] = ( US(1) - xRs*IS(1) - ob.k_VM*ob.mismatch[1] )*hs; - - /* Current model */ - double xLeq_inv = 1.0 / xLeq; - double xAlpha = xRreq*xLeq_inv; - xxn[2] = ( -xAlpha*xCM[0] + xRreq*IS(0) + xOmg*-xCM[1] + ob.k_CM*ob.mismatch[0] )*hs; - xxn[3] = ( -xAlpha*xCM[1] + xRreq*IS(1) + xOmg* xCM[0] + ob.k_CM*ob.mismatch[1] )*hs; - - /* hat _xB derivative - * */ - xxn[4] = 0.0; - xxn[5] = 0.0; - - /* hat _xRs derivative */ - deriv_rs = ob.gamma1 * (ob.mismatch[0]*IS(0) + ob.mismatch[1]*IS(1)); - xxn[6] = deriv_rs*hs; - - /* hat Lsigma derivative*/ - xxn[7] = 0; - - /* hat _xRreq derivative */ - deriv_rreq = ob.gamma3 * (ob.mismatch[0]*(IS(0)-xCM[0]*xLeq_inv) + ob.mismatch[1]*(IS(1)-xCM[1]*xLeq_inv)); - xxn[8] = deriv_rreq*hs; - - /* hat _xLeq derivative */ - // deriv_Leq = ob.gamma4 * (ob.mismatch[0]*(xAlpha*xLeq_inv*xCM[0]) + ob.mismatch[1]*(xAlpha*xLeq_inv*xCM[1])); - deriv_Leq = ob.gamma4 * (ob.mismatch[0]*xCM[0] + ob.mismatch[1]*xCM[1]); // coefficient absorbsion - xxn[9] = deriv_Leq*hs; - - /* hat _xOmg derivative */ - deriv_omg = ob.gamma5 * (ob.mismatch[0]*-xCM[1] + ob.mismatch[1]*xCM[0]); - xxn[10] = deriv_omg*hs; -} -void rK_fourteenth(double hs){ - double xx1[14]; - double xx2[14]; - double xx3[14]; - double xx4[14]; - double x_temp[14]; - double *p_x_temp=x_temp; - - /* Theoritically speaking, rhs_func should be time-varing like rhs_func(.,t). - To apply codes in DSP, we do time-varing updating of IS(0) and IS(1) outside rhs_func(.) to save time. */ - - // time instant t - US(0) = US_P(0); - US(1) = US_P(1); - IS(0) = IS_P(0); - IS(1) = IS_P(1); - rhs_func_14th( xx1, ob.statorFlux, ob.xCM, ob.taao_b, - ob.taao_rs, ob.taao_Lsigma, ob.taao_rreq, ob.taao_Leq, ob.taao_omg_integralPart, - ob.varPi, ob.taao_invJs, ob.taao_TLslashJs, hs); - x_temp[0] = ob.statorFlux[0] + xx1[0]*0.5; - x_temp[1] = ob.statorFlux[1] + xx1[1]*0.5; - x_temp[2] = ob.xCM[0] + xx1[2]*0.5; - x_temp[3] = ob.xCM[1] + xx1[3]*0.5; - x_temp[6] = ob.taao_rs + xx1[6]*0.5; - x_temp[7] = ob.taao_Lsigma + xx1[7]*0.5; - x_temp[8] = ob.taao_rreq + xx1[8]*0.5; - x_temp[9] = ob.taao_Leq + xx1[9]*0.5; - x_temp[10] = ob.taao_omg_integralPart + xx1[10]*0.5; - - // time instant t+hs/2 - IS(0) = 0.5*(IS_P(0)+IS_C(0)); - IS(1) = 0.5*(IS_P(1)+IS_C(1)); - rhs_func_14th( xx2, p_x_temp, p_x_temp+2, p_x_temp+4, - *(p_x_temp+6), *(p_x_temp+7), *(p_x_temp+8), *(p_x_temp+9), *(p_x_temp+10), - *(p_x_temp+11), *(p_x_temp+12), *(p_x_temp+13), hs); - x_temp[0] = ob.statorFlux[0] + xx2[0]*0.5; - x_temp[1] = ob.statorFlux[1] + xx2[1]*0.5; - x_temp[2] = ob.xCM[0] + xx2[2]*0.5; - x_temp[3] = ob.xCM[1] + xx2[3]*0.5; - x_temp[6] = ob.taao_rs + xx2[6]*0.5; - x_temp[7] = ob.taao_Lsigma + xx2[7]*0.5; - x_temp[8] = ob.taao_rreq + xx2[8]*0.5; - x_temp[9] = ob.taao_Leq + xx2[9]*0.5; - x_temp[10] = ob.taao_omg_integralPart + xx2[10]*0.5; - - // time instant t+hs/2 - rhs_func_14th( xx3, p_x_temp, p_x_temp+2, p_x_temp+4, - *(p_x_temp+6), *(p_x_temp+7), *(p_x_temp+8), *(p_x_temp+9), *(p_x_temp+10), - *(p_x_temp+11), *(p_x_temp+12), *(p_x_temp+13), hs); - x_temp[0] = ob.statorFlux[0] + xx3[0]; - x_temp[1] = ob.statorFlux[1] + xx3[1]; - x_temp[2] = ob.xCM[0] + xx3[2]; - x_temp[3] = ob.xCM[1] + xx3[3]; - x_temp[6] = ob.taao_rs + xx3[6]; - x_temp[7] = ob.taao_Lsigma + xx3[7]; - x_temp[8] = ob.taao_rreq + xx3[8]; - x_temp[9] = ob.taao_Leq + xx3[9]; - x_temp[10] = ob.taao_omg_integralPart + xx3[10]; - - // time instant t+hs - IS(0) = IS_C(0); - IS(1) = IS_C(1); - rhs_func_14th( xx4, p_x_temp, p_x_temp+2, p_x_temp+4, - *(p_x_temp+6), *(p_x_temp+7), *(p_x_temp+8), *(p_x_temp+9), *(p_x_temp+10), - *(p_x_temp+11), *(p_x_temp+12), *(p_x_temp+13), hs); - // \+=[^\n]*1\[(\d)\][^\n]*2\[(\d)\][^\n]*3\[(\d)\][^\n]*4\[(\d)\][^\n]*/ ([\d]+) - // += (xx1[$5] + 2*(xx2[$5] + xx3[$5]) + xx4[$5])*0.166666666666667; // $5 - ob.statorFlux[0] += (xx1[0] + 2*(xx2[0] + xx3[0]) + xx4[0])*0.166666666666667; // 0 - ob.statorFlux[1] += (xx1[1] + 2*(xx2[1] + xx3[1]) + xx4[1])*0.166666666666667; // 1 - ob.xCM[0] += (xx1[2] + 2*(xx2[2] + xx3[2]) + xx4[2])*0.166666666666667; // 2 - ob.xCM[1] += (xx1[3] + 2*(xx2[3] + xx3[3]) + xx4[3])*0.166666666666667; // 3 - ob.taao_rs += (xx1[6] + 2*(xx2[6] + xx3[6]) + xx4[6])*0.166666666666667; // 6 - ob.taao_Lsigma += (xx1[7] + 2*(xx2[7] + xx3[7]) + xx4[7])*0.166666666666667; // 7 - ob.taao_rreq += (xx1[8] + 2*(xx2[8] + xx3[8]) + xx4[8])*0.166666666666667; // 8 - ob.taao_Leq += (xx1[9] + 2*(xx2[9] + xx3[9]) + xx4[9])*0.166666666666667; // 9 - ob.taao_omg_integralPart += (xx1[10] + 2*(xx2[10] + xx3[10]) + xx4[10])*0.166666666666667; // 10 - - // 限幅 - if(ob.taao_Leq<0.2){ - ob.taao_Leq = 0.2; // Replace it with Proj() by Marino - } - - ob.taao_alpha = ob.taao_rreq/ob.taao_Leq; - - ob.taao_omg = ob.taao_omg_integralPart + ob.gamma5P * (ob.mismatch[0]*-ob.xCM[1] + ob.mismatch[1]*ob.xCM[0]); - ob.taao_speed = ob.taao_omg * RAD_PER_SEC_2_RPM; // 60/3 - - ob.xVM[0] = ob.statorFlux[0] - ob.taao_Lsigma * IS(0); - ob.xVM[1] = ob.statorFlux[1] - ob.taao_Lsigma * IS(1); -} - -/* Main Flow of Observer - * */ -void observation(){ - - /* OBSERVATION */ - if(ob.gamma5==0){ - ob.taao_omg_integralPart = im.omg; - } - rK_fourteenth(ob.Ts); // 放在最后，保证im.omg已经更新*（NON_ADAPTIVE case） - - IS_P(0) = IS_C(0); - IS_P(1) = IS_C(1); - - ob.mismatch_mod = sqrt(ob.mismatch[0]*ob.mismatch[0]+ob.mismatch[1]*ob.mismatch[1]); -} - - -/* Initialization Codes*/ -void acm_init(){ - im.us[0] = 0.0; - im.us[1] = 0.0; - im.is[0] = 0.0; - im.is[1] = 0.0; - im.us_curr[0] = 0.0; - im.us_curr[1] = 0.0; - im.is_curr[0] = 0.0; - im.is_curr[1] = 0.0; - im.us_prev[0] = 0.0; - im.us_prev[1] = 0.0; - im.is_prev[0] = 0.0; - im.is_prev[1] = 0.0; - - im.Js = 0.032; - im.Js_inv = 1.0/im.Js; - - im.Leq = 0.4482; - im.Lls = 0.0126; - #if NO_ROTOR_LEAKAGE - im.Llr = 0.0; - #else - im.Llr = 0.0126; - #endif - im.Lm = 0.5*(im.Leq+sqrtf(im.Leq*im.Leq+4*im.Llr*im.Leq)); - im.Lm_inv = 1.0/im.Lm; - im.Ls = im.Lm + im.Lls; - im.Lr = im.Lm + im.Llr; - im.sigma = 1.0 - im.Lm*im.Lm/im.Ls/im.Lr; - - im.rs = 3.04; - im.rr = 1.69; - im.Lsigma = im.Ls*im.sigma; - im.alpha = im.rr/im.Lr; - im.rreq = im.Leq * im.alpha; - im.Leq_inv = 1.0/im.Leq; - im.Tr = im.Lr/im.rr; - - im.omg = 0.0; - im.omg_curr = 0.0; - im.omg_prev = 0.0; - - im.npp = 2.0; // no. of pole pair - - im.theta_r = 0.0; -} -void ob_init(){ - - ob.k_VM = KVM_VALUE; - ob.k_CM = 0.0; - - ob.xVM[0] = 0; - ob.xVM[1] = 0; - ob.xCM[0] = 0.0; - ob.xCM[1] = 0.0; - ob.statorFlux[0] = 0.0; //0.01; // initial error exsits - ob.statorFlux[1] = 0.0; //0.01; - ob.varPi = 0.0; - ob.Tem = 0.0; - - ob.mismatch[0] = 0.0; - ob.mismatch[1] = 0.0; - ob.mismatch[2] = 0.0; - ob.mismatch_mod = 0.0; - ob.error[0] = 0.0; - ob.error[1] = 0.0; - - ob.gamma1 = GAMMA_RS; - ob.gamma2 = GAMMA_LSIGMA; - ob.gamma3 = GAMMA_RREQ; - ob.gamma4 = GAMMA_LEQ; - ob.gamma5 = GAMMA_OMEGAR; - ob.gamma5P = GAMMA_OMEGAR_P; - - ob.timebase = 0.0; - ob.Ts = TS; - - ob.taao_rs = im.rs*1.0; - ob.taao_Lsigma = im.Lsigma*1.0; - ob.taao_rreq = im.rreq*1.0; - ob.taao_Leq = im.Leq*1.0; - - ob.taao_alpha = ob.taao_rreq/ob.taao_Leq; - - ob.taao_omg = 0.0; // Actual value im.omg is updated in eQEP - ob.taao_omg_integralPart = 0.0; - ob.taao_speed = ob.taao_omg * RAD_PER_SEC_2_RPM; - - - ob.omega_e = 0.0; - - ob.taao_flux_cmd = TAAO_FLUX_COMMAND_VALUE; - ob.taao_flux_cmd_on = TAAO_FLUX_COMMAND_ON; - - } - -double TAAO_FluxModulusCommand(){ - if(ob.taao_flux_cmd_on){ - // return TAAO_FLUX_COMMAND_VALUE + M1*sin(OMG1*ob.timebase); // C + AC - return TAAO_FLUX_COMMAND_VALUE + M1*sin(OMG1*ob.timebase) + M2*sin(OMG2*ob.timebase) + M3*sin(OMG3*ob.timebase); // C + AC - }else{ - return TAAO_FLUX_COMMAND_VALUE; - } -} - -/* Consistent with DSP Programs Add_TDDA.c Terminate */ - -#endif