| 
									
										
										
										
											2018-05-21 14:48:01 -04:00
										 |  |  | # Mechanical bed tilt calibration with multiple Z steppers | 
					
						
							|  |  |  | # | 
					
						
							| 
									
										
										
										
											2019-05-27 09:36:02 -04:00
										 |  |  | # Copyright (C) 2018-2019  Kevin O'Connor <kevin@koconnor.net> | 
					
						
							| 
									
										
										
										
											2018-05-21 14:48:01 -04:00
										 |  |  | # | 
					
						
							|  |  |  | # This file may be distributed under the terms of the GNU GPLv3 license. | 
					
						
							|  |  |  | import logging | 
					
						
							| 
									
										
										
										
											2020-06-12 09:55:57 -04:00
										 |  |  | import mathutil | 
					
						
							|  |  |  | from . import probe | 
					
						
							| 
									
										
										
										
											2018-05-21 14:48:01 -04:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2019-05-27 09:36:02 -04:00
										 |  |  | class ZAdjustHelper: | 
					
						
							|  |  |  |     def __init__(self, config, z_count): | 
					
						
							| 
									
										
										
										
											2018-05-21 14:48:01 -04:00
										 |  |  |         self.printer = config.get_printer() | 
					
						
							| 
									
										
										
										
											2019-05-27 09:36:02 -04:00
										 |  |  |         self.name = config.get_name() | 
					
						
							|  |  |  |         self.z_count = z_count | 
					
						
							|  |  |  |         self.z_steppers = [] | 
					
						
							| 
									
										
										
										
											2019-01-08 11:09:55 -05:00
										 |  |  |         self.printer.register_event_handler("klippy:connect", | 
					
						
							|  |  |  |                                             self.handle_connect) | 
					
						
							| 
									
										
										
										
											2019-05-27 09:36:02 -04:00
										 |  |  |     def handle_connect(self): | 
					
						
							|  |  |  |         kin = self.printer.lookup_object('toolhead').get_kinematics() | 
					
						
							| 
									
										
										
										
											2020-01-13 21:39:55 -05:00
										 |  |  |         z_steppers = [s for s in kin.get_steppers() if s.is_active_axis('z')] | 
					
						
							| 
									
										
										
										
											2019-05-27 09:36:02 -04:00
										 |  |  |         if len(z_steppers) != self.z_count: | 
					
						
							|  |  |  |             raise self.printer.config_error( | 
					
						
							|  |  |  |                 "%s z_positions needs exactly %d items" % ( | 
					
						
							|  |  |  |                     self.name, len(z_steppers))) | 
					
						
							|  |  |  |         if len(z_steppers) < 2: | 
					
						
							| 
									
										
										
										
											2019-09-24 18:42:37 +03:00
										 |  |  |             raise self.printer.config_error( | 
					
						
							|  |  |  |                 "%s requires multiple z steppers" % (self.name,)) | 
					
						
							| 
									
										
										
										
											2019-05-27 09:36:02 -04:00
										 |  |  |         self.z_steppers = z_steppers | 
					
						
							|  |  |  |     def adjust_steppers(self, adjustments, speed): | 
					
						
							|  |  |  |         toolhead = self.printer.lookup_object('toolhead') | 
					
						
							|  |  |  |         gcode = self.printer.lookup_object('gcode') | 
					
						
							|  |  |  |         curpos = toolhead.get_position() | 
					
						
							|  |  |  |         # Report on movements | 
					
						
							|  |  |  |         stepstrs = ["%s = %.6f" % (s.get_name(), a) | 
					
						
							|  |  |  |                     for s, a in zip(self.z_steppers, adjustments)] | 
					
						
							|  |  |  |         msg = "Making the following Z adjustments:\n%s" % ("\n".join(stepstrs),) | 
					
						
							|  |  |  |         gcode.respond_info(msg) | 
					
						
							|  |  |  |         # Disable Z stepper movements | 
					
						
							| 
									
										
										
										
											2019-11-13 23:34:21 -05:00
										 |  |  |         toolhead.flush_step_generation() | 
					
						
							| 
									
										
										
										
											2019-05-27 09:36:02 -04:00
										 |  |  |         for s in self.z_steppers: | 
					
						
							| 
									
										
										
										
											2019-10-29 12:06:46 -04:00
										 |  |  |             s.set_trapq(None) | 
					
						
							| 
									
										
										
										
											2019-05-27 09:36:02 -04:00
										 |  |  |         # Move each z stepper (sorted from lowest to highest) until they match | 
					
						
							|  |  |  |         positions = [(-a, s) for a, s in zip(adjustments, self.z_steppers)] | 
					
						
							|  |  |  |         positions.sort() | 
					
						
							|  |  |  |         first_stepper_offset, first_stepper = positions[0] | 
					
						
							|  |  |  |         z_low = curpos[2] - first_stepper_offset | 
					
						
							|  |  |  |         for i in range(len(positions)-1): | 
					
						
							|  |  |  |             stepper_offset, stepper = positions[i] | 
					
						
							|  |  |  |             next_stepper_offset, next_stepper = positions[i+1] | 
					
						
							| 
									
										
										
										
											2019-11-13 23:34:21 -05:00
										 |  |  |             toolhead.flush_step_generation() | 
					
						
							| 
									
										
										
										
											2019-10-29 12:06:46 -04:00
										 |  |  |             stepper.set_trapq(toolhead.get_trapq()) | 
					
						
							| 
									
										
										
										
											2019-05-27 09:36:02 -04:00
										 |  |  |             curpos[2] = z_low + next_stepper_offset | 
					
						
							|  |  |  |             try: | 
					
						
							|  |  |  |                 toolhead.move(curpos, speed) | 
					
						
							|  |  |  |                 toolhead.set_position(curpos) | 
					
						
							|  |  |  |             except: | 
					
						
							|  |  |  |                 logging.exception("ZAdjustHelper adjust_steppers") | 
					
						
							| 
									
										
										
										
											2019-11-13 23:34:21 -05:00
										 |  |  |                 toolhead.flush_step_generation() | 
					
						
							| 
									
										
										
										
											2019-05-27 09:36:02 -04:00
										 |  |  |                 for s in self.z_steppers: | 
					
						
							| 
									
										
										
										
											2019-10-29 12:06:46 -04:00
										 |  |  |                     s.set_trapq(toolhead.get_trapq()) | 
					
						
							| 
									
										
										
										
											2019-05-27 09:36:02 -04:00
										 |  |  |                 raise | 
					
						
							|  |  |  |         # Z should now be level - do final cleanup | 
					
						
							|  |  |  |         last_stepper_offset, last_stepper = positions[-1] | 
					
						
							| 
									
										
										
										
											2019-11-13 23:34:21 -05:00
										 |  |  |         toolhead.flush_step_generation() | 
					
						
							| 
									
										
										
										
											2019-10-29 12:06:46 -04:00
										 |  |  |         last_stepper.set_trapq(toolhead.get_trapq()) | 
					
						
							| 
									
										
										
										
											2019-05-27 09:36:02 -04:00
										 |  |  |         curpos[2] += first_stepper_offset | 
					
						
							|  |  |  |         toolhead.set_position(curpos) | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2019-06-17 19:39:57 -07:00
										 |  |  | class RetryHelper: | 
					
						
							|  |  |  |     def __init__(self, config, error_msg_extra = ""): | 
					
						
							|  |  |  |         self.gcode = config.get_printer().lookup_object('gcode') | 
					
						
							|  |  |  |         self.default_max_retries = config.getint("retries", 0, minval=0) | 
					
						
							|  |  |  |         self.default_retry_tolerance = \ | 
					
						
							|  |  |  |             config.getfloat("retry_tolerance", 0., above=0.) | 
					
						
							|  |  |  |         self.value_label = "Probed points range" | 
					
						
							|  |  |  |         self.error_msg_extra = error_msg_extra | 
					
						
							| 
									
										
										
										
											2020-04-24 21:52:46 -04:00
										 |  |  |     def start(self, gcmd): | 
					
						
							|  |  |  |         self.max_retries = gcmd.get_int('RETRIES', self.default_max_retries, | 
					
						
							|  |  |  |                                         minval=0, maxval=30) | 
					
						
							|  |  |  |         self.retry_tolerance = gcmd.get_float('RETRY_TOLERANCE', | 
					
						
							|  |  |  |                                               self.default_retry_tolerance, | 
					
						
							|  |  |  |                                               minval=0.0, maxval=1.0) | 
					
						
							| 
									
										
										
										
											2019-06-17 19:39:57 -07:00
										 |  |  |         self.current_retry = 0 | 
					
						
							|  |  |  |         self.previous = None | 
					
						
							|  |  |  |         self.increasing = 0 | 
					
						
							| 
									
										
										
										
											2020-04-24 21:52:46 -04:00
										 |  |  |     def check_increase(self, error): | 
					
						
							| 
									
										
										
										
											2019-06-17 19:39:57 -07:00
										 |  |  |         if self.previous and error > self.previous + 0.0000001: | 
					
						
							|  |  |  |             self.increasing += 1 | 
					
						
							|  |  |  |         elif self.increasing > 0: | 
					
						
							|  |  |  |             self.increasing -= 1 | 
					
						
							|  |  |  |         self.previous = error | 
					
						
							|  |  |  |         return self.increasing > 1 | 
					
						
							| 
									
										
										
										
											2020-04-24 21:52:46 -04:00
										 |  |  |     def check_retry(self, z_positions): | 
					
						
							| 
									
										
										
										
											2019-06-17 19:39:57 -07:00
										 |  |  |         if self.max_retries == 0: | 
					
						
							|  |  |  |             return | 
					
						
							| 
									
										
										
										
											2020-05-18 04:38:22 +00:00
										 |  |  |         error = round(max(z_positions) - min(z_positions),6) | 
					
						
							| 
									
										
										
										
											2019-06-17 19:39:57 -07:00
										 |  |  |         self.gcode.respond_info( | 
					
						
							|  |  |  |             "Retries: %d/%d %s: %0.6f tolerance: %0.6f" % ( | 
					
						
							|  |  |  |                 self.current_retry, self.max_retries, self.value_label, | 
					
						
							|  |  |  |                 error, self.retry_tolerance)) | 
					
						
							| 
									
										
										
										
											2020-05-18 04:36:16 +00:00
										 |  |  |         if self.check_increase(error): | 
					
						
							|  |  |  |             raise self.gcode.error("Retries aborting: %s is increasing. %s" | 
					
						
							|  |  |  |                                    % (self.value_label, self.error_msg_extra)) | 
					
						
							| 
									
										
										
										
											2019-06-17 19:39:57 -07:00
										 |  |  |         if error <= self.retry_tolerance: | 
					
						
							|  |  |  |             return "done" | 
					
						
							|  |  |  |         self.current_retry += 1 | 
					
						
							|  |  |  |         if self.current_retry > self.max_retries: | 
					
						
							| 
									
										
										
										
											2020-04-23 16:32:52 -04:00
										 |  |  |             raise self.gcode.error("Too many retries") | 
					
						
							| 
									
										
										
										
											2019-06-17 19:39:57 -07:00
										 |  |  |         return "retry" | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2019-05-27 09:36:02 -04:00
										 |  |  | class ZTilt: | 
					
						
							|  |  |  |     def __init__(self, config): | 
					
						
							|  |  |  |         self.printer = config.get_printer() | 
					
						
							| 
									
										
										
										
											2018-05-21 14:48:01 -04:00
										 |  |  |         z_positions = config.get('z_positions').split('\n') | 
					
						
							|  |  |  |         try: | 
					
						
							|  |  |  |             z_positions = [line.split(',', 1) | 
					
						
							|  |  |  |                            for line in z_positions if line.strip()] | 
					
						
							|  |  |  |             self.z_positions = [(float(zp[0].strip()), float(zp[1].strip())) | 
					
						
							|  |  |  |                                 for zp in z_positions] | 
					
						
							|  |  |  |         except: | 
					
						
							|  |  |  |             raise config.error("Unable to parse z_positions in %s" % ( | 
					
						
							|  |  |  |                 config.get_name())) | 
					
						
							| 
									
										
										
										
											2019-06-17 19:44:09 -07:00
										 |  |  |         self.retry_helper = RetryHelper(config) | 
					
						
							| 
									
										
										
										
											2018-09-26 10:32:57 -04:00
										 |  |  |         self.probe_helper = probe.ProbePointsHelper(config, self.probe_finalize) | 
					
						
							| 
									
										
										
										
											2019-05-17 20:50:13 -07:00
										 |  |  |         self.probe_helper.minimum_points(2) | 
					
						
							| 
									
										
										
										
											2019-05-27 09:36:02 -04:00
										 |  |  |         self.z_helper = ZAdjustHelper(config, len(self.z_positions)) | 
					
						
							| 
									
										
										
										
											2018-05-21 14:48:01 -04:00
										 |  |  |         # Register Z_TILT_ADJUST command | 
					
						
							| 
									
										
										
										
											2020-04-24 21:52:46 -04:00
										 |  |  |         gcode = self.printer.lookup_object('gcode') | 
					
						
							|  |  |  |         gcode.register_command('Z_TILT_ADJUST', self.cmd_Z_TILT_ADJUST, | 
					
						
							|  |  |  |                                desc=self.cmd_Z_TILT_ADJUST_help) | 
					
						
							| 
									
										
										
										
											2018-05-21 14:48:01 -04:00
										 |  |  |     cmd_Z_TILT_ADJUST_help = "Adjust the Z tilt" | 
					
						
							| 
									
										
										
										
											2020-04-24 21:52:46 -04:00
										 |  |  |     def cmd_Z_TILT_ADJUST(self, gcmd): | 
					
						
							|  |  |  |         self.retry_helper.start(gcmd) | 
					
						
							|  |  |  |         self.probe_helper.start_probe(gcmd) | 
					
						
							| 
									
										
										
										
											2018-09-26 10:32:57 -04:00
										 |  |  |     def probe_finalize(self, offsets, positions): | 
					
						
							| 
									
										
										
										
											2018-09-26 19:08:34 -04:00
										 |  |  |         # Setup for coordinate descent analysis | 
					
						
							| 
									
										
										
										
											2018-08-18 12:25:57 -04:00
										 |  |  |         z_offset = offsets[2] | 
					
						
							| 
									
										
										
										
											2018-05-21 14:48:01 -04:00
										 |  |  |         logging.info("Calculating bed tilt with: %s", positions) | 
					
						
							|  |  |  |         params = { 'x_adjust': 0., 'y_adjust': 0., 'z_adjust': z_offset } | 
					
						
							| 
									
										
										
										
											2018-09-26 19:08:34 -04:00
										 |  |  |         # Perform coordinate descent | 
					
						
							| 
									
										
										
										
											2018-05-21 14:48:01 -04:00
										 |  |  |         def adjusted_height(pos, params): | 
					
						
							|  |  |  |             x, y, z = pos | 
					
						
							|  |  |  |             return (z - x*params['x_adjust'] - y*params['y_adjust'] | 
					
						
							|  |  |  |                     - params['z_adjust']) | 
					
						
							|  |  |  |         def errorfunc(params): | 
					
						
							|  |  |  |             total_error = 0. | 
					
						
							|  |  |  |             for pos in positions: | 
					
						
							|  |  |  |                 total_error += adjusted_height(pos, params)**2 | 
					
						
							|  |  |  |             return total_error | 
					
						
							|  |  |  |         new_params = mathutil.coordinate_descent( | 
					
						
							|  |  |  |             params.keys(), params, errorfunc) | 
					
						
							| 
									
										
										
										
											2018-09-26 19:08:34 -04:00
										 |  |  |         # Apply results | 
					
						
							| 
									
										
										
										
											2019-05-27 09:36:02 -04:00
										 |  |  |         speed = self.probe_helper.get_lift_speed() | 
					
						
							| 
									
										
										
										
											2018-05-21 14:48:01 -04:00
										 |  |  |         logging.info("Calculated bed tilt parameters: %s", new_params) | 
					
						
							| 
									
										
										
										
											2018-09-26 19:08:34 -04:00
										 |  |  |         x_adjust = new_params['x_adjust'] | 
					
						
							|  |  |  |         y_adjust = new_params['y_adjust'] | 
					
						
							|  |  |  |         z_adjust = (new_params['z_adjust'] - z_offset | 
					
						
							|  |  |  |                     - x_adjust * offsets[0] - y_adjust * offsets[1]) | 
					
						
							| 
									
										
										
										
											2019-05-27 09:36:02 -04:00
										 |  |  |         adjustments = [x*x_adjust + y*y_adjust + z_adjust | 
					
						
							|  |  |  |                        for x, y in self.z_positions] | 
					
						
							|  |  |  |         self.z_helper.adjust_steppers(adjustments, speed) | 
					
						
							| 
									
										
										
										
											2019-06-17 19:44:09 -07:00
										 |  |  |         return self.retry_helper.check_retry([p[2] for p in positions]) | 
					
						
							| 
									
										
										
										
											2018-05-21 14:48:01 -04:00
										 |  |  | 
 | 
					
						
							|  |  |  | def load_config(config): | 
					
						
							|  |  |  |     return ZTilt(config) |