diff options
Diffstat (limited to 'klippy/extras/skew_correction.py')
| -rw-r--r-- | klippy/extras/skew_correction.py | 160 |
1 files changed, 94 insertions, 66 deletions
diff --git a/klippy/extras/skew_correction.py b/klippy/extras/skew_correction.py index 94d8ceb7..f7346e59 100644 --- a/klippy/extras/skew_correction.py +++ b/klippy/extras/skew_correction.py @@ -11,10 +11,13 @@ import math + def calc_skew_factor(ac, bd, ad): - side = math.sqrt(2*ac*ac + 2*bd*bd - 4*ad*ad) / 2. - return math.tan(math.pi/2 - math.acos( - (ac*ac - side*side - ad*ad) / (2*side*ad))) + side = math.sqrt(2 * ac * ac + 2 * bd * bd - 4 * ad * ad) / 2.0 + return math.tan( + math.pi / 2 - math.acos((ac * ac - side * side - ad * ad) / (2 * side * ad)) + ) + class PrinterSkew: def __init__(self, config): @@ -22,82 +25,103 @@ class PrinterSkew: self.name = config.get_name() self.current_profile_name = "" self.toolhead = None - self.xy_factor = 0. - self.xz_factor = 0. - self.yz_factor = 0. + self.xy_factor = 0.0 + self.xz_factor = 0.0 + self.yz_factor = 0.0 self.skew_profiles = {} self._load_storage(config) - self.printer.register_event_handler("klippy:connect", - self._handle_connect) + self.printer.register_event_handler("klippy:connect", self._handle_connect) self.next_transform = None - gcode = self.printer.lookup_object('gcode') - gcode.register_command('GET_CURRENT_SKEW', self.cmd_GET_CURRENT_SKEW, - desc=self.cmd_GET_CURRENT_SKEW_help) - gcode.register_command('CALC_MEASURED_SKEW', - self.cmd_CALC_MEASURED_SKEW, - desc=self.cmd_CALC_MEASURED_SKEW_help) - gcode.register_command('SET_SKEW', self.cmd_SET_SKEW, - desc=self.cmd_SET_SKEW_help) - gcode.register_command('SKEW_PROFILE', self.cmd_SKEW_PROFILE, - desc=self.cmd_SKEW_PROFILE_help) + gcode = self.printer.lookup_object("gcode") + gcode.register_command( + "GET_CURRENT_SKEW", + self.cmd_GET_CURRENT_SKEW, + desc=self.cmd_GET_CURRENT_SKEW_help, + ) + gcode.register_command( + "CALC_MEASURED_SKEW", + self.cmd_CALC_MEASURED_SKEW, + desc=self.cmd_CALC_MEASURED_SKEW_help, + ) + gcode.register_command( + "SET_SKEW", self.cmd_SET_SKEW, desc=self.cmd_SET_SKEW_help + ) + gcode.register_command( + "SKEW_PROFILE", self.cmd_SKEW_PROFILE, desc=self.cmd_SKEW_PROFILE_help + ) + def _handle_connect(self): - gcode_move = self.printer.lookup_object('gcode_move') + gcode_move = self.printer.lookup_object("gcode_move") self.next_transform = gcode_move.set_move_transform(self, force=True) + def _load_storage(self, config): stored_profs = config.get_prefix_sections(self.name) # Remove primary skew_correction section, as it is not a stored profile - stored_profs = [s for s in stored_profs - if s.get_name() != self.name] + stored_profs = [s for s in stored_profs if s.get_name() != self.name] for profile in stored_profs: - name = profile.get_name().split(' ', 1)[1] + name = profile.get_name().split(" ", 1)[1] self.skew_profiles[name] = { - 'xy_skew': profile.getfloat("xy_skew"), - 'xz_skew': profile.getfloat("xz_skew"), - 'yz_skew': profile.getfloat("yz_skew"), + "xy_skew": profile.getfloat("xy_skew"), + "xz_skew": profile.getfloat("xz_skew"), + "yz_skew": profile.getfloat("yz_skew"), } + def calc_skew(self, pos): - skewed_x = pos[0] - pos[1] * self.xy_factor \ + skewed_x = ( + pos[0] + - pos[1] * self.xy_factor - pos[2] * (self.xz_factor - (self.xy_factor * self.yz_factor)) + ) skewed_y = pos[1] - pos[2] * self.yz_factor return [skewed_x, skewed_y] + pos[2:] + def calc_unskew(self, pos): - skewed_x = pos[0] + pos[1] * self.xy_factor \ - + pos[2] * self.xz_factor + skewed_x = pos[0] + pos[1] * self.xy_factor + pos[2] * self.xz_factor skewed_y = pos[1] + pos[2] * self.yz_factor return [skewed_x, skewed_y] + pos[2:] + def get_position(self): return self.calc_unskew(self.next_transform.get_position()) + def move(self, newpos, speed): corrected_pos = self.calc_skew(newpos) self.next_transform.move(corrected_pos, speed) + def _update_skew(self, xy_factor, xz_factor, yz_factor): self.xy_factor = xy_factor self.xz_factor = xz_factor self.yz_factor = yz_factor - gcode_move = self.printer.lookup_object('gcode_move') + gcode_move = self.printer.lookup_object("gcode_move") gcode_move.reset_last_position() + cmd_GET_CURRENT_SKEW_help = "Report current printer skew" + def cmd_GET_CURRENT_SKEW(self, gcmd): out = "Current Printer Skew:" planes = ["XY", "XZ", "YZ"] factors = [self.xy_factor, self.xz_factor, self.yz_factor] for plane, fac in zip(planes, factors): - out += '\n' + plane - out += " Skew: %.6f radians, %.2f degrees" % ( - fac, math.degrees(fac)) + out += "\n" + plane + out += " Skew: %.6f radians, %.2f degrees" % (fac, math.degrees(fac)) gcmd.respond_info(out) + cmd_CALC_MEASURED_SKEW_help = "Calculate skew from measured print" + def cmd_CALC_MEASURED_SKEW(self, gcmd): - ac = gcmd.get_float("AC", above=0.) - bd = gcmd.get_float("BD", above=0.) - ad = gcmd.get_float("AD", above=0.) + ac = gcmd.get_float("AC", above=0.0) + bd = gcmd.get_float("BD", above=0.0) + ad = gcmd.get_float("AD", above=0.0) factor = calc_skew_factor(ac, bd, ad) - gcmd.respond_info("Calculated Skew: %.6f radians, %.2f degrees" - % (factor, math.degrees(factor))) + gcmd.respond_info( + "Calculated Skew: %.6f radians, %.2f degrees" + % (factor, math.degrees(factor)) + ) + cmd_SET_SKEW_help = "Set skew based on lengths of measured object" + def cmd_SET_SKEW(self, gcmd): if gcmd.get_int("CLEAR", 0): - self._update_skew(0., 0., 0.) + self._update_skew(0.0, 0.0, 0.0) return planes = ["XY", "XZ", "YZ"] for plane in planes: @@ -111,57 +135,61 @@ class PrinterSkew: except Exception: raise gcmd.error( "skew_correction: improperly formatted entry for " - "plane [%s]\n%s" % (plane, gcmd.get_commandline())) - factor = plane.lower() + '_factor' + "plane [%s]\n%s" % (plane, gcmd.get_commandline()) + ) + factor = plane.lower() + "_factor" setattr(self, factor, calc_skew_factor(*lengths)) + cmd_SKEW_PROFILE_help = "Profile management for skew_correction" + def cmd_SKEW_PROFILE(self, gcmd): - if gcmd.get('LOAD', None) is not None: - name = gcmd.get('LOAD') + if gcmd.get("LOAD", None) is not None: + name = gcmd.get("LOAD") self.current_profile_name = name prof = self.skew_profiles.get(name) if prof is None: gcmd.respond_info( - "skew_correction: Load failed, unknown profile [%s]" - % (name)) + "skew_correction: Load failed, unknown profile [%s]" % (name) + ) return - self._update_skew(prof['xy_skew'], prof['xz_skew'], prof['yz_skew']) - elif gcmd.get('SAVE', None) is not None: - name = gcmd.get('SAVE') - configfile = self.printer.lookup_object('configfile') + self._update_skew(prof["xy_skew"], prof["xz_skew"], prof["yz_skew"]) + elif gcmd.get("SAVE", None) is not None: + name = gcmd.get("SAVE") + configfile = self.printer.lookup_object("configfile") cfg_name = self.name + " " + name - configfile.set(cfg_name, 'xy_skew', self.xy_factor) - configfile.set(cfg_name, 'xz_skew', self.xz_factor) - configfile.set(cfg_name, 'yz_skew', self.yz_factor) + configfile.set(cfg_name, "xy_skew", self.xy_factor) + configfile.set(cfg_name, "xz_skew", self.xz_factor) + configfile.set(cfg_name, "yz_skew", self.yz_factor) # Copy to local storage self.skew_profiles[name] = { - 'xy_skew': self.xy_factor, - 'xz_skew': self.xz_factor, - 'yz_skew': self.yz_factor + "xy_skew": self.xy_factor, + "xz_skew": self.xz_factor, + "yz_skew": self.yz_factor, } gcmd.respond_info( "Skew Correction state has been saved to profile [%s]\n" "for the current session. The SAVE_CONFIG command will\n" - "update the printer config file and restart the printer." - % (name)) - elif gcmd.get('REMOVE', None) is not None: - name = gcmd.get('REMOVE') + "update the printer config file and restart the printer." % (name) + ) + elif gcmd.get("REMOVE", None) is not None: + name = gcmd.get("REMOVE") if name in self.skew_profiles: - configfile = self.printer.lookup_object('configfile') - configfile.remove_section('skew_correction ' + name) + configfile = self.printer.lookup_object("configfile") + configfile.remove_section("skew_correction " + name) del self.skew_profiles[name] gcmd.respond_info( "Profile [%s] removed from storage for this session.\n" "The SAVE_CONFIG command will update the printer\n" - "configuration and restart the printer" % (name)) + "configuration and restart the printer" % (name) + ) else: gcmd.respond_info( - "skew_correction: No profile named [%s] to remove" - % (name)) + "skew_correction: No profile named [%s] to remove" % (name) + ) + def get_status(self, eventtime): - return { - 'current_profile_name': self.current_profile_name - } + return {"current_profile_name": self.current_profile_name} + def load_config(config): return PrinterSkew(config) |