From 4ab46b3f9b7e6ffeb112b446e366b879037bd1ef Mon Sep 17 00:00:00 2001 From: Sgr A* VMT <1611902585@qq.com> Date: Sun, 28 Jan 2024 21:52:57 +0800 Subject: [PATCH] implement new temprature compensation --- arg_fit.py | 176 ++++++++------ idm.py | 604 ++++++++++++++++++++++++++--------------------- install.sh | 5 +- requirements.txt | 3 +- 4 files changed, 442 insertions(+), 346 deletions(-) diff --git a/arg_fit.py b/arg_fit.py index d324a97..0be2f8e 100644 --- a/arg_fit.py +++ b/arg_fit.py @@ -2,49 +2,37 @@ from scipy.optimize import curve_fit import numpy as np import matplotlib.pyplot as plt -import pandas as pd -import warnings -warnings.filterwarnings("ignore") class TempModel: - def __init__(self, amfg, tcc, tcfl, tctl, fmin, fmin_temp): - self.amfg = amfg - self.tcc = tcc - self.tcfl = tcfl - self.tctl = tctl + def __init__(self, a_a, a_b, b_a, b_b, fmin, fmin_temp): + self.a_a=a_a + self.a_b=a_b + self.b_a=b_a + self.b_b=b_b self.fmin = fmin self.fmin_temp = fmin_temp - def _tcf(self, f, df, dt, tctl): - tctl = self.tctl if tctl is None else tctl - tc = self.tcc + self.tcfl * df + tctl * df * df - return f + self.amfg * tc * dt * f - - def compensate(self, freq, temp_source, temp_target, tctl=None): - dt = temp_target - temp_source - dfmin = self.fmin * self.amfg * self.tcc * \ - (temp_source - self.fmin_temp) - df = freq - (self.fmin + dfmin) - if dt < 0.: - f2 = self._tcf(freq, df, dt, tctl) - dfmin2 = self.fmin * self.amfg * self.tcc * \ - (temp_target - self.fmin_temp) - df2 = f2 - (self.fmin + dfmin2) - f3 = self._tcf(f2, df2, -dt, tctl) - ferror = freq - f3 - freq = freq + ferror - df = freq - (self.fmin + dfmin) - return self._tcf(freq, df, dt, tctl) + def compensate(self, freq, temp_source, temp_target): + if self.a_a == None or self.a_b == None or self.b_a == None or self.b_b == None: + return freq + A=4*(temp_source*self.a_a)**2+4*temp_source*self.a_a*self.b_a+self.b_a**2+4*self.a_a + B=8*temp_source**2*self.a_a*self.a_b+4*temp_source*(self.a_a*self.b_b+self.a_b*self.b_a)+2*self.b_a*self.b_b+4*self.a_b-4*(freq-model.fmin)*self.a_a + C=4*(temp_source*self.a_b)**2+4*temp_source*self.a_b*self.b_b+self.b_b**2-4*(freq-model.fmin)*self.a_b + if(B**2-4*A*C<0): + param_c=freq-param_linear(freq-model.fmin,self.a_a,self.a_b)*temp_source**2-param_linear(freq-model.fmin,self.b_a,self.b_b)*temp_source + return param_linear(freq-model.fmin,self.a_a,self.a_b)*temp_target**2+param_linear(freq-model.fmin,self.b_a,self.b_b)*temp_target+param_c + ax=(np.sqrt(B**2-4*A*C)-B)/2/A + param_a=param_linear(ax,self.a_a,self.a_b) + param_b=param_linear(ax,self.b_a,self.b_b) + return param_a*(temp_target+param_b/2/param_a)**2+ax+model.fmin + #print(-param_linear(ax,self.b_a,self.b_b)/2/param_linear(ax,self.a_a,self.a_b)) + #param_c=freq-param_linear(freq-model.fmin,self.a_a,self.a_b)*temp_source**2-param_linear(freq-model.fmin,self.b_a,self.b_b)*temp_source + #return param_linear(freq-model.fmin,self.a_a,self.a_b)*temp_target**2+param_linear(freq-model.fmin,self.b_a,self.b_b)*temp_target+param_c def line_fit(x,a,b,c): return a*x**2+b*x+c -def fit(data,tcc,tcfl,tctl): - result=[] - model.tcc=tcc - model.tcfl=tcfl - model.tctl-tctl - for j in range(len(datas)): - for i in range(len(data)): - result.append(model.compensate(datas[j][3000],35,data[i])) - return result +def line0(x,a,c): + return a*x**2+c +def line120(x,a,c): + return a*x**2-240*a*x+c def area_find(temp,freq): middle=int(len(temp)/100/2)*100 i=j=100 @@ -54,7 +42,7 @@ def area_find(temp,freq): if(i_flag): i=i+100 if middle-i>=0: - linear_params, params_covariance = curve_fit(line_fit, temp[middle-i:middle+j],freq[middle-i:middle+j],maxfev=100000,ftol=1e-10,xtol=1e-20) + linear_params, params_covariance = curve_fit(line_fit, temp[middle-i:middle+j],freq[middle-i:middle+j],maxfev=100000,ftol=1e-10,xtol=1e-10) minus=line_fit(temp[middle-i:middle+j],linear_params[0],linear_params[1],linear_params[2])-freq[middle-i:middle+j] if np.sum(np.square(minus))/len(minus)>threshold: i=i-100 @@ -62,73 +50,111 @@ def area_find(temp,freq): if(j_flag): j=j+100 if middle+j<=len(freq): - linear_params, params_covariance = curve_fit(line_fit, temp[middle-i:middle+j],freq[middle-i:middle+j],maxfev=100000,ftol=1e-10,xtol=1e-20) + linear_params, params_covariance = curve_fit(line_fit, temp[middle-i:middle+j],freq[middle-i:middle+j],maxfev=100000,ftol=1e-10,xtol=1e-10) minus=line_fit(temp[middle-i:middle+j],linear_params[0],linear_params[1],linear_params[2])-freq[middle-i:middle+j] if np.sum(np.square(minus))/len(minus)>threshold: j=j-100 j_flag=False - linear_params, params_covariance = curve_fit(line_fit, temp[middle-i:middle+j],freq[middle-i:middle+j],maxfev=100000,ftol=1e-10,xtol=1e-20) + linear_params, params_covariance = curve_fit(line_fit, temp[middle-i:middle+j],freq[middle-i:middle+j],maxfev=100000,ftol=1e-10,xtol=1e-10) return linear_params def data_process(path): - data=[] - file_path = path # 替换为你的文件路径 - with open(file_path, 'r') as file: + freq=[] + temp=[] + with open(path, 'r') as file: # 逐行读取文件内容 lines = file.readlines() # 遍历每行内容 for line in lines: - data.append(line.split(',')) - file.close() - full_data=pd.DataFrame(data[1:-1],columns=data[0]) - temp=np.array(full_data['temp']).astype(np.float32) - freq=np.array(full_data['freq']).astype(np.float32) - freq=freq[::100] - temp=temp[::100] - plt.plot(temp[10:],freq[10:]) - linear_params=area_find(temp,freq) - plt.plot(temp,line_fit(temp,linear_params[0],linear_params[1],linear_params[2])) - data0=line_fit(np.arange(5,80,0.01),linear_params[0],linear_params[1],linear_params[2]) - return data0 + data=line.split(',') + try: + freq.append(float(data[3])) + temp.append(float(data[5])) + except:pass + dv=int(len(temp)/1000) + if dv>1: + freq=np.array(freq[::dv]) + temp=np.array(temp[::dv]) + plt.plot(temp[20:],freq[20:]) + #linear_params=area_find(temp[20:],freq[20:]) + param_bounds=([0,-np.inf,-np.inf],[np.inf,np.inf,np.inf]) + linear_params, params_covariance = curve_fit(line_fit, temp[20:],freq[20:],bounds=param_bounds,maxfev=100000,ftol=1e-10,xtol=1e-10) + + try: + plt.title("Range:"+str(int(np.max(freq[20:])-np.min(freq[20:])))) + except: + pass + axis=-1*linear_params[1]/2/linear_params[0] + if(axis>120): + linear_params1, params_covariance = curve_fit(line120, temp[20:],freq[20:],bounds=([0,-np.inf],[np.inf,np.inf]),maxfev=100000,ftol=1e-10,xtol=1e-10) + plt.plot(temp[20:],line120(temp[20:],linear_params1[0],linear_params1[1])) + return [linear_params1[0],-240*linear_params1[0],line120(120,linear_params1[0],linear_params1[1])] + elif(axis<0): + linear_params1, params_covariance = curve_fit(line0, temp[20:],freq[20:],bounds=([0,-np.inf],[np.inf,np.inf]),maxfev=100000,ftol=1e-10,xtol=1e-10) + plt.plot(temp[20:],line0(temp[20:],linear_params1[0],linear_params1[1])) + return [linear_params1[0],0,line0(axis,linear_params1[0],linear_params1[1])] + plt.plot(temp[20:],line_fit(temp[20:],linear_params[0],linear_params[1],linear_params[2])) + linear_params[2]=line_fit(axis,linear_params[0],linear_params[1],linear_params[2]) + return linear_params +def param_linear(x,a,b): + return a*x+b + while(1): plt.figure(figsize=(25, 15)) paths=['./data1','./data2','./data3','./data4'] - datas=[] + a=[] + b=[] + freqs=[] num=241 - threshold=int(input('threshold set(recommend start from 250):\n请输入阈值设置(默认推荐250):\n')) + #threshold=int(input('threshold set(recommend start from 1000):\n请输入阈值设置(默认推荐1000):\n')) try: for path in paths: plt.subplot(num) num+=1 - datas.append(data_process(path)) + temp=data_process(path) + a.append(temp[0]) + b.append(temp[1]) + freqs.append(temp[2]) except: print("please make sure you have move the 4 data file to IDM folder\n请确认你有把4个文件拷到IDM文件夹内") break #反向求值 - model=TempModel(1,-2.1429828e-05,-1.8980091e-10,3.6738370e-16,2943053.84,20.33) - p0=[-2.1429828e-05,-1.8980091e-10,3.6738370e-16] - params, params_covariance = curve_fit(fit,np.arange(5,80,0.01),np.hstack(datas),p0=p0,maxfev=1000000,ftol=1e-10,xtol=1e-10) + model=TempModel(None,None,None,None,2943053.8415908813,23.33) + linear_params, params_covariance = curve_fit(param_linear, np.array(freqs)-model.fmin,a,maxfev=100000,ftol=1e-10,xtol=1e-10) + model.a_a=linear_params[0] + model.a_b=linear_params[1] + linear_params1, params_covariance = curve_fit(param_linear, np.array(freqs)-model.fmin,b,maxfev=100000,ftol=1e-10,xtol=1e-10) + model.b_a=linear_params1[0] + model.b_b=linear_params1[1] for path in paths: plt.subplot(num) num+=1 - data=[] - file_path = path # 替换为你的文件路径 - with open(file_path, 'r') as file: + freq=[] + temp=[] + with open(path, 'r') as file: # 逐行读取文件内容 lines = file.readlines() # 遍历每行内容 for line in lines: - data.append(line.split(',')) - file.close() - full_data=pd.DataFrame(data[1:-1],columns=data[0]) - temp=np.array(full_data['temp']).astype(np.float32) - freq=np.array(full_data['freq']).astype(np.float32) - freq=freq[::100] - temp=temp[::100] + data=line.split(',') + try: + freq.append(float(data[3])) + temp.append(float(data[5])) + except:pass + dv=int(len(temp)/10000) + if dv>1: + freq=np.array(freq[::dv]) + temp=np.array(temp[::dv]) + temp=temp[200:] + freq=freq[200:] result0=[] for i in range(len(temp)): - result0.append(model.compensate(freq[i],temp[i],20.66)) - plt.plot(temp[10:],result0[10:]) - plt.savefig('fit.png') + result0.append(model.compensate(freq[i],temp[i],50)) + plt.plot(temp,result0) + try: + plt.title("Range:"+str(int(np.max(result0)-np.min(result0)))) + except: + pass + plt.savefig('fit_output.png') print('fit result:') - print('tc_tcc:'+str(params[0])+'\ntc_tcfl:'+str(params[1])+'\ntc_tctl:'+str(params[2])) + print('tc_a_a:'+str(model.a_a)+'\ntc_a_b:'+str(model.a_b)+'\ntc_b_a:'+str(model.b_a)+'\ntc_b_b:'+str(model.b_b)) break \ No newline at end of file diff --git a/idm.py b/idm.py index b584de6..49efe90 100644 --- a/idm.py +++ b/idm.py @@ -33,25 +33,24 @@ class IDMProbe: self.reactor = self.printer.get_reactor() self.name = config.get_name() - self.speed = config.getfloat('speed', 5.0, above=0.) - self.lift_speed = config.getfloat('lift_speed', self.speed, above=0.) - self.backlash_comp = config.getfloat('backlash_comp', 0.5) + self.speed = config.getfloat("speed", 5.0, above=0.0) + self.lift_speed = config.getfloat("lift_speed", self.speed, above=0.0) + self.backlash_comp = config.getfloat("backlash_comp", 0.5) - self.x_offset = config.getfloat('x_offset', 0.) - self.y_offset = config.getfloat('y_offset', 0.) + self.x_offset = config.getfloat("x_offset", 0.0) + self.y_offset = config.getfloat("y_offset", 0.0) - self.trigger_distance = config.getfloat('trigger_distance', 2.) - self.trigger_dive_threshold = config.getfloat( - 'trigger_dive_threshold', 1.) - self.trigger_hysteresis = config.getfloat('trigger_hysteresis', 0.006) + self.trigger_distance = config.getfloat("trigger_distance", 2.0) + self.trigger_dive_threshold = config.getfloat("trigger_dive_threshold", 1.0) + self.trigger_hysteresis = config.getfloat("trigger_hysteresis", 0.006) self.z_settling_time = config.getint("z_settling_time", 5, minval=0) # If using paper for calibration, this would be .1mm - self.cal_nozzle_z = config.getfloat('cal_nozzle_z', 0.1) - self.cal_floor = config.getfloat('cal_floor', 0.2) - self.cal_ceil = config.getfloat('cal_ceil', 5.) - self.cal_speed = config.getfloat('cal_speed', 1.) - self.cal_move_speed = config.getfloat('cal_move_speed', 10.) + self.cal_nozzle_z = config.getfloat("cal_nozzle_z", 0.1) + self.cal_floor = config.getfloat("cal_floor", 0.2) + self.cal_ceil = config.getfloat("cal_ceil", 5.0) + self.cal_speed = config.getfloat("cal_speed", 1.0) + self.cal_move_speed = config.getfloat("cal_move_speed", 10.0) # Load models self.model = None @@ -59,13 +58,13 @@ class IDMProbe: self.model_temp_builder = IDMTempModelBuilder.load(config) self.model_temp = None self.fmin = None - self.default_model_name = config.get('default_model_name', 'default') + self.default_model_name = config.get("default_model_name", "default") self.model_manager = ModelManager(self) # Temperature sensor integration self.last_temp = 0 - self.measured_min = 99999999. - self.measured_max = 0. + self.measured_min = 99999999.0 + self.measured_max = 0.0 self.last_sample = None self.hardware_failure = None @@ -73,8 +72,7 @@ class IDMProbe: self.mesh_helper = IDMMeshHelper.create(self, config) self._stream_en = 0 - self._stream_timeout_timer = self.reactor.register_timer( - self._stream_timeout) + self._stream_timeout_timer = self.reactor.register_timer(self._stream_timeout) self._stream_callbacks = {} self._stream_latency_requests = {} self._stream_buffer = [] @@ -84,55 +82,55 @@ class IDMProbe: self._stream_flush_event = threading.Event() self._log_stream = None self._data_filter = AlphaBetaFilter( - config.getfloat('filter_alpha', 0.5), - config.getfloat('filter_beta', 0.000001), + config.getfloat("filter_alpha", 0.5), + config.getfloat("filter_beta", 0.000001), ) self.trapq = None - mainsync = self.printer.lookup_object('mcu')._clocksync + mainsync = self.printer.lookup_object("mcu")._clocksync self._mcu = MCU(config, SecondarySync(self.reactor, mainsync)) - self.printer.add_object('mcu ' + self.name, self._mcu) + self.printer.add_object("mcu " + self.name, self._mcu) self.cmd_queue = self._mcu.alloc_command_queue() self.mcu_probe = IDMEndstopWrapper(self) # Register z_virtual_endstop - self.printer.lookup_object('pins').register_chip('probe', self) + self.printer.lookup_object("pins").register_chip("probe", self) # Register event handlers - self.printer.register_event_handler('klippy:connect', + self.printer.register_event_handler("klippy:connect", self._handle_connect) - self.printer.register_event_handler('klippy:mcu_identify', + self.printer.register_event_handler("klippy:mcu_identify", self._handle_mcu_identify) self._mcu.register_config_callback(self._build_config) self._mcu.register_response(self._handle_idm_data, "idm_data") # Register webhooks - webhooks = self.printer.lookup_object('webhooks') + webhooks = self.printer.lookup_object("webhooks") self._api_dump_helper = APIDumpHelper(self) - webhooks.register_endpoint('idm/status', self._handle_req_status) - webhooks.register_endpoint('idm/dump', self._handle_req_dump) + webhooks.register_endpoint("idm/status", self._handle_req_status) + webhooks.register_endpoint("idm/dump", self._handle_req_dump) # Register gcode commands - self.gcode = self.printer.lookup_object('gcode') - self.gcode.register_command('IDM_STREAM', self.cmd_IDM_STREAM, + self.gcode = self.printer.lookup_object("gcode") + self.gcode.register_command("IDM_STREAM", self.cmd_IDM_STREAM, desc=self.cmd_IDM_STREAM_help) - self.gcode.register_command('IDM_QUERY', self.cmd_IDM_QUERY, + self.gcode.register_command("IDM_QUERY", self.cmd_IDM_QUERY, desc=self.cmd_IDM_QUERY_help) - self.gcode.register_command('IDM_CALIBRATE', + self.gcode.register_command("IDM_CALIBRATE", self.cmd_IDM_CALIBRATE, desc=self.cmd_IDM_CALIBRATE_help) - self.gcode.register_command('IDM_ESTIMATE_BACKLASH', + self.gcode.register_command("IDM_ESTIMATE_BACKLASH", self.cmd_IDM_ESTIMATE_BACKLASH, desc=self.cmd_IDM_ESTIMATE_BACKLASH_help) - self.gcode.register_command('PROBE', self.cmd_PROBE, + self.gcode.register_command("probe", self.cmd_PROBE, desc=self.cmd_PROBE_help) - self.gcode.register_command('PROBE_ACCURACY', self.cmd_PROBE_ACCURACY, + self.gcode.register_command("PROBE_ACCURACY", self.cmd_PROBE_ACCURACY, desc=self.cmd_PROBE_ACCURACY_help) - self.gcode.register_command('Z_OFFSET_APPLY_PROBE', + self.gcode.register_command("Z_OFFSET_APPLY_PROBE", self.cmd_Z_OFFSET_APPLY_PROBE, desc=self.cmd_Z_OFFSET_APPLY_PROBE_help) # Event handlers def _handle_connect(self): - self.phoming = self.printer.lookup_object('homing') + self.phoming = self.printer.lookup_object("homing") # Ensure streaming mode is stopped self.idm_stream_cmd.send([0]) @@ -146,13 +144,16 @@ class IDMProbe: def _handle_mcu_identify(self): constants = self._mcu.get_constants() - - self.sensor_freq = self._mcu._mcu_freq if self._mcu._mcu_freq < 20000000 else self._mcu._mcu_freq/2 - + if self._mcu._mcu_freq < 20000000: + self.sensor_freq = self._mcu._mcu_freq + elif self._mcu._mcu_freq < 100000000: + self.sensor_freq = self._mcu._mcu_freq/2 + else: + self.sensor_freq = self._mcu._mcu_freq/6 self.inv_adc_max = 1.0 / constants.get("ADC_MAX") - self.temp_smooth_count = constants.get('IDM_ADC_SMOOTH_COUNT') - self.thermistor = thermistor.Thermistor(10000., 0.) - self.thermistor.setup_coefficients_beta(25., 47000., 4041.) + self.temp_smooth_count = constants.get("IDM_ADC_SMOOTH_COUNT") + self.thermistor = thermistor.Thermistor(10000.0, 0.0) + self.thermistor.setup_coefficients_beta(25., 47000.0, 4041.0) self.toolhead = self.printer.lookup_object("toolhead") self.trapq = self.toolhead.get_trapq() @@ -173,14 +174,14 @@ class IDMProbe: cq=self.cmd_queue) def stats(self, eventtime): - return False, '%s: coil_temp=%.1f' % (self.name, self.last_temp) + return False, "%s: coil_temp=%.1f" % (self.name, self.last_temp) # Virtual endstop def setup_pin(self, pin_type, pin_params): - if pin_type != 'endstop' or pin_params['pin'] != 'z_virtual_endstop': + if pin_type != "endstop" or pin_params["pin"] != "z_virtual_endstop": raise pins.error("Probe virtual endstop only useful as endstop pin") - if pin_params['invert'] or pin_params['pullup']: + if pin_params["invert"] or pin_params["pullup"]: raise pins.error("Can not pullup/invert probe virtual endstop") return self.mcu_probe @@ -197,19 +198,19 @@ class IDMProbe: def get_lift_speed(self, gcmd=None): if gcmd is not None: - return gcmd.get_float("LIFT_SPEED", self.lift_speed, above=0.) + return gcmd.get_float("LIFT_SPEED", self.lift_speed, above=0.0) return self.lift_speed def run_probe(self, gcmd): if self.model is None: raise self.printer.command_error("No IDM model loaded") - speed = gcmd.get_float("PROBE_SPEED", self.speed, above=0.) - allow_faulty = gcmd.get_int('ALLOW_FAULTY_COORDINATE', 0) != 0 + speed = gcmd.get_float("PROBE_SPEED", self.speed, above=0.0) + allow_faulty = gcmd.get_int("ALLOW_FAULTY_COORDINATE", 0) != 0 lift_speed = self.get_lift_speed(gcmd) - toolhead = self.printer.lookup_object('toolhead') + toolhead = self.printer.lookup_object("toolhead") curtime = self.reactor.monotonic() - if 'z' not in toolhead.get_status(curtime)['homed_axes']: + if "z" not in toolhead.get_status(curtime)["homed_axes"]: raise self.printer.command_error("Must home before probe") self._start_streaming() @@ -231,7 +232,7 @@ class IDMProbe: curtime = self.reactor.monotonic() status = self.toolhead.get_kinematics().get_status(curtime) pos = self.toolhead.get_position() - pos[2] = status['axis_minimum'][2] + pos[2] = status["axis_minimum"][2] try: self.phoming.probing_move(self.mcu_probe, pos, speed) self._sample_printtime_sync(self.z_settling_time) @@ -246,7 +247,7 @@ class IDMProbe: tdt = self.trigger_dive_threshold (dist, samples) = self._sample(5, num_samples) - x, y = samples[0]['pos'][0:2] + x, y = samples[0]["pos"][0:2] if self._is_faulty_coordinate(x, y, True): msg = "Probing within a faulty area" if not allow_faulty: @@ -268,9 +269,15 @@ class IDMProbe: # correct height and take a new sample. self._move_to_probing_height(speed) (dist, samples) = self._sample(self.z_settling_time, num_samples) - - pos = samples[0]['pos'] - + # get z compensation from axis_twist_compensation + pos = samples[0]["pos"] + axis_twist_compensation = self.printer.lookup_object( + 'axis_twist_compensation', None) + z_compensation = 0 + if axis_twist_compensation is not None: + z_compensation = ( + axis_twist_compensation.get_z_compensation_value(pos)) + dist+=z_compensation self.gcode.respond_info("probe at %.3f,%.3f,%.3f is z=%.6f" % (pos[0], pos[1], pos[2], dist)) @@ -280,7 +287,7 @@ class IDMProbe: def _start_calibration(self, gcmd): - allow_faulty = gcmd.get_int('ALLOW_FAULTY_COORDINATE', 0) != 0 + allow_faulty = gcmd.get_int("ALLOW_FAULTY_COORDINATE", 0) != 0 if gcmd.get("SKIP_MANUAL_PROBE", None) is not None: kin = self.toolhead.get_kinematics() kin_spos = {s.get_name(): s.get_commanded_position() @@ -296,7 +303,7 @@ class IDMProbe: else: curtime = self.printer.get_reactor().monotonic() kin_status = self.toolhead.get_status(curtime) - if 'xy' not in kin_status['homed_axes']: + if "xy" not in kin_status["homed_axes"]: raise self.printer.command_error("Must home X and Y " "before calibration") @@ -309,10 +316,10 @@ class IDMProbe: gcmd.respond_raw("!! " + msg + "\n") forced_z = False - if 'z' not in kin_status['homed_axes']: + if "z" not in kin_status["homed_axes"]: self.toolhead.get_last_move_time() pos = self.toolhead.get_position() - pos[2] = kin_status['axis_maximum'][2] - 1.0 + pos[2] = kin_status["axis_maximum"][2] - 1.0 self.toolhead.set_position(pos, homing_axes=[2]) forced_z = True @@ -327,13 +334,13 @@ class IDMProbe: return gcmd.respond_info("IDM calibration starting") - cal_nozzle_z = gcmd.get_float('NOZZLE_Z', self.cal_nozzle_z) - cal_floor = gcmd.get_float('FLOOR', self.cal_floor) - cal_ceil = gcmd.get_float('CEIL', self.cal_ceil) + cal_nozzle_z = gcmd.get_float("NOZZLE_Z", self.cal_nozzle_z) + cal_floor = gcmd.get_float("FLOOR", self.cal_floor) + cal_ceil = gcmd.get_float("CEIL", self.cal_ceil) cal_min_z = kin_pos[2] - cal_nozzle_z + cal_floor cal_max_z = kin_pos[2] - cal_nozzle_z + cal_ceil - cal_speed = gcmd.get_float('SPEED', self.cal_speed) - move_speed = gcmd.get_float('MOVE_SPEED', self.cal_move_speed) + cal_speed = gcmd.get_float("SPEED", self.cal_speed) + move_speed = gcmd.get_float("MOVE_SPEED", self.cal_move_speed) toolhead = self.toolhead curtime = self.reactor.monotonic() @@ -370,10 +377,10 @@ class IDMProbe: self._stop_streaming() # Fit the sampled data - z_offset = [s['pos'][2]-cal_min_z+cal_floor + z_offset = [s["pos"][2]-cal_min_z+cal_floor for s in samples] - freq = [s['freq'] for s in samples] - temp = [s['temp'] for s in samples] + freq = [s["freq"] for s in samples] + temp = [s["temp"] for s in samples] inv_freq = [1/f for f in freq] poly = Polynomial.fit(inv_freq, z_offset, 9) temp_median = median(temp) @@ -430,9 +437,9 @@ class IDMProbe: def _check_hardware(self, sample): if not self.hardware_failure: msg = None - if sample['data'] == 0xFFFFFFF: + if sample["data"] == 0xFFFFFFF: msg = "coil is shorted or not connected" - elif self.fmin is not None and sample['freq'] > 1.35 * self.fmin: + elif self.fmin is not None and sample["freq"] > 1.35 * self.fmin: msg = "coil expected max frequency exceeded" if msg: msg = "IDM hardware issue: " + msg @@ -446,26 +453,26 @@ class IDMProbe: self.printer.invoke_shutdown(self.hardware_failure) def _enrich_sample_time(self, sample): - clock = sample['clock'] = self._mcu.clock32_to_clock64(sample['clock']) - sample['time'] = self._mcu.clock_to_print_time(clock) + clock = sample["clock"] = self._mcu.clock32_to_clock64(sample["clock"]) + sample["time"] = self._mcu.clock_to_print_time(clock) def _enrich_sample_temp(self, sample): - temp_adc = sample['temp'] / self.temp_smooth_count * self.inv_adc_max - sample['temp'] = self.thermistor.calc_temp(temp_adc) + temp_adc = sample["temp"] / self.temp_smooth_count * self.inv_adc_max + sample["temp"] = self.thermistor.calc_temp(temp_adc) def _enrich_sample_freq(self, sample): - sample['data_smooth'] = self._data_filter.value() - sample['freq'] = self.count_to_freq(sample['data_smooth']) + sample["data_smooth"] = self._data_filter.value() + sample["freq"] = self.count_to_freq(sample["data_smooth"]) self._check_hardware(sample) def _enrich_sample(self, sample): - sample['dist'] = self.freq_to_dist(sample['freq'], sample['temp']) - pos, vel = self._get_trapq_position(sample['time']) + sample["dist"] = self.freq_to_dist(sample["freq"], sample["temp"]) + pos, vel = self._get_trapq_position(sample["time"]) if pos is None: return - sample['pos'] = pos - sample['vel'] = vel + sample["pos"] = pos + sample["vel"] = vel def _start_streaming(self): if self._stream_en == 0: @@ -534,7 +541,7 @@ class IDMProbe: updated_timer = True self._enrich_sample_temp(sample) - temp = sample['temp'] + temp = sample["temp"] if self.model_temp is not None and not (-40 < temp < 180): msg = ("IDM temperature sensor faulty(read %.2f C)," " disabling temperaure compensation" % (temp,)) @@ -548,7 +555,7 @@ class IDMProbe: self.measured_max = max(self.measured_max, temp) self._enrich_sample_time(sample) - self._data_filter.update(sample['time'], sample['data']) + self._data_filter.update(sample["time"], sample["data"]) self._enrich_sample_freq(sample) if len(self._stream_callbacks) > 0: @@ -581,12 +588,12 @@ class IDMProbe: def _get_trapq_position(self, print_time): ffi_main, ffi_lib = chelper.get_ffi() - data = ffi_main.new('struct pull_move[1]') - count = ffi_lib.trapq_extract_old(self.trapq, data, 1, 0., print_time) + data = ffi_main.new("struct pull_move[1]") + count = ffi_lib.trapq_extract_old(self.trapq, data, 1, 0.0, print_time) if not count: return None, None move = data[0] - move_time = max(0., min(move.move_t, print_time - move.print_time)) + move_time = max(0.0, min(move.move_t, print_time - move.print_time)) dist = (move.start_v + .5 * move.accel * move_time) * move_time pos = (move.start_x + move.x_r * dist, move.start_y + move.y_r * dist, move.start_z + move.z_r * dist) @@ -594,14 +601,14 @@ class IDMProbe: return pos, velocity def _sample_printtime_sync(self, skip=0, count=1): - toolhead = self.printer.lookup_object('toolhead') + toolhead = self.printer.lookup_object("toolhead") move_time = toolhead.get_last_move_time() settle_clock = self._mcu.print_time_to_clock(move_time) samples = [] total = skip + count def cb(sample): - if sample['clock'] >= settle_clock: + if sample["clock"] >= settle_clock: samples.append(sample) if len(samples) >= total: raise StopStreaming @@ -618,7 +625,7 @@ class IDMProbe: def _sample(self, skip, count): samples = self._sample_printtime_sync(skip, count) - return (median([s['dist'] for s in samples]), samples) + return (median([s["dist"] for s in samples]), samples) def _sample_async(self, count=1): samples = [] @@ -656,8 +663,8 @@ class IDMProbe: if self.model is not None: model = self.model.name return { - 'last_sample': self.last_sample, - 'model': model, + "last_sample": self.last_sample, + "model": model, } # Webhook handlers @@ -666,12 +673,12 @@ class IDMProbe: temp = None sample = self._sample_async() out = { - 'freq': sample['freq'], - 'dist': sample['dist'], + "freq": sample["freq"], + "dist": sample["dist"], } - temp = sample['temp'] + temp = sample["temp"] if temp is not None: - out['temp'] = temp + out["temp"] = temp web_request.send(out) def _handle_req_dump(self, web_request): @@ -691,16 +698,16 @@ class IDMProbe: cmd_IDM_ESTIMATE_BACKLASH_help = "Estimate Z axis backlash" def cmd_IDM_ESTIMATE_BACKLASH(self, gcmd): # Get to correct Z height - overrun = gcmd.get_float('OVERRUN', 1.) - speed = gcmd.get_float("PROBE_SPEED", self.speed, above=0.) + overrun = gcmd.get_float("OVERRUN", 1.0) + speed = gcmd.get_float("PROBE_SPEED", self.speed, above=0.0) cur_z = self.toolhead.get_position()[2] self.toolhead.manual_move([None, None, cur_z+overrun], speed) self.run_probe(gcmd) lift_speed = self.get_lift_speed(gcmd) - target = gcmd.get_float('Z', self.trigger_distance) + target = gcmd.get_float("Z", self.trigger_distance) - num_samples = gcmd.get_int('SAMPLES', 20) + num_samples = gcmd.get_int("SAMPLES", 20) wait = self.z_settling_time samples_up = [] @@ -744,14 +751,14 @@ class IDMProbe: cmd_IDM_QUERY_help = "Take a sample from the sensor" def cmd_IDM_QUERY(self, gcmd): sample = self._sample_async() - last_value = sample['freq'] - dist = sample['dist'] - temp = sample['temp'] + last_value = sample["freq"] + dist = sample["dist"] + temp = sample["temp"] self.last_sample = { - 'time': sample['time'], - 'value': last_value, - 'temp': temp, - 'dist': dist, + "time": sample["time"], + "value": last_value, + "temp": temp, + "dist": dist, } if dist is None: gcmd.respond_info("Last reading: %.2fHz, %.2fC, no model" % @@ -777,18 +784,18 @@ class IDMProbe: f.write("time,data,data_smooth,freq,dist,temp,pos_x,pos_y,pos_z,vel\n") def cb(sample): - pos = sample.get('pos', None) + pos = sample.get("pos", None) obj = "%.4f,%d,%.2f,%.5f,%.5f,%.2f,%s,%s,%s,%s\n" % ( - sample['time'], - sample['data'], - sample['data_smooth'], - sample['freq'], - sample['dist'], - sample['temp'], + sample["time"], + sample["data"], + sample["data_smooth"], + sample["freq"], + sample["dist"], + sample["temp"], "%.3f" % (pos[0],) if pos is not None else "", "%.3f" % (pos[1],) if pos is not None else "", "%.3f" % (pos[2],) if pos is not None else "", - "%.3f" % (sample['vel'],) if 'vel' in sample else "" + "%.3f" % (sample["vel"],) if "vel" in sample else "" ) f.write(obj) @@ -797,11 +804,11 @@ class IDMProbe: cmd_PROBE_ACCURACY_help = "Probe Z-height accuracy at current XY position" def cmd_PROBE_ACCURACY(self, gcmd): - speed = gcmd.get_float("PROBE_SPEED", self.speed, above=0.) + speed = gcmd.get_float("PROBE_SPEED", self.speed, above=0.0) lift_speed = self.get_lift_speed(gcmd) sample_count = gcmd.get_int("SAMPLES", 10, minval=1) sample_retract_dist = gcmd.get_float("SAMPLE_RETRACT_DIST", 0) - allow_faulty = gcmd.get_int('ALLOW_FAULTY_COORDINATE', 0) != 0 + allow_faulty = gcmd.get_int("ALLOW_FAULTY_COORDINATE", 0) != 0 pos = self.toolhead.get_position() gcmd.respond_info("PROBE_ACCURACY at X:%.3f Y:%.3f Z:%.3f" " (samples=%d retract=%.3f" @@ -842,7 +849,7 @@ class IDMProbe: cmd_Z_OFFSET_APPLY_PROBE_help = "Adjust the probe's z_offset" def cmd_Z_OFFSET_APPLY_PROBE(self, gcmd): gcode_move = self.printer.lookup_object("gcode_move") - offset = gcode_move.get_status()['homing_origin'].z + offset = gcode_move.get_status()["homing_origin"].z if offset == 0: self.gcode.respond_info("Nothing to do: Z Offset is 0") @@ -869,11 +876,11 @@ class IDMProbe: class IDMModel: @classmethod def load(cls, name, config, idm): - coef = config.getfloatlist('model_coef') - temp = config.getfloat('model_temp') - domain = config.getfloatlist('model_domain', count=2) - [min_z, max_z] = config.getfloatlist('model_range', count=2) - offset = config.getfloat('model_offset', 0.) + coef = config.getfloatlist("model_coef") + temp = config.getfloat("model_temp") + domain = config.getfloatlist("model_domain", count=2) + [min_z, max_z] = config.getfloatlist("model_range", count=2) + offset = config.getfloat("model_offset", 0.0) poly = Polynomial(coef, domain) return IDMModel(name, idm, poly, temp, min_z, max_z, offset) @@ -887,17 +894,17 @@ class IDMModel: self.offset = offset def save(self, idm, show_message=True): - configfile = idm.printer.lookup_object('configfile') + configfile = idm.printer.lookup_object("configfile") section = "idm model " + self.name - configfile.set(section, 'model_coef', + configfile.set(section, "model_coef", ",\n ".join(map(str, self.poly.coef))) - configfile.set(section, 'model_domain', + configfile.set(section, "model_domain", ",".join(map(str, self.poly.domain))) - configfile.set(section, 'model_range', + configfile.set(section, "model_range", "%f,%f" % (self.min_z, self.max_z)) - configfile.set(section, 'model_temp', + configfile.set(section, "model_temp", "%f" % (self.temp)) - configfile.set(section, 'model_offset', "%.5f" % (self.offset,)) + configfile.set(section, "model_offset", "%.5f" % (self.offset,)) if show_message: idm.gcode.respond_info("IDM calibration for model '%s' has " "been updated\nfor the current session. The SAVE_CONFIG " @@ -908,9 +915,9 @@ class IDMModel: [begin, end] = self.poly.domain invfreq = 1/freq if invfreq > end: - return float('inf') + return float("inf") elif invfreq < begin: - return float('-inf') + return float("-inf") else: return float(self.poly(invfreq) - self.offset) @@ -932,7 +939,7 @@ class IDMModel: f = (end + begin) / 2 v = self.poly(f) if abs(v-dist) < max_e: - return float(1./f) + return float(1.0 /f) elif v < dist: begin = f else: @@ -949,12 +956,12 @@ class IDMModel: return freq class IDMTempModelBuilder: - _DEFAULTS = {'amfg': 1.0, - 'tcc': -1.56165495e-05, - 'tcfl': -1.11115902e-12, - 'tctl': 3.6738370e-16, - 'fmin' : None, - 'fmin_temp' : None} + _DEFAULTS = {"a_a": None, + "a_b": None, + "b_a": None, + "b_b": None, + "fmin" : None, + "fmin_temp" : None} @classmethod def load(cls, config): @@ -963,80 +970,83 @@ class IDMTempModelBuilder: def __init__(self, config): self.parameters = IDMTempModelBuilder._DEFAULTS.copy() for key in self.parameters.keys(): - param = config.getfloat('tc_' + key, None) + param = config.getfloat("tc_" + key, None) if param is not None: self.parameters[key] = param def build(self): - if self.parameters['fmin'] is None or \ - self.parameters['fmin_temp'] is None: + if self.parameters["fmin"] is None or \ + self.parameters["fmin_temp"] is None: return None - logging.info('idm: built tempco model %s', self.parameters) + logging.info("idm: built tempco model %s", self.parameters) return IDMTempModel(**self.parameters) def build_with_base(self, idm): base_data = idm.idm_base_read_cmd.send([6, 0]) - (f_count, adc_count) = struct.unpack(" cs: return + # If we are looking for a zero reference, check if we + # are close enough and if so, add to the bin. + if zcs > 0: + dx = x - self.zero_ref_mode[1][0] + dy = y - self.zero_ref_mode[1][1] + dist = math.sqrt(dx*dx+dy*dy) + if dist <= zcs: + self.zero_ref_bin.append(d) + k = (xi, yi) if k not in clusters: @@ -1729,25 +1795,29 @@ class IDMMeshHelper: line.append(median(data)) matrix.append(line) - rri = gcmd.get_int('RELATIVE_REFERENCE_INDEX', self.rri) - if rri is not None: + z_offset = None + if self.zero_ref_mode and self.zero_ref_mode[0] == "rri": + rri = self.zero_ref_mode[1] if rri < 0 or rri >= self.res_x * self.res_y: rri = None + if rri is not None: + rri_x = rri % self.res_x + rri_y = int(math.floor(rri / self.res_x)) + z_offset = matrix[rri_y][rri_x] + elif self.zero_ref_mode and self.zero_ref_mode[0] == "pos": + z_offset = td - self.zero_ref_val - if rri is not None: - rri_x = rri % self.res_x - rri_y = int(math.floor(rri / self.res_x)) - z_offset = matrix[rri_y][rri_x] + if z_offset is not None: for i, line in enumerate(matrix): matrix[i] = [z-z_offset for z in line] params = self.bm.bmc.mesh_config - params['min_x'] = self.min_x - params['max_x'] = self.max_x - params['min_y'] = self.min_y - params['max_y'] = self.max_y - params['x_count'] = self.res_x - params['y_count'] = self.res_y + params["min_x"] = self.min_x + params["max_x"] = self.max_x + params["min_y"] = self.min_y + params["max_y"] = self.max_y + params["x_count"] = self.res_x + params["y_count"] = self.res_y mesh = bed_mesh.ZMesh(params) try: mesh.build_mesh(matrix) @@ -1755,7 +1825,7 @@ class IDMMeshHelper: raise self.gcode.error(str(e)) self.bm.set_mesh(mesh) self.gcode.respond_info("Mesh calibration complete") - self.bm.save_profile(gcmd.get('PROFILE', "default")) + self.bm.save_profile(gcmd.get("PROFILE", "default")) class Region: def __init__(self, x_min, x_max, y_min, y_max): @@ -1808,17 +1878,17 @@ def median(samples): def load_config(config): idm = IDMProbe(config) - config.get_printer().add_object('probe', IDMProbeWrapper(idm)) + config.get_printer().add_object("probe", IDMProbeWrapper(idm)) temp = IDMTempWrapper(idm) - config.get_printer().add_object('temperature_sensor IDM_coil', temp) - pheaters = idm.printer.load_object(config, 'heaters') - pheaters.available_sensors.append('temperature_sensor IDM_coil') + config.get_printer().add_object("temperature_sensor IDM_coil", temp) + pheaters = idm.printer.load_object(config, "heaters") + pheaters.available_sensors.append("temperature_sensor IDM_coil") return idm def load_config_prefix(config): - idm = config.get_printer().lookup_object('idm') + idm = config.get_printer().lookup_object("idm") name = config.get_name() - if name.startswith('idm model '): + if name.startswith("idm model "): name = name[10:] model = IDMModel.load(name, config, idm) idm._register_model(name, model) diff --git a/install.sh b/install.sh index 6c3fca5..89af6f9 100644 --- a/install.sh +++ b/install.sh @@ -12,12 +12,13 @@ fi # install idm requirements to env echo "idm: installing python requirements to env, this may take 10+ minutes." -sudo apt-get install g++ gfortran +sudo apt-get install g++ +sudo apt-get install gfortran +sudo apt-get install libopenblas-dev "${KENV}/bin/pip" install -r "${BKDIR}/requirements.txt" # update link to idm.py echo "idm: linking klippy to idm.py." -sudo apt-get install g++ gfortran libopenblas-dev if [ -e "${KDIR}/klippy/extras/idm.py" ]; then rm "${KDIR}/klippy/extras/idm.py" fi diff --git a/requirements.txt b/requirements.txt index 493a1c6..66690cf 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,5 +1,4 @@ # klipper python virtual environment requirements for IDM numpy>=1.16.6 scipy>=1.10.0 -matplotlib>=3.7.0 -pandas>=1.4.2 \ No newline at end of file +matplotlib>=3.7.0 \ No newline at end of file