initial stab at sending G code with offset and rotated data, need to test out with actual machine

callbackloghandler.py

@@ -0,0 +1,38 @@
+"""
+Gerbil - Copyright (c) 2015 Michael Franzl
+
+Permission is hereby granted, free of charge, to any person obtaining a
+copy of this software and associated documentation files (the "Software"),
+to deal in the Software without restriction, including without limitation
+the rights to use, copy, modify, merge, publish, distribute, sublicense,
+and/or sell copies of the Software, and to permit persons to whom the
+Software is furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included
+in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
+OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+OTHER DEALINGS IN THE SOFTWARE.
+"""
+
+import logging
+
+class CallbackLogHandler(logging.StreamHandler):
+    def __init__(self, cb=None):
+        super(CallbackLogHandler, self).__init__()
+        self.callback = cb
+
+    def emit(self, record):
+        if self.callback:
+            #txt = "{0} {1}".format(record.level, record.msg)
+            #print(self.format(record))
+            #self.callback("on_log", self.format(record))
+            #self.callback("on_log", record.levelno, record.msg % record.args)
+            self.callback("on_log", record)
+        else:
+            logging.StreamHandler.emit(self, record)

gcode_machine.py

@@ -0,0 +1,844 @@
+"""
+gcode_machine - Copyright (c) 2016 Michael Franzl
+
+Permission is hereby granted, free of charge, to any person obtaining a
+copy of this software and associated documentation files (the "Software"),
+to deal in the Software without restriction, including without limitation
+the rights to use, copy, modify, merge, publish, distribute, sublicense,
+and/or sell copies of the Software, and to permit persons to whom the
+Software is furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included
+in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
+OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+OTHER DEALINGS IN THE SOFTWARE.
+"""
+
+import re
+import logging
+import math
+import numpy as np
+
+class GcodeMachine:
+    """ This implements a simple CNC state machine that can be used
+    for simulation and processing of G-code.
+    
+    For usage, see README.md.
+    
+    Callbacks:
+    
+    on_feed_change
+    : Emitted when a F keyword is parsed from the G-Code.
+    : 1 argument: the feed rate in mm/min
+
+    on_var_undefined
+    : Emitted when a variable is to be substituted but no substitution value has been set previously.
+    : 1 argument: the key of the undefined variable
+    """
+    
+    def __init__(self, impos=(0,0,0), ics="G54", cs_offsets={"G54":(0,0,0)}):
+        """ Initialization.
+        """
+        
+        self.logger = logging.getLogger('gerbil')
+        
+        
+        ## @var line
+        # Holds the current Gcode line
+        self.line = ""
+        
+        ## @var vars
+        # A dict holding values for variable substitution.
+        self.vars = {}
+        
+        ## @var callback method for parent applications
+        self.callback = self._default_callback
+        
+        ## @var do_feed_override
+        # If set to True, F commands will be replaced with the feed
+        # speed set in `request_feed`. If set to False, no feed speed
+        # processing will be done.
+        self.do_feed_override = False
+        
+        ## @var do_fractionize_lines
+        # If set to True, linear movements over the threshold of
+        # `fract_linear_threshold` will be broken down into small line
+        # segments of length `fract_linear_segment_len`. If set to False
+        # no processing is done on lines.
+        self.do_fractionize_lines = False
+        
+        ## @var do_fractionize_arcs
+        # If set to True, arcs will be broken up into small line segments.
+        # If set to False, no processing is done on arcs.
+        self.do_fractionize_arcs = False
+        
+        ## @var fract_linear_threshold
+        # The threshold for the fractionization of lines.
+        self.fract_linear_threshold = 1
+        
+        ## @var fract_linear_segment_len
+        # The length of the segments of fractionized lines.
+        self.fract_linear_segment_len = 0.5
+        
+        ## @var spindle_factor
+        # Scale S by this value
+        self.spindle_factor = 1
+        
+        ## @var request_feed
+        # If `do_feed_override` is True, a F command will be inserted
+        # to the currently active command with this value.
+        self.request_feed = None
+
+        ## @var current_feed
+        # The current feed rate of the machine
+        self.current_feed = None
+        
+        ## @var contains_feed
+        # True if the current line contains a F command
+        self.contains_feed = False
+        
+        ## @var current_distance_mode
+        # Contains the current distance mode of the machine as string
+        self.current_distance_mode = "G90"
+        
+        ## @var current_motion_mode
+        # Contains the current motion mode of the machine as integer (0 to 3)
+        self.current_motion_mode = 0
+        
+        ## @var current_plane_mode
+        # Contains the current plane mode of the machine as string
+        self.current_plane_mode = "G17"
+        
+       
+        ## @var cs_offsets
+        # Coordinate system offsets. A Python dict with
+        # 3-tuples as offsets.
+        self.cs_offsets = cs_offsets
+        
+        ## @var cs
+        # Current coordinate system as string.
+        self.cs = ics
+        
+        ## @var pos_m
+        # Contains the current machine position before execution
+        # of the currently set line (the target of the last command)
+        self.pos_m = list(impos) # initial machine position
+        
+        ## @var pos_w
+        # Contains the current working position before execution
+        # of the currently set line (the target of the last command)
+        self.pos_w = list(np.subtract(self.pos_m, self.cs_offsets[self.cs]))
+       
+        
+        ## @var target_m
+        # Contains the position target of the currently set command in
+        # the machine coordinate system.
+        self.target_m = [None, None, None]
+        
+        ## @var target_m
+        # Contains the position target of the currently set command in
+        # the currently seleted coordinate system.
+        self.target_w = [None, None, None]
+        
+        ## @var offset
+        # Contains the offset of the arc center from current position
+        self.offset = [0, 0, 0] 
+        
+        ## @var radius
+        # Contains the radius of the current arc
+        self.radius = None
+        
+        ## @var contains_radius
+        # True if the current line contains a R word
+        self.contains_radius = False
+        
+        ## @var current_spindle_speed
+        # Contains the current spindle speed (S word)
+        self.current_spindle_speed = None
+        
+        ## @var contains_spindle
+        # True if the current line contains the S word
+        self.contains_spindle = False
+        
+        ## @var dist
+        # Distance that current line will travel
+        self.dist = 0 
+        
+        ## @var dists
+        # Distance that current line will travel, in [x,y,z] directions
+        self.dist_xyz = [0, 0, 0]
+        
+        ## @var line_is_unsupported_cmd
+        # True if current line is not in the whitelist
+        self.line_is_unsupported_cmd = False
+        
+        self.comment = ""
+        
+        self.special_comment_prefix = "_gcm"
+        
+        # precompile regular expressions
+        self._axes_regexps = []
+        self._axes_words = ["X", "Y", "Z"]
+        for i in range(0, 3):
+            word = self._axes_words[i]
+            self._axes_regexps.append(re.compile(".*" + word + "([-.\d]+)"))
+            
+        self._offset_regexps = []
+        self._offset_words = ["I", "J", "K"]
+        for i in range(0, 3):
+            word = self._offset_words[i]
+            self._offset_regexps.append(re.compile(".*" + word + "([-.\d]+)"))
+            
+        self._re_radius = re.compile(".*R([-.\d]+)")
+        
+        self._re_use_var = re.compile("#(\d*)")
+        self._re_set_var = re.compile("^\s*#(\d+)=([\d.-]+)")
+        
+        self._re_feed = re.compile(".*F([.\d]+)")
+        self._re_feed_replace = re.compile(r"F[.\d]+")
+        
+        self._re_spindle = re.compile(".*S([\d]+)")
+        self._re_spindle_replace = re.compile(r"S[.\d]+")
+        
+        self._re_motion_mode = re.compile("G([0123])*([^\\d]|$)")
+        self._re_distance_mode = re.compile("(G9[01])([^\d]|$)")
+        self._re_plane_mode = re.compile("(G1[789])([^\d]|$)")
+        self._re_cs = re.compile("(G5[4-9])")
+        
+        self._re_comment_paren_convert = re.compile("(.*)\((.*?)\)\s*$")
+        self._re_comment_paren_replace = re.compile(r"\(.*?\)")
+        self._re_comment_get_comment = re.compile("(.*?)(;.*)")
+        
+        self._re_match_cmd_number = re.compile("([GMT])(\d+)")
+        self._re_expand_multicommands = re.compile("([GMT])")
+
+
+        ## @var whitelist_commands
+        # Strip line from everything that is not in this list
+        self.whitelist_commands = {
+            "G": [
+                # non-modal commands
+                4,  # Dwell
+                10, # set coordintate system
+                28, # Go to predefined position
+                30, # Go to predefined position
+                53, # move in machine coordinates
+                92, # set coordintate system offset
+                
+                # motion modes
+                0, # fast linear
+                1, # slow linear
+                2, # slow arc CW
+                3, # slow arc CCW
+                38, # probe
+                80, # canned cycle stop
+                
+                # feed rate modes
+                93, # inverse
+                94, # normal
+                
+                # units
+                20, # inch
+                21, # mm
+                
+                # distance modes
+                90, # absulute
+                91, # incremental
+                
+                # plane select
+                17, # XY
+                18, # ZY
+                19, # YZ
+                
+                # tool length offset
+                43, # set compensation
+                49, # cancel compensation
+                
+                # coordinate systems
+                54,
+                55,
+                56,
+                57,
+                58,
+                59,
+                ],
+            
+            "M": [
+                # program flow
+                0,
+                1,
+                2,
+                30,
+                
+                # coolant
+                8,
+                9,
+                
+                # spindle
+                3,
+                4,
+                5,
+                ]
+            }
+        
+        self.logger.info("Preprocessor Class Initialized")
+        
+        
+    def reset(self):
+        """
+        Reset to initial state.
+        """
+        self.vars = {}
+        self.current_feed = None
+        self.current_motion_mode = 0
+        self.current_distance_mode = "G90"
+        self.callback("on_feed_change", self.current_feed)
+        
+    @property
+    def position_m(self):
+        return list(self.pos_m)
+    
+    @position_m.setter
+    def position_m(self, pos):
+        self.pos_m = list(pos)
+        self.pos_w = list(np.subtract(self.pos_m, self.cs_offsets[self.cs]))
+        
+    @property
+    def current_cs(self):
+        return self.cs
+    
+    @current_cs.setter
+    def current_cs(self, label):
+        self.cs = label
+        self.pos_w = list(np.subtract(self.pos_m, self.cs_offsets[self.cs]))
+        
+        
+    def set_line(self, line):
+        """
+        Load a Gcode line into the machine. It will be available in `self.line`.
+        
+        @param line
+        A string of Gcode.
+        """
+        self.line = line
+        self.transform_comments()
+
+        
+    def split_lines(self):
+        """
+        Some Gcode generating software spit out 'composite' commands
+        like "M3 T2". This really is bad practice.
+        
+        This method splits such 'composite' commands up into separate
+        commands and returns a list of commands. Machine state is not
+        changed.
+        """
+        commands = re.sub(self._re_expand_multicommands, "\n\g<0>", self.line).strip()
+        lines = commands.split("\n")
+        lines[0] = lines[0] + self.comment # preserve comment
+        return lines
+    
+    
+    def strip(self):
+        """
+        Remove blank spaces and newlines from beginning and end, and remove blank spaces from the middle of the line.
+        """
+        self.line = self.line.replace(" ", "")
+        self.line = self.line.strip()
+        
+        
+    def tidy(self):
+        """
+        Strips G-Code not contained in the whitelist.
+        """
+        
+        # transform [MG]\d to G\d\d for better parsing
+        def format_cmd_number(matchobj):
+            cmd = matchobj.group(1)
+            cmd_nr = int(matchobj.group(2))
+            self.line_is_unsupported_cmd = not (cmd in self.whitelist_commands and cmd_nr in self.whitelist_commands[cmd])
+            return "{}{:02d}".format(cmd, cmd_nr)
+        
+        self.line = re.sub(self._re_match_cmd_number, format_cmd_number, self.line)
+        
+        if self.line_is_unsupported_cmd:
+            self.line = ";" + self.line + " ;" + self.special_comment_prefix + ".unsupported"
+    
+    
+    def parse_state(self):
+        """
+        This method...
+        
+        * parses motion mode
+        * parses distance mode
+        * parses plane mode
+        * parses feed rate
+        * parses spindle speed
+        * parses arcs (offsets and radi)
+        * calculates travel distances
+        
+        ... and updates the machine's state accordingly.
+        """
+    
+        # parse G0 .. G3 and remember
+        m = re.match(self._re_motion_mode, self.line)
+        if m: self.current_motion_mode = int(m.group(1))
+        
+        # parse G90 and G91 and remember
+        m = re.match(self._re_distance_mode, self.line)
+        if m: self.current_distance_mode = m.group(1)
+            
+        # parse G17, G18 and G19 and remember
+        m = re.match(self._re_plane_mode, self.line)
+        if m: self.current_plane_mode = m.group(1)
+        
+        m = re.match(self._re_cs, self.line)
+        if m: self.current_cs = m.group(1)
+            
+        # see if current line has F
+        m = re.match(self._re_feed, self.line)
+        self.contains_feed = True if m else False
+        if m: self.feed_in_current_line = float(m.group(1))
+        
+        # look for spindle S
+        m = re.match(self._re_spindle, self.line)
+        self.contains_spindle = True if m else False
+        if m: self.current_spindle_speed = int(m.group(1))
+        
+        # arc parsing and calculations
+        if self.current_motion_mode == 2 or self.current_motion_mode == 3:
+            self.offset = [None, None, None]
+            for i in range(0, 3):
+                # loop over I, J, K offsets
+                regexp = self._offset_regexps[i]
+                
+                m = re.match(regexp, self.line)
+                if m: self.offset[i] = float(m.group(1))
+                    
+            # parses arcs
+            m = re.match(self._re_radius, self.line)
+            self.contains_radius = True if m else False
+            if m: self.radius = float(m.group(1))
+                
+                
+        # calculate distance traveled by this G-Code cmd in xyz
+        self.dist_xyz = [0, 0, 0] 
+        for i in range(0, 3):
+            # loop over X, Y, Z axes
+            regexp = self._axes_regexps[i]
+            
+            m = re.match(regexp, self.line)
+            if m:
+                if self.current_distance_mode == "G90":
+                    # absolute distances
+                    self.target_m[i] = self.cs_offsets[self.cs][i] + float(m.group(1))
+                    self.target_w[i] = float(m.group(1))
+                    
+                    # calculate distance
+                    self.dist_xyz[i] = self.target_m[i] - self.pos_m[i]
+                else:
+                    # G91 relative distances
+                    self.dist_xyz[i] = float(m.group(1))
+                    self.target_m[i] += self.dist_xyz[i]
+                    self.target_w[i] += self.dist_xyz[i]
+            else:
+                # no movement along this axis, stays the same
+                self.target_m[i] = self.pos_m[i]
+                self.target_w[i] = self.pos_w[i]
+                    
+        # calculate travelling distance
+        self.dist = math.sqrt(self.dist_xyz[0] * self.dist_xyz[0] + self.dist_xyz[1] * self.dist_xyz[1] + self.dist_xyz[2] * self.dist_xyz[2])
+        
+        
+    
+    
+    def fractionize(self):
+        """
+        Breaks lines longer than a certain threshold into shorter segments.
+        
+        Also breaks circles into segments.
+        
+        Returns a list of command strings. Does not update the machine state.
+        """
+
+        result = []
+
+        if self.do_fractionize_lines == True and self.current_motion_mode == 1 and self.dist > self.fract_linear_threshold:
+            result = self._fractionize_linear_motion()
+           
+        elif self.do_fractionize_arcs == True and (self.current_motion_mode == 2 or self.current_motion_mode == 3):
+            result = self._fractionize_circular_motion()
+            
+        else:
+            # this motion cannot be fractionized
+            # return the line as it was passed in
+            result = [self.line]
+
+        result[0] = result[0] + self.comment # preserve comment
+        return result
+        
+    
+    def done(self):
+        """
+        When all processing/inspecting of a command has been done, call this method.
+        This will virtually 'move' the tool of the machine if the current command
+        is a motion command.
+        """
+        if not (self.current_motion_mode == 0 or self.current_motion_mode == 1):
+            # only G0 and G1 can stay active without re-specifying
+            self.current_motion_mode = None 
+            
+        # move the 'tool'
+        for i in range(0, 3):
+            # loop over X, Y, Z axes
+            if self.target_m[i] != None: # keep state
+                self.pos_m[i] = self.target_m[i]
+                self.pos_w[i] = self.target_w[i]
+                
+        # re-add comment
+        self.line += self.comment
+                
+        #print("DONE", self.line, self.pos_m, self.pos_w, self.target)
+
+
+    def find_vars(self):
+        """
+        Parses all variables in a G-Code line (#1, #2, etc.) and populates
+        the internal `vars` dict with corresponding keys and values
+        """
+        
+        m = re.match(self._re_set_var, self.line)
+        if m:
+            key = m.group(1)
+            val = str(float(m.group(2))) # get rid of extra zeros
+            self.vars[key] = val
+            self.line = ";" + self.line
+        
+        # find variable usages
+        keys = re.findall(self._re_use_var, self.line)
+        for key in keys:
+            if not key in self.vars: self.vars[key] = None
+        
+        
+    def substitute_vars(self):
+        """
+        Substitute a variable with a value from the `vars` dict.
+        """
+        keys = re.findall(self._re_use_var, self.line)
+        
+        for key in keys:
+            val = None
+            if key in self.vars:
+                val = self.vars[key]
+            
+            if val == None:
+                self.line = ""
+                self.callback("on_var_undefined", key)
+                return self.line
+            else:
+                self.line = self.line.replace("#" + key, str(val))
+                self.logger.info("SUBSTITUED VAR #{} -> {}".format(key, val))
+    
+    
+    def scale_spindle(self):
+        if self.contains_spindle:
+            # strip the original S setting
+            self.line = re.sub(self._re_spindle_replace, "", self.line).strip()
+            self.line += "S{:d}".format(int(self.current_spindle_speed * self.spindle_factor))
+                
+        
+    def override_feed(self):
+        """
+        Call this method to
+        
+        * get a callback when the current command contains an F word
+        * 
+        """
+    
+        if self.do_feed_override == False and self.contains_feed:
+            # Notify parent app of detected feed in current line (useful for UIs)
+            if self.current_feed != self.feed_in_current_line:
+                self.callback("on_feed_change", self.feed_in_current_line)
+            self.current_feed = self.feed_in_current_line
+            
+            
+        if self.do_feed_override == True and self.request_feed:
+           
+            if self.contains_feed:
+                # strip the original F setting
+                self.logger.info("STRIPPING FEED: " + self.line)
+                self.line = re.sub(self._re_feed_replace, "", self.line).strip()
+
+            if (self.current_feed != self.request_feed):
+                self.line += "F{:0.1f}".format(self.request_feed)
+                self.current_feed = self.request_feed
+                self.logger.info("OVERRIDING FEED: " + str(self.current_feed))
+                self.callback("on_feed_change", self.current_feed)
+
+        
+
+    def transform_comments(self):
+        """
+        Comments in Gcode can be set with semicolon or parentheses.
+        This method transforms parentheses comments to semicolon comments.
+        """
+        
+        # transform () comment at end of line into semicolon comment
+        self.line = re.sub(self._re_comment_paren_convert, "\g<1>;\g<2>", self.line)
+        
+        # remove all in-line () comments
+        self.line = re.sub(self._re_comment_paren_replace, "", self.line)
+
+        m = re.match(self._re_comment_get_comment, self.line)
+        if m:
+            self.line = m.group(1)
+            self.comment = m.group(2)
+        else:
+            self.comment = ""
+
+
+    def _fractionize_circular_motion(self):
+        """
+        This function is a direct port of Grbl's C code into Python (gcode.c)
+        with slight refactoring for Python by Michael Franzl.
+        See https://github.com/grbl/grbl
+        
+        """
+        
+        # implies self.current_motion_mode == 2 or self.current_motion_mode == 3
+        
+        if self.current_plane_mode == "G17":
+            axis_0 = 0 # X axis
+            axis_1 = 1 # Y axis
+            axis_linear = 2 # Z axis
+        elif self.current_plane_mode == "G18":
+            axis_0 = 2 # Z axis
+            axis_1 = 0 # X axis
+            axis_linear = 1 # Y axis
+        elif self.current_plane_mode == "G19":
+            axis_0 = 1 # Y axis
+            axis_1 = 2 # Z axis
+            axis_linear = 0 # X axis
+            
+        is_clockwise_arc = True if self.current_motion_mode == 2 else False
+            
+        # deltas between target and (current) position
+        x = self.target_w[axis_0] - self.pos_w[axis_0]
+        y = self.target_w[axis_1] - self.pos_w[axis_1]
+        
+        if self.contains_radius:
+            # RADIUS MODE
+            # R given, no IJK given, self.offset must be calculated
+            
+            if tuple(self.target_w) == tuple(self.pos_w):
+                self.logger.error("Arc in Radius Mode: Identical start/end {}".format(self.line))
+                return [self.line]
+            
+            h_x2_div_d = 4.0 * self.radius * self.radius - x * x - y * y;
+            
+            if h_x2_div_d < 0:
+                self.logger.error("Arc in Radius Mode: Radius error {}".format(self.line))
+                return [self.line]
+
+            # Finish computing h_x2_div_d.
+            h_x2_div_d = -math.sqrt(h_x2_div_d) / math.sqrt(x * x + y * y);
+            
+            if not is_clockwise_arc:
+                h_x2_div_d = -h_x2_div_d
+    
+            if self.radius < 0:
+                h_x2_div_d = -h_x2_div_d; 
+                self.radius = -self.radius;
+                
+            self.offset[axis_0] = 0.5*(x-(y*h_x2_div_d))
+            self.offset[axis_1] = 0.5*(y+(x*h_x2_div_d))
+            
+        else:
+            # CENTER OFFSET MODE, no R given so must be calculated
+            
+            if self.offset[axis_0] == None or self.offset[axis_1] == None:
+                raise Exception("Arc in Offset Mode: No offsets in plane. {}".format(self.line))
+                #self.logger.error("Arc in Offset Mode: No offsets in plane")
+                #return [self.line]
+            
+            # Arc radius from center to target
+            x -= self.offset[axis_0]
+            y -= self.offset[axis_1]
+            target_r = math.sqrt(x * x + y * y)
+            
+            # Compute arc radius for mc_arc. Defined from current location to center.
+            self.radius = math.sqrt(self.offset[axis_0] * self.offset[axis_0] + self.offset[axis_1] * self.offset[axis_1])
+            
+            # Compute difference between current location and target radii for final error-checks.
+            delta_r = math.fabs(target_r - self.radius);
+            if delta_r > 0.005:
+                if delta_r > 0.5:
+                    raise Exception("Arc in Offset Mode: Invalid Target. r={:f} delta_r={:f} {}".format(self.radius, delta_r, self.line))
+                    #self.logger.warning("Arc in Offset Mode: Invalid Target. r={:f} delta_r={:f} {}".format(self.radius, delta_r, self.line))
+                    #return []
+                if delta_r > (0.001 * self.radius):
+                    raise Exception("Arc in Offset Mode: Invalid Target. r={:f} delta_r={:f} {}".format(self.radius, delta_r, self.line))
+                    #self.logger.warning("Arc in Offset Mode: Invalid Target. r={:f} delta_r={:f} {}".format(self.radius, delta_r, self.line))
+                    #return []
+        
+        #print(self.pos_m, self.target, self.offset, self.radius, axis_0, axis_1, axis_linear, is_clockwise_arc)
+        
+        #print("MCARC", self.line, self.pos_w, self.target_w, self.offset, self.radius, axis_0, axis_1, axis_linear, is_clockwise_arc)
+        
+        gcode_list = self._mc_arc(self.pos_w, self.target_w, self.offset, self.radius, axis_0, axis_1, axis_linear, is_clockwise_arc)
+        
+        return gcode_list
+        
+
+    def _mc_arc(self, position, target, offset, radius, axis_0, axis_1, axis_linear, is_clockwise_arc):
+        """
+        This function is a direct port of Grbl's C code into Python (motion_control.c)
+        with slight refactoring for Python by Michael Franzl.
+        See https://github.com/grbl/grbl
+        """
+        
+        gcode_list = []
+        gcode_list.append(";" + self.special_comment_prefix + ".arc_begin[{}]".format(self.line))
+        
+        do_restore_distance_mode = False
+        if self.current_distance_mode == "G91":
+            # it's bad to concatenate many small floating point segments due to accumulating errors
+            # each arc will use G90
+            do_restore_distance_mode = True
+            gcode_list.append("G90")
+        
+        center_axis0 = position[axis_0] + offset[axis_0]
+        center_axis1 = position[axis_1] + offset[axis_1]
+        # radius vector from center to current location
+        r_axis0 = -offset[axis_0]
+        r_axis1 = -offset[axis_1]
+        # radius vector from target to center
+        rt_axis0 = target[axis_0] - center_axis0
+        rt_axis1 = target[axis_1] - center_axis1
+        
+        angular_travel = math.atan2(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1)
+        
+        arc_tolerance = 0.004
+        arc_angular_travel_epsilon = 0.0000005
+        
+        if is_clockwise_arc: # Correct atan2 output per direction
+            if angular_travel >= -arc_angular_travel_epsilon: angular_travel -= 2*math.pi
+        else:
+            if angular_travel <= arc_angular_travel_epsilon: angular_travel += 2*math.pi
+            
+       
+            
+        segments = math.floor(math.fabs(0.5 * angular_travel * radius) / math.sqrt(arc_tolerance * (2 * radius - arc_tolerance)))
+        
+        #print("angular_travel:{:f}, radius:{:f}, arc_tolerance:{:f}, segments:{:d}".format(angular_travel, radius, arc_tolerance, segments))
+        
+        words = ["X", "Y", "Z"]
+        if segments:
+            theta_per_segment = angular_travel / segments
+            linear_per_segment = (target[axis_linear] - position[axis_linear]) / segments
+            
+            position_last = list(position)
+            for i in range(1, segments):
+                cos_Ti = math.cos(i * theta_per_segment);
+                sin_Ti = math.sin(i * theta_per_segment);
+                r_axis0 = -offset[axis_0] * cos_Ti + offset[axis_1] * sin_Ti;
+                r_axis1 = -offset[axis_0] * sin_Ti - offset[axis_1] * cos_Ti;
+            
+                position[axis_0] = center_axis0 + r_axis0;
+                position[axis_1] = center_axis1 + r_axis1;
+                position[axis_linear] += linear_per_segment;
+
+                gcodeline = ""
+                if i == 1:
+                    gcodeline += "G1"
+                    
+                for a in range(0,3):
+                    if position[a] != position_last[a]: # only write changes
+                        txt = "{}{:0.3f}".format(words[a], position[a])
+                        txt = txt.rstrip("0").rstrip(".")
+                        gcodeline += txt
+                        position_last[a] = position[a]
+                        
+                if i == 1:
+                    if self.contains_feed: gcodeline += "F{:.1f}".format(self.feed_in_current_line)
+                    if self.contains_spindle: gcodeline += "S{:d}".format(self.current_spindle_speed)
+                    
+                gcode_list.append(gcodeline)
+            
+            
+        
+        # make sure we arrive at target
+        gcodeline = ""
+        if segments <= 1:
+            gcodeline += "G1"
+        
+        for a in range(0,3):
+            if target[a] != position[a]:
+                txt = "{}{:0.3f}".format(words[a], target[a])
+                txt = txt.rstrip("0").rstrip(".")
+                gcodeline += txt
+           
+        if segments <= 1:
+            # no segments were rendered (very small arc) so we have to put S and F here
+            if self.contains_feed: gcodeline += "F{:.1f}".format(self.feed_in_current_line)
+            if self.contains_spindle: gcodeline += "S{:d}".format(self.current_spindle_speed)
+                
+        gcode_list.append(gcodeline)
+        
+        if do_restore_distance_mode == True:
+          gcode_list.append(self.current_distance_mode)
+        
+        gcode_list.append(";" + self.special_comment_prefix + ".arc_end")
+        
+        return gcode_list
+    
+        
+    def _fractionize_linear_motion(self):
+        gcode_list = []
+        gcode_list.append(";" + self.special_comment_prefix + ".line_begin[{}]".format(self.line))
+        
+        num_fractions = int(self.dist / self.fract_linear_segment_len)
+        
+        for k in range(0, num_fractions):
+            # render segments
+            txt = ""
+            if k == 0:
+                txt += "G1"
+                
+            for i in range(0, 3):
+                # loop over X, Y, Z axes
+                segment_length = self.dist_xyz[i] / num_fractions
+                coord_rel = (k + 1) * segment_length
+                if self.current_distance_mode == "G90":
+                    # absolute distances
+                    coord_abs = self.pos_w[i] + coord_rel
+                    if coord_rel != 0:
+                        # only output for changes
+                        txt += "{}{:0.3f}".format(self._axes_words[i], coord_abs)
+                        txt = txt.rstrip("0").rstrip(".")
+                else:
+                    # relative distances
+                    txt += "{}{:0.3f}".format(self._axes_words[i], segment_length)
+                    txt = txt.rstrip("0").rstrip(".")
+                
+            if k == 0:
+                if self.contains_feed: txt += "F{:.1f}".format(self.feed_in_current_line)
+                if self.contains_spindle: txt += "S{:d}".format(self.current_spindle_speed)
+                
+            
+            gcode_list.append(txt)
+        
+        gcode_list.append(";" + self.special_comment_prefix + ".line_end")
+        return gcode_list
+
+        
+
+
+    def _default_callback(self, status, *args):
+        print("PREPROCESSOR DEFAULT CALLBACK", status, args)

interface.py

@@ -0,0 +1,118 @@
+"""
+Gerbil - Copyright (c) 2015 Michael Franzl
+
+Permission is hereby granted, free of charge, to any person obtaining a
+copy of this software and associated documentation files (the "Software"),
+to deal in the Software without restriction, including without limitation
+the rights to use, copy, modify, merge, publish, distribute, sublicense,
+and/or sell copies of the Software, and to permit persons to whom the
+Software is furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included
+in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
+OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+OTHER DEALINGS IN THE SOFTWARE.
+"""
+
+import serial
+import time
+import threading
+import logging
+
+class Interface:
+    """Implements opening, closing, writing and threaded reading from the serial port. Read data are put into a Thread Queue.
+    """
+    
+    def __init__(self, name, path, baud=115200):
+        """Straightforward initialization tasks.
+        
+        @param name
+        An informal name of the instance. Useful if you are running
+        several instances to control several serial ports at once.
+        It is only used for logging output and UI messages.
+        
+        @param path
+        The serial port device node living under /dev.
+        e.g. /dev/ttyACM0 or /dev/ttyUSB0
+        
+        @param baud
+        The baud rate. Default is 115200 for Grbl > v0.9i.
+        """
+        
+        self.name = name
+        self.path = path
+        self.baud = baud
+        self.queue = None
+        self.logger = logging.getLogger("gerbil.interface")
+        
+        self._buf_receive = ""
+        self._do_receive = False
+        
+    def start(self, queue):
+        """
+        Open the device node and start a Thread for reading.
+        
+        @param queue
+        An instance of Python3's `Queue()` class.
+        """
+        self.queue = queue
+        
+        self.logger.info("%s: connecting to %s with baudrate %i", self.name, self.path, self.baud)
+        
+        self.serialport = serial.Serial(self.path, self.baud, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS, timeout=1, writeTimeout=0)
+        self.serialport.flushInput()
+        self.serialport.flushOutput()
+        self._do_receive = True
+        self.serial_thread = threading.Thread(target=self._receiving)
+        self.serial_thread.start()
+        
+    def stop(self):
+        """
+        Close the device node and shut down the reading Thread.
+        """
+        self._do_receive = False
+        self.logger.info("%s: stop()", self.name)
+        self.serial_thread.join()
+        self.logger.info("%s: JOINED thread", self.name)
+        self.logger.info("%s: Closing port", self.name)
+        self.serialport.flushInput()
+        self.serialport.flushOutput()
+        self.serialport.close()
+        
+    def write(self, data):
+        """
+        Write `data` to the device node. If data is empty, no write is performed. The number of written characters is returned.
+        """
+        if len(data) > 0:
+            num_written = self.serialport.write(bytes(data,"ascii"))
+            return num_written
+        else:
+            self.logger.debug("%s: nothing to write", self.name)
+
+    def _receiving(self):
+        while self._do_receive == True:
+            data = self.serialport.read(1)
+            waiting = self.serialport.inWaiting()
+            data += self.serialport.read(waiting)
+            self._handle_data(data)
+
+    def _handle_data(self, data):
+        try:
+            asci = data.decode("ascii")
+        except UnicodeDecodeError:
+            self.logger.info("%s: Received a non-ascii byte. Probably junk. Dropping it.", self.name)
+            asci = ""
+            
+        for i in range(0, len(asci)):
+            char = asci[i]
+            self._buf_receive += char
+            # not all received lines are complete (end with \n)
+            if char == "\n":
+                self.queue.put(self._buf_receive.strip())
+                self._buf_receive = ""

realWorldGcodeSender.py

@@ -8,6 +8,8 @@ import time
 import math
 from svgpathtools import Path, Line, QuadraticBezier, CubicBezier, Arc
 from svgpathtools import svg2paths, wsvg, svg2paths2, polyline
+#import matplotlib
+#matplotlib.use('GTK3Agg') 
 from matplotlib import pyplot as plt
 from matplotlib.widgets import TextBox
 from matplotlib.backend_bases import MouseButton
@@ -18,6 +20,11 @@ from pygcode.gcodes import MODAL_GROUP_MAP
 import re
 
 import sys
+#sys.path.insert(1, 'C:\\Git\\gerbil\\')
+#sys.path.insert(1, 'C:\\Git\\gcode_machine\\')
+from gerbil import Gerbil
+import serial.tools.list_ports
+
 
 class Point3D:
   def __init__(self, X, Y, Z = None):
@@ -186,11 +193,24 @@ def arcToPoints(startX, startY, endX, endY, i, j, clockWise, curZ):
     points.append(Point3D(x, y, curZ))
     return points
 
+def rotate(origin, point, angle):
+        """
+        Rotate a point counterclockwise by a given angle around a given origin.
+
+        The angle should be given in radians.
+        """
+        ox, oy = origin
+        px, py = point
+
+        qx = ox + math.cos(angle) * (px - ox) - math.sin(angle) * (py - oy)
+        qy = oy + math.sin(angle) * (px - ox) + math.cos(angle) * (py - oy)
+        return qx, qy
+
 ####################################################################################
 # OverlayGcode class
 ####################################################################################
 class OverlayGcode:
-    def __init__(self, cv2Overhead):
+    def __init__(self, cv2Overhead, gCodeFile):
         global bedViewSizePixels
         global bedSize
         
@@ -199,7 +219,7 @@ class OverlayGcode:
         self.xOffset = 0
         self.yOffset = 0
         self.rotation = 0
-        self.cv2Overhead = cv2Overhead
+        self.cv2Overhead = cv2.cvtColor(cv2Overhead, cv2.COLOR_BGR2RGB)
         self.move = False
 
         fig, ax = plt.subplots()
@@ -207,7 +227,7 @@ class OverlayGcode:
         plt.subplots_adjust(bottom=0.01, right = 0.99)
         plt.axis([self.bedViewSizePixels,0, self.bedViewSizePixels, 0])
         #Generate matplotlib plot from opencv image
-        self.matPlotImage = plt.imshow(cv2.cvtColor(self.cv2Overhead, cv2.COLOR_BGR2RGB))
+        self.matPlotImage = plt.imshow(self.cv2Overhead)
         ###############################################
         # Generate controls for plot
         ###############################################
@@ -237,13 +257,18 @@ class OverlayGcode:
 
         cid = fig.canvas.mpl_connect('button_press_event', self.onclick)
         cid = fig.canvas.mpl_connect('button_release_event', self.onrelease)
-        cid = fig.canvas.mpl_connect('motion_notify_event', self.onmove)
+        cid = fig.canvas.mpl_connect('motion_notify_event', self.onmousemove)
+        cid = fig.canvas.mpl_connect('key_press_event', self.onkeypress)
+
+        #Create object to handle controlling the CNC machine and sending the g codes to it
+        self.sender = GCodeSender(gCodeFile)
+        
 
         self.points = []
         self.laserPowers = []
         self.machine = Machine()
 
-        with open('test.nc', 'r') as fh:
+        with open(gCodeFile, 'r') as fh:
           for line_text in fh.readlines():
             line = pygcode.Line(line_text)
             prevPos = self.machine.pos
@@ -325,20 +350,7 @@ class OverlayGcode:
 
     def rotatePoints(self, points, origin, angle):
       for point in points:
-        point.X, point.Y = self.rotate(origin, [point.X, point.Y], angle)
-
-    def rotate(self, origin, point, angle):
-        """
-        Rotate a point counterclockwise by a given angle around a given origin.
-
-        The angle should be given in radians.
-        """
-        ox, oy = origin
-        px, py = point
-
-        qx = ox + math.cos(angle) * (px - ox) - math.sin(angle) * (py - oy)
-        qy = oy + math.sin(angle) * (px - ox) + math.cos(angle) * (py - oy)
-        return qx, qy
+        point.X, point.Y = rotate(origin, [point.X, point.Y], angle)
 
     def overlaySvg(self, image, xOff = 0, yOff = 0, rotation = 0):
       """
@@ -363,13 +375,13 @@ class OverlayGcode:
       for point, laserPower in zip(transformedPoints, self.laserPowers):
         newPoint = (int(point.X), int(point.Y))
         if prevPoint is not None:
-          cv2.line(overlay, prevPoint, newPoint, (int(laserPower * 255), 0, 0), 1)
+          cv2.line(overlay, prevPoint, newPoint, (int(laserPower * 255), 0, 0), 2)
         prevPoint = newPoint
       return overlay
 
     def updateOverlay(self):
         overlay = self.overlaySvg(self.cv2Overhead, self.xOffset, self.yOffset, self.rotation)
-        self.matPlotImage.set_data(cv2.cvtColor(overlay, cv2.COLOR_BGR2RGB))
+        self.matPlotImage.set_data(overlay)
         self.matPlotImage.figure.canvas.draw()
 
     def onUpdateXOffset(self, text):
@@ -390,8 +402,23 @@ class OverlayGcode:
       self.rotation = float(text)
       self.updateOverlay()
 
-    def onmove(self, event):
+    def onmousemove(self, event):
       self.move = True
+      self.mouseX = event.xdata
+      self.mouseY = event.ydata
+
+    def onkeypress(self, event):
+        if   event.key == 's':
+            self.sender.send_file(self.xOffset, self.yOffset, self.rotation)
+        elif event.key == 'h':
+            self.sender.home_machine()
+        elif event.key == 'z':
+            #Find X, Y, and Z position of the aluminum reference block on the work pice
+            #sepcify the X and Y estimated position of the reference block
+            self.sender.zero_on_workpice(self.refX, self.refY)
+        elif event.key == 'm':
+            self.sender.move_to(self.mouseX / self.bedViewSizePixels * self.bedSize.X \
+                              , self.mouseY / self.bedViewSizePixels * self.bedSize.Y)
 
     def onclick(self, event):
       self.move = False
@@ -570,6 +597,64 @@ def display_4_lines(pixels, frame, flip=False):
   cv2.line(frame, line2,line3,(0,255,255),3)
   cv2.line(frame, line3,line4,(0,255,255),3)
   cv2.line(frame, line4,line1,(0,255,255),3)
+
+class GCodeSender:
+    def __init__(self, gCodeFile):
+        self.grbl = Gerbil(self.gerbil_callback)
+        self.grbl.setup_logging()
+
+        ports = serial.tools.list_ports.comports()
+        for p in ports:
+            print(p.device)
+
+        self.grbl.cnect("/dev/ttyUSB0", 57600)
+        self.grbl.poll_start()
+        self.gCodeFile = gCodeFile
+
+
+
+    def gerbil_callback(self, eventstring, *data):
+        args = []
+        for d in data:
+            args.append(str(d))
+        print("GERBIL CALLBACK: event={} data={}".format(eventstring.ljust(30), ", ".join(args)))
+
+    def home_machine(self):
+        self.gerbil.send_imediately("$H\n")
+        pass
+
+    def zero_on_workpice(self, guessX, guessY):
+        pass
+
+    def move_to(self, x, y , z = None, feedRate = 100):
+        if z == None:
+            zStr = ""
+        else:
+            zStr = " Z" + str(z)
+        xStr = " X" + str(X)
+        yStr = " Y" + str(Y)
+        fStr = " F" + str(feedRate)
+        self.gerbil.send_immediately("G1" + xStr + yStr + zStr + fStr + "\n")
+
+    def send_file(self, xOffset, yOffset, rotation):
+        #Set to inches
+        self.gerbil.send_immediately("G20\n")
+
+        self.gerbil.send_immediately("G54 X" + str(xOffset) + " Y" + str(yOffset) + "\n")
+
+        deg = -rotation * 180 / math.pi
+        self.gerbil.send_immediately("G68 X0 Y0 R" + str(deg) + "\n")
+        #Set back to mm, typically the units g code assumes
+        self.gerbil.send_immediately("G21\n")
+
+        # Turn off rotated coordinate system
+        self.gerbil.send_immediately("G69\n")
+
+        with open(self.gCodeFile, 'r') as fh:
+            for line_text in fh.readlines():
+                self.gerbil.stream(line_text)
+
+
 #############################################################################
 # Main
 #############################################################################
@@ -653,13 +738,17 @@ gray = cv2.resize(frame, (1280, 700))
 cv2.imshow('image',gray)
 cv2.waitKey()
   
-#############################################################
-# Warp perspective to perpendicular to bed view
-#############################################################
+######################################################################
+# Warp perspective to perpendicular to bed view, create overlay calss
+######################################################################
+gCodeFile = 'test.nc'
 cv2Overhead = cv2.warpPerspective(frame, bedPixelToPhysicalLoc, (frame.shape[1], frame.shape[0]))
 cv2Overhead = cv2.resize(cv2Overhead, (bedViewSizePixels, bedViewSizePixels))
-GCodeOverlay = OverlayGcode(cv2Overhead)
+GCodeOverlay = OverlayGcode(cv2Overhead, gCodeFile)
 
+######################################################################
+# Create a G Code sender now that overlay is created
+######################################################################
 
 plt.show()