From b37222e3a1ef1401d8f4fa5854c78d961740cb7c Mon Sep 17 00:00:00 2001 From: Arksine Date: Mon, 10 Jun 2019 14:24:26 -0400 Subject: [PATCH] skew_correction: initial implementation Signed-off-by: Eric Callahan --- klippy/extras/skew_correction.py | 82 ++++++++++++++++++++++++++++++++ 1 file changed, 82 insertions(+) create mode 100644 klippy/extras/skew_correction.py diff --git a/klippy/extras/skew_correction.py b/klippy/extras/skew_correction.py new file mode 100644 index 00000000..7d3d09d8 --- /dev/null +++ b/klippy/extras/skew_correction.py @@ -0,0 +1,82 @@ +# Printer Skew Correction +# +# This implementation is a port of Marlin's skew correction as +# implemented in planner.h, Copyright (C) Marlin Firmware +# +# https://github.com/MarlinFirmware/Marlin/tree/1.1.x/Marlin +# +# Copyright (C) 2019 Eric Callahan +# +# This file may be distributed under the terms of the GNU GPLv3 license. + +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))) + +def get_skew_from_plane(plane, config): + ac = config.getfloat(plane + "_ac_length", None, above=0.) + if ac is None: + return 0. + bd = config.getfloat(plane + "_bd_length", above=0.) + ad = config.getfloat(plane + "_ad_length", above=0.) + return calc_skew_factor(ac, bd, ad) + +class PrinterSkew: + def __init__(self, config): + self.printer = config.get_printer() + self.gcode = self.printer.lookup_object('gcode') + self.toolhead = None + self.xy_factor = get_skew_from_plane("xy", config) + self.xz_factor = get_skew_from_plane("xz", config) + self.yz_factor = get_skew_from_plane("yz", config) + self.printer.register_event_handler("klippy:ready", + self._handle_ready) + self.next_transform = None + self.gcode.register_command( + 'GET_CURRENT_SKEW', self.cmd_GET_CURRENT_SKEW, + desc=self.cmd_GET_CURRENT_SKEW_help) + self.gcode.register_command( + 'CALC_MEASURED_SKEW', self.cmd_CALC_MEASURED_SKEW, + desc=self.cmd_CALC_MEASURED_SKEW_help) + def _handle_ready(self): + self.next_transform = self.gcode.set_move_transform(self, force=True) + def calc_skew(self, pos): + 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], pos[3]] + def calc_unskew(self, pos): + 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], pos[3]] + 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) + cmd_GET_CURRENT_SKEW_help = "Report current printer skew" + def cmd_GET_CURRENT_SKEW(self, params): + 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)) + self.gcode.respond_info(out) + cmd_CALC_MEASURED_SKEW_help = "Calculate skew from measured print" + def cmd_CALC_MEASURED_SKEW(self, params): + ac = self.gcode.get_float("AC", params, above=0.) + bd = self.gcode.get_float("BD", params, above=0.) + ad = self.gcode.get_float("AD", params, above=0.) + factor = calc_skew_factor(ac, bd, ad) + self.gcode.respond_info( + "Calculated Skew: %.6f radians, %.2f degrees" % + (factor, math.degrees(factor))) + +def load_config(config): + return PrinterSkew(config)