parser for circular interpolation

cnc/enums.py

@@ -2,16 +2,40 @@
 """
 
 
-class Plane(object):
+class Enum(object):
+    """ Base class for enums
+    """
+    __global_increment = 1
+
+    def __init__(self, for_str):
+        """ Initialize base class for enumerates.
+        :param for_str: return value for build in str() function
+        """
+        self.value = Enum.__global_increment
+        self._str = for_str
+        Enum.__global_increment += 1
+
+    def __eq__(self, other):
+        return self.value == other.value
+
+    def __str__(self):
+        return self._str
+
+
+class Plane(Enum):
     """ Enum for choosing plane for circular interpolation.
     """
-    PLANE_XY = 1
+    pass
-    PLANE_ZX = 2
+
-    PLANE_YZ = 3
+PLANE_XY = Plane("XY")
+PLANE_ZX = Plane("ZX")
+PLANE_YZ = Plane("YZ")
 
 
-class RotationDirection(object):
+class RotationDirection(Enum):
     """ Enum for choosing rotation direction.
     """
-    CW = 1
+    pass
-    CCW = 2
+
+CW = RotationDirection("CW")
+CCW = RotationDirection("CCW")

cnc/gmachine.py

@@ -1,9 +1,11 @@
 from __future__ import division
 import time
 import logging
+import math
 
 from cnc import hal
 from cnc.coordinates import Coordinates
+from cnc.enums import *
 from cnc.config import *
 
 
@@ -28,6 +30,8 @@ class GMachine(object):
         self._local = None
         self._convertCoordinates = 0
         self._absoluteCoordinates = 0
+        self._plane = None
+        self._radius = None
         self.reset()
         hal.init()
 
@@ -47,40 +51,139 @@ class GMachine(object):
         self._local = Coordinates(0.0, 0.0, 0.0)
         self._convertCoordinates = 1.0
         self._absoluteCoordinates = True
+        self._plane = PLANE_XY
+        self._radius = Coordinates(0.0, 0.0, 0.0)
 
     def _spindle(self, spindle_speed):
         hal.join()
         hal.spindle_control(100.0 * spindle_speed / SPINDLE_MAX_RPM)
 
-    def _move(self, delta, velocity):
+    def __check_delta(self, delta):
+        pos = self._position + delta
+        if not pos.is_in_aabb(Coordinates(0.0, 0.0, 0.0),
+               Coordinates(TABLE_SIZE_X_MM, TABLE_SIZE_Y_MM, TABLE_SIZE_Z_MM)):
+            raise GMachineException("out of effective area")
+
+    def _move_linear(self, delta, velocity):
         delta = delta.round(1.0 / STEPPER_PULSES_PER_MM)
         if delta.is_zero():
             return
-        np = self._position + delta
+        self.__check_delta(delta)
-        if not np.is_in_aabb(Coordinates(0.0, 0.0, 0.0),
-                             Coordinates(TABLE_SIZE_X_MM, TABLE_SIZE_Y_MM, TABLE_SIZE_Z_MM)):
-            raise GMachineException("out of effective area")
         hal.move_linear(delta, velocity)
         # save position
-        self._position = np
+        self._position = self._position + delta
+
+    def __quarter(self, pa, pb):
+        if pa >= 0 and pb >= 0:
+            return 1
+        if pa < 0 and pb >= 0:
+            return 2
+        if pa < 0 and pb < 0:
+            return 3
+        if pa >= 0 and pb < 0:
+            return 4
+
+    def __adjust_circle(self, da, db, ra, rb, dir, pa, pb, ma, mb):
+        r = math.sqrt(ra * ra + rb * rb)
+        if r == 0:
+            raise GMachineException("circle radius is zero")
+        l = math.sqrt(da * da + db * db)
+        sq = self.__quarter(-ra, -rb)
+        if l == 0:  # full circle
+            ea = da
+            eb = db
+            eq = 5  # mark as non-existing to check all
+        else:
+            ea = da / l * r + ra
+            eb = db / l * r + rb
+            eq = self.__quarter(ea - ra, eb - rb)
+        # iterate coordinates quarters and check if we fit table
+        q = sq
+        pq = q
+        for _ in range(0, 4):
+            if dir == CW:
+                q -= 1
+            else:
+                q += 1
+            if q <= 0:
+                q = 4
+            elif q >= 5:
+                q = 1
+            if q == eq:
+                break
+            is_raise = False
+            if (pq == 1 and q == 4) or (pq == 4 and q == 1):
+                is_raise = (pa + ra  + r > ma)
+            elif (pq == 1 and q == 2) or (pq == 2 and q == 1):
+                is_raise = (pb + rb + r > mb)
+            elif (pq == 2 and q == 3) or (pq == 3 and q == 2):
+                is_raise = (pa + ra - r < 0)
+            elif (pq == 3 and q == 4) or (pq == 4 and q == 3):
+                is_raise = (pb + rb - r < 0)
+            if is_raise:
+                raise GMachineException("out of effective area")
+            pq = q
+        return ea, eb
+
+    def _circular(self, delta, radius, velocity, direction):
+        delta = delta.round(1.0 / STEPPER_PULSES_PER_MM)
+        self.__check_delta(delta)
+        # get delta vector and put it on circle
+        circle_end = Coordinates(0,0,0)
+        if self._plane == PLANE_XY:
+            circle_end.x, circle_end.y = self.__adjust_circle(delta.x, delta.y,
+                                          radius.x, radius.y, direction,
+                                          self._position.x, self._position.y,
+                                          TABLE_SIZE_X_MM, TABLE_SIZE_Y_MM)
+            circle_end.z = delta.z
+        elif self._plane == PLANE_YZ:
+            circle_end.y, circle_end.z = self.__adjust_circle(delta.y, delta.z,
+                                          radius.y, radius.z, direction,
+                                          self._position.y, self._position.z,
+                                          TABLE_SIZE_Y_MM, TABLE_SIZE_Z_MM)
+            circle_end.x = delta.x
+        elif self._plane == PLANE_ZX:
+            circle_end.z, circle_end.x = self.__adjust_circle(delta.z, delta.x,
+                                          radius.z, radius.x, direction,
+                                          self._position.z, self._position.x,
+                                          TABLE_SIZE_Z_MM, TABLE_SIZE_X_MM)
+            circle_end.y = delta.y
+        circle_end = circle_end.round(1.0 / STEPPER_PULSES_PER_MM)
+        hal.move_circular(circle_end, radius, self._plane, velocity, direction)
+        # if finish coords is not on circle, move some distance linearly
+        linear_delta = delta - circle_end
+        if not linear_delta.is_zero():
+            logging.info("Moving additionally {} to finish circle command".
+                         format(linear_delta))
+            hal.move_linear(linear_delta, velocity)
+        # save position
+        self._position = self._position + circle_end + linear_delta
 
     def home(self):
         """ Move head to park position
         """
         d = Coordinates(0, 0, -self._position.z)
-        self._move(d, STEPPER_MAX_VELOCITY_MM_PER_MIN)
+        self._move_linear(d, STEPPER_MAX_VELOCITY_MM_PER_MIN)
         d = Coordinates(-self._position.x, -self._position.y, 0)
-        self._move(d, STEPPER_MAX_VELOCITY_MM_PER_MIN)
+        self._move_linear(d, STEPPER_MAX_VELOCITY_MM_PER_MIN)
 
     def position(self):
         """ Return current machine position (after the latest command)
             Note that hal might still be moving motors and in this case
             function will block until motors stops.
             This function for tests only.
+            :return current position.
         """
         hal.join()
         return self._position
 
+    def plane(self):
+        """ Return current plane for circular interpolation. This function for
+            tests only.
+            :return current plane.
+        """
+        return self._plane
+
     def do_command(self, gcode):
         """ Perform action.
         :param gcode: GCode object which represent one gcode line
@@ -103,6 +206,7 @@ class GMachine(object):
         velocity = gcode.get('F', self._velocity)
         spindle_rpm = gcode.get('S', self._spindle_rpm)
         pause = gcode.get('P', self._pause)
+        radius = gcode.radius(self._radius, self._convertCoordinates)
         # check parameters
         if velocity <= 0 or velocity > STEPPER_MAX_VELOCITY_MM_PER_MIN:
             raise GMachineException("bad feed speed")
@@ -112,12 +216,22 @@ class GMachine(object):
             raise GMachineException("bad delay")
         # select command and run it
         if c == 'G0':  # rapid move
-            self._move(delta, STEPPER_MAX_VELOCITY_MM_PER_MIN)
+            self._move_linear(delta, STEPPER_MAX_VELOCITY_MM_PER_MIN)
         elif c == 'G1':  # linear interpolation
-            self._move(delta, velocity)
+            self._move_linear(delta, velocity)
+        elif c == 'G2':  # circular interpolation, clockwise
+            self._circular(delta, radius, velocity, CW)
+        elif c == 'G3':  # circular interpolation, counterclockwise
+            self._circular(delta, radius, velocity, CCW)
         elif c == 'G4':  # delay in s
             hal.join()
             time.sleep(pause)
+        elif c == 'G17':  # XY plane select
+            self._plane = PLANE_XY
+        elif c == 'G18':  # ZX plane select
+            self._plane = PLANE_ZX
+        elif c == 'G19':  # YZ plane select
+            self._plane = PLANE_YZ
         elif c == 'G20':  # switch to inches
             self._convertCoordinates = 25.4
         elif c == 'G21':  # switch to mm
@@ -150,5 +264,6 @@ class GMachine(object):
         self._velocity = velocity
         self._spindle_rpm = spindle_rpm
         self._pause = pause
+        self._radius = radius
         logging.debug("position {}, {}, {}".format(
             self._position.x, self._position.y, self._position.z))

cnc/hal.py

@@ -25,11 +25,24 @@
 #        do_something()
 #
 #
+#    def move_circular(delta, radius, plane, velocity, direction):
+#        """ Move with circular interpolation.
+#        :param delta: finish position delta from the beginning, must be on
+#                      circle on specified plane. Zero means full circle.
+#        :param radius: vector to center of circle.
+#        :param plane: plane to interpolate.
+#        :param velocity: velocity in mm per min.
+#        :param direction: clockwise or counterclockwise.
+#        """
+#        do_something()
+#
+#
 #    def join():
 #        """ Wait till motors work.
 #        """
 #        do_something()
 #
+#
 #    def deinit():
 #        """ De-initialise hal, stop any hardware.
 #        """
@@ -51,6 +64,8 @@ if 'spindle_control' not in locals():
     raise NotImplementedError("hal.spindle_control() not implemented")
 if 'move_linear' not in locals():
     raise NotImplementedError("hal.move_linear() not implemented")
+if 'move_circular' not in locals():
+    raise NotImplementedError("hal.move_circular() not implemented")
 if 'join' not in locals():
     raise NotImplementedError("hal.join() not implemented")
 if 'deinit' not in locals():

cnc/hal_raspberry/hal.py

@@ -90,7 +90,7 @@ def init():
 
 def spindle_control(percent):
     """ Spindle control implementation.
-    :param percent: Spindle speed in percent. If 0, stop the spindle.
+    :param percent: spindle speed in percent. If 0, stop the spindle.
     """
     logging.info("spindle control: {}%".format(percent))
     if percent > 0:
@@ -101,7 +101,7 @@ def spindle_control(percent):
 
 def move_linear(delta, velocity):
     """ Move head to specified position
-    :param delta: Coordinated object, delta position in mm
+    :param delta: coordinated object, delta position in mm
     :param velocity: velocity in mm per min
     """
     logging.info("move {} with velocity {}".format(delta, velocity))
@@ -166,6 +166,20 @@ def move_linear(delta, velocity):
                  + str(round(generator.total_time_s(), 2)) + "s")
 
 
+def move_circular(delta, radius, plane, velocity, direction):
+    """ Move with circular interpolation.
+    :param delta: finish position delta from the beginning, must be on
+                  circle on specified plane. Zero means full circle.
+    :param radius: vector to center of circle.
+    :param plane: plane to interpolate.
+    :param velocity: velocity in mm per min.
+    :param direction: clockwise or counterclockwise.
+    """
+    logging.info("TODO move_circular {} {} {} with radius {} and velocity {}".
+                 format(plane, delta, direction, radius, velocity))
+    # TODO
+
+
 def join():
     """ Wait till motors work.
     """

cnc/hal_virtual.py

@@ -85,6 +85,20 @@ def move_linear(delta, velocity):
                  + "s, estimated " + str(round(generator.total_time_s(), 2)) + "s")
 
 
+def move_circular(delta, radius, plane, velocity, direction):
+    """ Move with circular interpolation.
+    :param delta: finish position delta from the beginning, must be on
+                  circle on specified plane. Zero means full circle.
+    :param radius: vector to center of circle.
+    :param plane: plane to interpolate.
+    :param velocity: velocity in mm per min.
+    :param direction: clockwise or counterclockwise.
+    """
+    logging.info("TODO move_circular {} {} {} with radius {} and velocity {}".
+                 format(plane, delta, direction, radius, velocity))
+    # TODO
+
+
 def join():
     """ Wait till motors work.
     """

runtests.sh

@@ -19,6 +19,7 @@ if ! which $app &> /dev/null; then
     app="./pycnc"
 fi
 res="$($app tests/rects.gcode 2>&1)"
+res="$res$($app tests/circles.gcode 2>&1)"
 res="$res$($app tests/test_parser.gcode 2>&1)"
 if echo "$res" | grep -q -i error; then
   echo "FAILED"

tests/circles.gcode

@@ -1,3 +1,4 @@
+G17
 G0 X90 Y90
 G1 Z10
 f1800
@@ -9,4 +10,7 @@ G2 X90 Y90 I-10 J10 ; three quoter circle
 G3 X90 Y90 Z 20 I-10 J-10 ; spiral
 G2 X92.07 Y85 I-5 J-5 ; small arc
 G2 X90 Y90 I-7.07 J0; more then 270 degree arc
-
+G18
+G2 X90 Y90 K-5
+G19
+G2 X90 Y90 K-5

tests/test_gcode.py

@@ -2,7 +2,6 @@ import unittest
 import math
 
 from cnc.coordinates import *
-from cnc.enums import *
 from cnc.gcode import *
 
 

tests/test_gmachine.py

@@ -69,6 +69,37 @@ class TestGMachine(unittest.TestCase):
         self.assertRaises(GMachineException,
                           m.do_command, GCode.parse_line("G1X0Y0Z-1"))
 
+    def test_g2_g3(self):
+        m = GMachine()
+        self.assertRaises(GMachineException,
+                          m.do_command, GCode.parse_line("G3I1J1F-1"))
+        m.do_command(GCode.parse_line("G19"))
+        self.assertRaises(GMachineException,
+                          m.do_command, GCode.parse_line("G3I1J0K0"))
+        m.do_command(GCode.parse_line("G18"))
+        self.assertRaises(GMachineException,
+                          m.do_command, GCode.parse_line("G3I0J1K0"))
+        m.do_command(GCode.parse_line("G17"))
+        self.assertRaises(GMachineException,
+                          m.do_command, GCode.parse_line("G3I0J0K1"))
+        self.assertRaises(GMachineException,
+                          m.do_command, GCode.parse_line("G2X99999999Y99999999I1J1"))
+        self.assertRaises(GMachineException,
+                          m.do_command, GCode.parse_line("G2X2Y2Z99999999I1J1"))
+        self.assertEqual(m.position(), Coordinates(0, 0, 0))
+        self.assertRaises(GMachineException,
+                          m.do_command, GCode.parse_line("G2X4Y4I2J2"))
+        self.assertRaises(GMachineException,
+                          m.do_command, GCode.parse_line("G3X4Y4I2J2"))
+        m.do_command(GCode.parse_line("G1X1"))
+        m.do_command(GCode.parse_line("G2J1"))
+        m.do_command(GCode.parse_line("G3J1"))
+        self.assertEqual(m.position(), Coordinates(1, 0, 0))
+        m.do_command(GCode.parse_line("G1X5Y5"))
+        m.do_command(GCode.parse_line("G2X0Y0Z5I-2J-2"))
+        self.assertEqual(m.position(), Coordinates(0, 0, 5))
+
+
     def test_g4(self):
         m = GMachine()
         st = time.time()
@@ -77,6 +108,15 @@ class TestGMachine(unittest.TestCase):
         self.assertRaises(GMachineException,
                           m.do_command, GCode.parse_line("G4P-0.5"))
 
+    def test_g17_g18_g19(self):
+        m = GMachine()
+        m.do_command(GCode.parse_line("G19"))
+        self.assertEqual(m.plane(), PLANE_YZ)
+        m.do_command(GCode.parse_line("G18"))
+        self.assertEqual(m.plane(), PLANE_ZX)
+        m.do_command(GCode.parse_line("G17"))
+        self.assertEqual(m.plane(), PLANE_XY)
+
     def test_g20_g21(self):
         m = GMachine()
         m.do_command(GCode.parse_line("G20"))