skew_correction: initial implementation

Signed-off-by:  Eric Callahan <arksine.code@gmail.com>
This commit is contained in:
Arksine 2019-06-10 14:24:26 -04:00 committed by KevinOConnor
parent 2d7f32584f
commit b37222e3a1
1 changed files with 82 additions and 0 deletions

View File

@ -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 <arksine.code@gmail.com>
#
# 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)