From 7d8c70363a4241ce7c16868e347d7c0f7855f5f3 Mon Sep 17 00:00:00 2001 From: Aleksej Vasiljkovic Date: Wed, 21 Aug 2019 21:44:45 +0200 Subject: [PATCH] gcode_arcs: Add support for G2/G3 commands R Still missing, also might be somewhat dirty since code is converted into G1 commands. Signed-off-by: Aleksej Vasiljkovic Signed-off-by: Kevin O'Connor --- config/example-extras.cfg | 10 ++ klippy/extras/gcode_arcs.py | 186 ++++++++++++++++++++++++++++++++++++ 2 files changed, 196 insertions(+) create mode 100644 klippy/extras/gcode_arcs.py diff --git a/config/example-extras.cfg b/config/example-extras.cfg index 98116151..e3c83186 100644 --- a/config/example-extras.cfg +++ b/config/example-extras.cfg @@ -1801,3 +1801,13 @@ # A list of G-Code commands to execute when the delay duration has # elapsed. G-Code templates are supported. This parameter must be # provided. + + +# enables arc (G2/G3) commands. Only IJ version is supported +# example: "G2 X125 Y32 Z10 E5 I10.5 J10.5" +#[gcode_arcs] +#resolution: 1.0 +# An Arc will be split in segments. Each segment will in x mm set here. +# Lower values will producse a finer arc, but also more to do for +# your machine. This also means that arcs smaller then this value +# will be a line only diff --git a/klippy/extras/gcode_arcs.py b/klippy/extras/gcode_arcs.py new file mode 100644 index 00000000..b0d6e795 --- /dev/null +++ b/klippy/extras/gcode_arcs.py @@ -0,0 +1,186 @@ +# adds support fro ARC commands via G2/G3 +# +# Copyright (C) 2019 Aleksej Vasiljkovic +# +# function planArc() originates from https://github.com/MarlinFirmware/Marlin +# Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm +# +# This file may be distributed under the terms of the GNU GPLv3 license. + + +# uses the plan_arc function from marlin which does steps in mm rather then +# in degrees. # Coordinates created by this are converted into G1 commands. +# +# note: only IJ version available + +import math +import re + +class ArcSupport: + def __init__(self, config): + self.printer = config.get_printer() + self.mm_per_arc_segment = config.getfloat('resolution', 1) + self.debug = True #will respond motion to terminal as G1 code + + self.gcode = self.printer.lookup_object('gcode') + self.gcode.register_command("G2", self.cmd_G2, desc=self.cmd_G2_help) + self.gcode.register_command("G3", self.cmd_G2, desc=self.cmd_G3_help) + + cmd_G2_help = "Counterclockwise rotation move" + cmd_G3_help = "Clockwise rotaion move" + + def cmd_G2(self, params): + + # set vars + currentPos = self.printer.lookup_object('toolhead').get_position() + asStartX = currentPos[0] + asStartY = currentPos[1] + asStartZ = currentPos[2] + + asX = params.get("X", None) + asY = params.get("Y", None) + asZ = params.get("Z", None) + + asR = float(params.get("R", 0.)) #radius + asI = float(params.get("I", 0.)) + asJ = float(params.get("J", 0.)) + + asE = float(params.get("E", 0.)) + asF = float(params.get("F", -1)) + + # --------- health checks of code ----------- + if (asX == None or asY == None): + raise self.gcode.error("g2/g3: Coords missing") + + elif asR == 0 and asI == 0 and asJ==0: + raise self.gcode.error("g2/g3: neither R nor I and J given") + + elif asR > 0 and (asI !=0 or asJ!=0): + raise self.gcode.error("g2/g3: R, I and J were given. Invalid") + else: # -------- execute conversion ----------- + coords = [] + clockwise = params['#command'].lower().startswith("g2") + asY = float(asY) + asX = float(asX) + + # use radius + # if asR > 0: + # not sure if neccessary since R barely seems to be used + + # use IJK + + if asI != 0 or asJ!=0: + coords = self.planArc(currentPos, + [asX,asY,0.,0.], + [asI, asJ], + clockwise) + ############################### + # converting coords into G1 codes (lazy aproch) + if len(coords)>0: + + # build dict and call cmd_G1 + for coord in coords: + g1_params = {'X': coord[0], 'Y': coord[1]} + if asZ: + g1_params['Z']= float(asZ)/len(coords) + if asE>0: + g1_params['E']= float(asE)/len(coords) + if asF>0: + g1_params['F']= asF + + self.gcode.cmd_G1(g1_params) + + + + + else: + self.gcode.respond_info( + "could not tranlate from '" + params['#original'] + "'") + + + # function planArc() originates from marlin plan_arc() + # https://github.com/MarlinFirmware/Marlin + # + # The arc is approximated by generating many small linear segments. + # The length of each segment is configured in MM_PER_ARC_SEGMENT + # Arcs smaller then this value, will be a Line only + + def planArc( + self, + currentPos, + targetPos=[0.,0.,0.,0.], + offset=[0.,0.], + clockwise=False): + # todo: sometimes produces full circles + coords = [] + MM_PER_ARC_SEGMENT = self.mm_per_arc_segment + + X_AXIS = 0 + Y_AXIS = 1 + Z_AXIS = 2 + + # Radius vector from center to current location + r_P = offset[0]*-1 + r_Q = offset[1]*-1 + + radius = math.hypot(r_P, r_Q) + center_P = currentPos[X_AXIS] - r_P + center_Q = currentPos[Y_AXIS] - r_Q + rt_X = targetPos[X_AXIS] - center_P + rt_Y = targetPos[Y_AXIS] - center_Q + linear_travel = targetPos[Z_AXIS] - currentPos[Z_AXIS] + + angular_travel = math.atan2(r_P * rt_Y - r_Q * rt_X, + r_P * rt_X + r_Q * rt_Y) + if (angular_travel < 0): angular_travel+= math.radians(360) + if (clockwise): angular_travel-= math.radians(360) + + # Make a circle if the angular rotation is 0 + # and the target is current position + if (angular_travel == 0 + and currentPos[X_AXIS] == targetPos[X_AXIS] + and currentPos[Y_AXIS] == targetPos[Y_AXIS]): + angular_travel = math.radians(360) + + + flat_mm = radius * angular_travel + mm_of_travel = linear_travel + if(mm_of_travel == linear_travel): + mm_of_travel = math.hypot(flat_mm, linear_travel) + else: + mm_of_travel = math.abs(flat_mm) + + + if (mm_of_travel < 0.001): + return coords + + segments = int(math.floor(mm_of_travel / (MM_PER_ARC_SEGMENT))) + if(segments<1): + segments=1 + + + raw = [0.,0.,0.,0.] + theta_per_segment = float(angular_travel / segments) + linear_per_segment = float(linear_travel / segments) + + # Initialize the linear axis + raw[Z_AXIS] = currentPos[Z_AXIS]; + + + for i in range(1,segments+1): + cos_Ti = math.cos(i * theta_per_segment) + sin_Ti = math.sin(i * theta_per_segment) + r_P = -offset[0] * cos_Ti + offset[1] * sin_Ti + r_Q = -offset[0] * sin_Ti - offset[1] * cos_Ti + + raw[X_AXIS] = center_P + r_P + raw[Y_AXIS] = center_Q + r_Q + raw[Z_AXIS] += linear_per_segment + + coords.append([raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS] ]) + + return coords + + +def load_config(config): + return ArcSupport(config)