diff --git a/pygcode/gcodes.py b/pygcode/gcodes.py index 6c08bdd..f10186a 100644 --- a/pygcode/gcodes.py +++ b/pygcode/gcodes.py @@ -152,7 +152,7 @@ class GCode(object): # Execution Order exec_order = 999 # if not otherwise specified, run last - def __init__(self, *words): + def __init__(self, *words, **params): """ :param word: Word instance defining gcode (eg: Word('G0') for rapid movement) :param params: list of Word instances (eg: Word('X-1.2') as x-coordinate) @@ -170,6 +170,8 @@ class GCode(object): # Add Given Parameters for param_word in param_words: self.add_parameter(param_word) + for (k, v) in params.items(): + self.add_parameter(Word(k, v)) def __repr__(self): param_str = '' diff --git a/pygcode/transform.py b/pygcode/transform.py index 60fb6af..e3b0c00 100644 --- a/pygcode/transform.py +++ b/pygcode/transform.py @@ -1,5 +1,6 @@ -from math import acos, atan2, pi, sqrt +from math import acos, atan2, pi, sqrt, ceil +from .gcodes import GCodeLinearMove from .gcodes import GCodeArcMove, GCodeArcMoveCW, GCodeArcMoveCCW from .gcodes import GCodePlaneSelect, GCodeSelectXYPlane, GCodeSelectYZPlane, GCodeSelectZXPlane from .gcodes import GCodeAbsoluteDistanceMode, GCodeIncrementalDistanceMode @@ -15,14 +16,27 @@ from .utils import Vector3, Quaternion, plane_projection class ArcLinearizeMethod(object): pass - def __init__(self, max_error, radius): + def __init__(self, max_error, plane_normal, + arc_p_start, arc_p_end, arc_p_center, + arc_radius, arc_angle, helical_start, helical_end): self.max_error = max_error - self.radius = radius + self.plane_normal = plane_normal + self.arc_p_start = arc_p_start + self.arc_p_end = arc_p_end + self.arc_p_center = arc_p_center + self.arc_radius = arc_radius + self.arc_angle = arc_angle + self.helical_start = helical_start + self.helical_end = helical_end def get_max_wedge_angle(self): """Calculate angular coverage of a single line reaching maximum allowable error""" raise NotImplementedError("not overridden") + def iter_vertices(self): + """Yield absolute (, ) for each line for the arc""" + raise NotImplementedError("not overridden") + class ArcLinearizeInside(ArcLinearizeMethod): """Start and end points of each line are on the original arc""" @@ -34,9 +48,30 @@ class ArcLinearizeInside(ArcLinearizeMethod): # - Simplest maths, easiest to explain & visually verify def get_max_wedge_angle(self): - return 2 * acos((self.radius - self.max_error) / self.radius) + return abs(2 * acos((self.arc_radius - self.max_error) / self.arc_radius)) + def iter_vertices(self): + wedge_count = int(ceil(abs(self.arc_angle) / self.get_max_wedge_angle())) + wedge_angle = self.arc_angle / wedge_count + start_radius = self.arc_p_start - self.arc_p_center + helical_delta = (self.helical_end - self.helical_start) / wedge_count + l_p_start = start_radius + self.arc_p_center + l_start = l_p_start + self.helical_start + for i in range(wedge_count): + q_end = Quaternion.new_rotate_axis( + angle=wedge_angle * (i+1), + axis=-self.plane_normal, + ) + # Projected on selected plane + l_p_end = (q_end * start_radius) + self.arc_p_center + # Helical displacement + l_end = l_p_end + (self.helical_start + (helical_delta * (i+1))) + + yield (l_start, l_end) + + # start of next line is the end of this line + (l_p_start, l_start) = (l_p_end, l_end) @@ -64,7 +99,7 @@ DEFAULT_LA_ARCDISTMODE = GCodeIncrementalArcDistanceMode def linearize_arc(arc_gcode, start_pos, plane=None, method_class=None, dist_mode=None, arc_dist_mode=None, - max_error=0.01, precision_fmt="{0:.3f}"): + max_error=0.01, decimal_places=3): # set defaults if method_class is None: method_class = DEFAULT_LA_method_class @@ -177,40 +212,56 @@ def linearize_arc(arc_gcode, start_pos, plane=None, method_class=None, # - helical_start distance along plane.normal of arc start # - helical_disp distance along plane.normal of arc end - # TODO: debug printing - print(( - "linearize_arc params\n" - " - arc_p_start {arc_p_start}\n" - " - arc_p_end {arc_p_end}\n" - " - arc_p_center {arc_p_center}\n" - " - arc_radius {arc_radius}\n" - " - arc_angle {arc_angle:.4f} ({arc_angle_deg:.3f} deg)\n" - " - helical_start {helical_start}\n" - " - helical_end {helical_end}\n" - ).format( - arc_p_start=arc_p_start, - arc_p_end=arc_p_end, - arc_p_center=arc_p_center, - arc_radius=arc_radius, - arc_angle=arc_angle, arc_angle_deg=arc_angle * (180/pi), - helical_start=helical_start, - helical_end=helical_end, - )) + #print(( + # "linearize_arc params\n" + # " - arc_p_start {arc_p_start}\n" + # " - arc_p_end {arc_p_end}\n" + # " - arc_p_center {arc_p_center}\n" + # " - arc_radius {arc_radius}\n" + # " - arc_angle {arc_angle:.4f} ({arc_angle_deg:.3f} deg)\n" + # " - helical_start {helical_start}\n" + # " - helical_end {helical_end}\n" + #).format( + # arc_p_start=arc_p_start, + # arc_p_end=arc_p_end, + # arc_p_center=arc_p_center, + # arc_radius=arc_radius, + # arc_angle=arc_angle, arc_angle_deg=arc_angle * (180/pi), + # helical_start=helical_start, + # helical_end=helical_end, + #)) + method_class_params = { + 'max_error': max_error, + 'plane_normal': plane.normal, + 'arc_p_start': arc_p_start, + 'arc_p_end': arc_p_end, + 'arc_p_center': arc_p_center, + 'arc_radius': arc_radius, + 'arc_angle': arc_angle, + 'helical_start': helical_start, + 'helical_end': helical_end, + } + method = method_class(**method_class_params) - - method = method_class( - max_error=max_error, - radius=arc_radius, - ) - - #plane_projection(vect, normal) - - pass - # Steps: - # - calculate: - # - - # - calculate number of linear segments + #import ipdb; ipdb.set_trace() + if isinstance(dist_mode, GCodeAbsoluteDistanceMode): + # Absolute coordinates + for line_vertices in method.iter_vertices(): + (l_start, l_end) = line_vertices + yield GCodeLinearMove(**dict(zip('XYZ', l_end.xyz))) + else: + # Incremental coordinates (beware cumulative errors) + cur_pos = arc_start + for line_vertices in method.iter_vertices(): + (l_start, l_end) = line_vertices + l_delta = l_end - cur_pos + + # round delta coordinates (introduces errors) + for axis in 'xyz': + setattr(l_delta, axis, round(getattr(l_delta, axis), decimal_places)) + yield GCodeLinearMove(**dict(zip('XYZ', l_delta.xyz))) + cur_pos += l_delta # mitigate errors by also adding them the accumulated cur_pos # ==================== Arc Precision Adjustment ==================== diff --git a/pygcode/words.py b/pygcode/words.py index 059dc42..776859f 100644 --- a/pygcode/words.py +++ b/pygcode/words.py @@ -16,7 +16,7 @@ def _clean_codestr(value): return "%g" % value CLEAN_NONE = lambda v: v -CLEAN_FLOAT = lambda v: "%g" % v +CLEAN_FLOAT = lambda v: "{0:g}".format(round(v, 3)) CLEAN_CODE = _clean_codestr CLEAN_INT = lambda v: "%g" % v diff --git a/scripts/pygcode-normalize.py b/scripts/pygcode-normalize.py index d5e3eb3..613ea00 100755 --- a/scripts/pygcode-normalize.py +++ b/scripts/pygcode-normalize.py @@ -1,5 +1,7 @@ #!/usr/bin/env python import argparse +import re +from collections import defaultdict for pygcode_lib_type in ('installed_lib', 'relative_lib'): try: @@ -46,10 +48,18 @@ parser.add_argument( # Arcs parser.add_argument( - '--arcs_linearize', '-al', dest='arcs_linearize', + '--arc_linearize', '-al', dest='arc_linearize', action='store_const', const=True, default=False, help="convert G2/3 commands to a series of linear G1 linear interpolations", ) + +parser.add_argument( + '--arc_lin_method', '-alm', dest='arc_lin_method', default='i', + help="Method of linearizing arcs, i=inner, o=outer, m=middle. List 2 " + "for ,, eg 'i,o'. also: 'm' is equivalent to 'm,m' ", + metavar='{i,o,m}[,{i,o,m]', +) + parser.add_argument( '--arc_alignment', '-aa', dest='arc_alignment', type=str, choices=('XYZ','IJK','R'), default=None, @@ -61,6 +71,31 @@ parser.add_argument( args = parser.parse_args() +# arc linearizing method (manually parsing) +ARC_LIN_CLASS_MAP = { + 'i': ArcLinearizeInside, + 'o': ArcLinearizeOutside, + 'm': ArcLinearizeMid, +} + +arc_lin_method_regex = re.compile(r'^(?P[iom])(,(?P[iom]))?$', re.I) +if args.arc_lin_method: + match = arc_lin_method_regex.search(args.arc_lin_method) + if not match: + raise RuntimeError("parameter for --arc_lin_method is invalid: '%s'" % args.arc_lin_method) + + # changing args.arc_lin_method (because I'm a fiend) + args.arc_lin_method = {} + args.arc_lin_method['G2'] = ARC_LIN_CLASS_MAP[match.group('g2')] + if match.group('g3'): + args.arc_lin_method['G3'] = ARC_LIN_CLASS_MAP[match.group('g3')] + else: + args.arc_lin_method['G3'] = args.arc_lin_method['G2'] +else: + args.arc_lin_method = defaultdict(lambda: ArcLinearizeInside) # just to be sure + + + # =================== Create Virtual CNC Machine =================== class MyMode(Mode): default_mode = args.machine_mode @@ -76,40 +111,45 @@ def gcodes2str(gcodes): # =================== Process File =================== -print(args) for line_str in args.infile[0].readlines(): line = Line(line_str) - if line.comment: - print("===== %s" % line.comment.text) + #if line.comment: + # print("===== %s" % line.comment.text) effective_gcodes = machine.block_modal_gcodes(line.block) - if any(isinstance(g, GCodeArcMove) for g in effective_gcodes): - print("---------> Found an Arc <----------") + if args.arc_linearize and any(isinstance(g, GCodeArcMove) for g in effective_gcodes): + #print("---------> Found an Arc <----------") (befores, (arc,), afters) = split_gcodes(effective_gcodes, GCodeArcMove) # TODO: debug printing (for now) if befores: - print("befores: %s" % gcodes2str(befores)) + print(gcodes2str(befores)) machine.process_gcodes(*befores) - print("arc: %s" % str(arc)) - linearize_arc( - arc_gcode=arc, - start_pos=machine.pos, - plane=machine.mode.plane_selection, - method_class=ArcLinearizeInside, # FIXME: selectable from args - dist_mode=machine.mode.distance, - arc_dist_mode=machine.mode.arc_ijk_distance, - max_error=args.precision, - ) + #print("arc: %s" % str(arc)) + linearize_params = { + 'arc_gcode': arc, + 'start_pos': machine.pos, + 'plane': machine.mode.plane_selection, + 'method_class': args.arc_lin_method["%s%i" % (arc.word.letter, arc.word.value)], + 'dist_mode': machine.mode.distance, + 'arc_dist_mode': machine.mode.arc_ijk_distance, + 'max_error': args.precision, + 'decimal_places': 3, + } + for linear_gcode in linearize_arc(**linearize_params): + print(linear_gcode) machine.process_gcodes(arc) if afters: - print("afters: %s" % gcodes2str(afters)) + print(gcodes2str(afters)) machine.process_gcodes(*afters) + if line.comment: + print(str(line.comment)) else: + print(str(line)) machine.process_block(line.block) - print("%r, %s" % (sorted(line.block.gcodes), line.block.modal_params)) + #print("%r, %s" % (sorted(line.block.gcodes), line.block.modal_params))