circular interpolation

README.md

@@ -31,8 +31,10 @@ perfect choice for easy development of this project.
 Video demo - [YouTube video](https://youtu.be/vcedo59raS4)
 
 # Current command support
-G0, G1, G4, G20, G21, G28, G53, G90, G91, G92, M2, M3, M5, M30  
-Commands can be easily added, see [gmachine.py](./cnc/gmachine.py) file.
+G0, G1, G2, G3, G4, G17, G18, G19, G20, G21, G28, G53, G90, G91, G92, M2, M3,
+M5, M30  
+Commands can be easily added, see [gmachine.py](./cnc/gmachine.py) file.  
+Four axis are supported - X, Y, Z, E
 
 # Config
 All configs are stored in [config.py](./cnc/config.py) and contain hardware

cnc/gmachine.py

@@ -7,6 +7,7 @@ from cnc import hal
 from cnc.coordinates import Coordinates
 from cnc.enums import *
 from cnc.config import *
+from cnc.pulses import *
 
 
 class GMachineException(Exception):
@@ -31,7 +32,6 @@ class GMachine(object):
         self._convertCoordinates = 0
         self._absoluteCoordinates = 0
         self._plane = None
-        self._radius = None
         self.reset()
         hal.init()
 
@@ -52,7 +52,6 @@ class GMachine(object):
         self._convertCoordinates = 1.0
         self._absoluteCoordinates = True
         self._plane = PLANE_XY
-        self._radius = Coordinates(0.0, 0.0, 0.0, 0.0)
 
     def _spindle(self, spindle_speed):
         hal.join()
@@ -72,7 +71,10 @@ class GMachine(object):
         if delta.is_zero():
             return
         self.__check_delta(delta)
-        hal.move_linear(delta, velocity)
+
+        logging.info("Moving linearly {}".format(delta))
+        gen = PulseGeneratorLinear(delta, velocity)
+        hal.move(gen)
         # save position
         self._position = self._position + delta
 
@@ -90,16 +92,18 @@ class GMachine(object):
         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
+        if da == 0 and db == 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)
+            b = (db - rb) / (da - ra)
+            ea = math.copysign(math.sqrt(r * r / (1.0 + abs(b))), da - ra)
+            eb = math.copysign(math.sqrt(r * r - ea * ea), db - rb)
+            eq = self.__quarter(ea, eb)
+            ea += ra
+            eb += rb
         # iterate coordinates quarters and check if we fit table
         q = sq
         pq = q
@@ -133,6 +137,10 @@ class GMachine(object):
                             1.0 / STEPPER_PULSES_PER_MM_Y,
                             1.0 / STEPPER_PULSES_PER_MM_Z,
                             1.0 / STEPPER_PULSES_PER_MM_E)
+        radius = radius.round(1.0 / STEPPER_PULSES_PER_MM_X,
+                              1.0 / STEPPER_PULSES_PER_MM_Y,
+                              1.0 / STEPPER_PULSES_PER_MM_Z,
+                              1.0 / STEPPER_PULSES_PER_MM_E)
         self.__check_delta(delta)
         # get delta vector and put it on circle
         circle_end = Coordinates(0, 0, 0, 0)
@@ -159,13 +167,19 @@ class GMachine(object):
                                       1.0 / STEPPER_PULSES_PER_MM_Y,
                                       1.0 / STEPPER_PULSES_PER_MM_Z,
                                       1.0 / STEPPER_PULSES_PER_MM_E)
-        hal.move_circular(circle_end, radius, self._plane, velocity, direction)
+        logging.info("Moving circularly {} {} {} with radius {}"
+                     " and velocity {}".
+                     format(self._plane, circle_end, direction, radius, velocity))
+        gen = PulseGeneratorCircular(circle_end, radius, self._plane, direction,
+                                     velocity)
+        hal.move(gen)
         # 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)
+            gen = PulseGeneratorLinear(linear_delta, velocity)
+            hal.move(gen)
         # save position
         self._position = self._position + circle_end + linear_delta
 
@@ -217,7 +231,8 @@ 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)
+        radius = gcode.radius(Coordinates(0.0, 0.0, 0.0, 0.0),
+                              self._convertCoordinates)
         # check parameters
         if velocity <= 0 or velocity > STEPPER_MAX_VELOCITY_MM_PER_MIN:
             raise GMachineException("bad feed speed")
@@ -275,5 +290,4 @@ class GMachine(object):
         self._velocity = velocity
         self._spindle_rpm = spindle_rpm
         self._pause = pause
-        self._radius = radius
         logging.debug("position {}".format(self._position))

cnc/hal.py

@@ -17,22 +17,9 @@
 #        do_something()
 #
 #
-#    def move_linear(delta, velocity):
-#        """ Move head to specified distance with specified speed.
-#        :param delta: Coordinated object, delta position in mm
-#        :param velocity: velocity in mm per min
-#        """
-#        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.
+#    def move(generator):
+#        """ Move head to according pulses in PulseGenerator.
+#        :param generator: PulseGenerator object
 #        """
 #        do_something()
 #
@@ -62,12 +49,9 @@ if 'init' not in locals():
     raise NotImplementedError("hal.init() not implemented")
 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 'move' not in locals():
+    raise NotImplementedError("hal.move() not implemented")
 if 'join' not in locals():
     raise NotImplementedError("hal.join() not implemented")
 if 'deinit' not in locals():
     raise NotImplementedError("hal.deinit() not implemented")
-

cnc/hal_raspberry/hal.py

@@ -108,14 +108,10 @@ def spindle_control(percent):
         pwm.remove_pin(SPINDLE_PWM_PIN)
 
 
-def move_linear(delta, velocity):
+def move(generator):
     """ Move head to specified position
-    :param delta: coordinated object, delta position in mm
-    :param velocity: velocity in mm per min
+    :param generator: PulseGenerator object.
     """
-    logging.info("move {} with velocity {}".format(delta, velocity))
-    # initialize generator
-    generator = PulseGeneratorLinear(delta, velocity)
     # wait if previous command still works
     while dma.is_active():
         time.sleep(0.001)
@@ -124,6 +120,7 @@ def move_linear(delta, velocity):
     dma.clear()
     prev = 0
     is_ran = False
+    instant = INSTANT_RUN
     st = time.time()
     for dir, tx, ty, tz, te in generator:
         if dir:  # set up directions
@@ -166,14 +163,15 @@ def move_linear(delta, velocity):
         # matter for pulses with 1-2us length.
         prev = k + STEPPER_PULSE_LINGTH_US
         # instant run handling
-        if not is_ran and INSTANT_RUN:
+        if not is_ran and instant:
             if k > 500000:  # wait at least 500 ms is uploaded
                 if time.time() - st > 0.5:
-                    # may be instant run should be canceled here?
                     logging.warn("Buffer preparing for instant run took more "
                                  "time then buffer time")
-                dma.run_stream()
-                is_ran = True
+                    instant = False
+                else:
+                    dma.run_stream()
+                    is_ran = True
     pt = time.time()
     if not is_ran:
         dma.run(False)
@@ -184,20 +182,6 @@ 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

@@ -2,7 +2,7 @@ from __future__ import division
 import logging
 import time
 
-from cnc.pulses import PulseGeneratorLinear
+from cnc.pulses import PulseGeneratorLinear, PulseGeneratorCircular
 from cnc.config import *
 from cnc.coordinates import Coordinates
 
@@ -25,36 +25,39 @@ def spindle_control(percent):
     logging.info("spindle control: {}%".format(percent))
 
 
-def move_linear(delta, velocity):
-    """ Move head to specified position
-    :param delta: Coordinated object, delta position in mm
-    :param velocity: velocity in mm per min
+def move(generator):
+    """ Move head to specified position.
+    :param generator: PulseGenerator object.
     """
-    logging.info("move {} with velocity {}".format(delta, velocity))
+    delta = generator.delta()
     ix = iy = iz = ie = 0
-    generator = PulseGeneratorLinear(delta, velocity)
     lx, ly, lz, le = None, None, None, None
     dx, dy, dz, de = 0, 0, 0, 0
     mx, my, mz, me = 0, 0, 0, 0
+    cx, cy, cz, ce = 0, 0, 0, 0
+    dirx, diry, dirz, dire = 1, 1, 1, 1
     st = time.time()
     direction_found = False
     for dir, tx, ty, tz, te in generator:
         if dir:
             direction_found = True
-            assert (tx < 0 and delta.x < 0) or (tx > 0 and delta.x > 0) \
-                   or delta.x == 0
-            assert (ty < 0 and delta.y < 0) or (ty > 0 and delta.y > 0) \
-                   or delta.y == 0
-            assert (tz < 0 and delta.z < 0) or (tz > 0 and delta.z > 0) \
-                   or delta.z == 0
-            assert (te < 0 and delta.e < 0) or (te > 0 and delta.e > 0) \
-                   or delta.e == 0
+            dirx, diry, dirz, dire = tx, ty, tz, te
+            if isinstance(generator, PulseGeneratorLinear):
+                assert (tx < 0 and delta.x < 0) or (tx > 0 and delta.x > 0) \
+                       or delta.x == 0
+                assert (ty < 0 and delta.y < 0) or (ty > 0 and delta.y > 0) \
+                       or delta.y == 0
+                assert (tz < 0 and delta.z < 0) or (tz > 0 and delta.z > 0) \
+                       or delta.z == 0
+                assert (te < 0 and delta.e < 0) or (te > 0 and delta.e > 0) \
+                       or delta.e == 0
             continue
         if tx is not None:
             if tx > mx:
                 mx = tx
             tx = int(round(tx * 1000000))
-            ix += 1
+            ix += dirx
+            cx += 1
             if lx is not None:
                 dx = tx - lx
                 assert dx > 0, "negative or zero time delta detected for x"
@@ -65,7 +68,8 @@ def move_linear(delta, velocity):
             if ty > my:
                 my = ty
             ty = int(round(ty * 1000000))
-            iy += 1
+            iy += diry
+            cy += 1
             if ly is not None:
                 dy = ty - ly
                 assert dy > 0, "negative or zero time delta detected for y"
@@ -76,7 +80,8 @@ def move_linear(delta, velocity):
             if tz > mz:
                 mz = tz
             tz = int(round(tz * 1000000))
-            iz += 1
+            iz += dirz
+            cz += 1
             if lz is not None:
                 dz = tz - lz
                 assert dz > 0, "negative or zero time delta detected for z"
@@ -87,7 +92,8 @@ def move_linear(delta, velocity):
             if te > me:
                 me = te
             te = int(round(te * 1000000))
-            ie += 1
+            ie += dire
+            ce += 1
             if le is not None:
                 de = te - le
                 assert de > 0, "negative or zero time delta detected for e"
@@ -100,30 +106,16 @@ def move_linear(delta, velocity):
         assert f.count(f[0]) == len(f), "fast forwarded pulse detected"
     pt = time.time()
     assert direction_found, "direction not found"
-    assert ix / STEPPER_PULSES_PER_MM_X == abs(delta.x), "x wrong number of pulses"
-    assert iy / STEPPER_PULSES_PER_MM_Y == abs(delta.y), "y wrong number of pulses"
-    assert iz / STEPPER_PULSES_PER_MM_Z == abs(delta.z), "z wrong number of pulses"
-    assert ie / STEPPER_PULSES_PER_MM_E == abs(delta.e), "e wrong number of pulses"
+    assert ix / STEPPER_PULSES_PER_MM_X == delta.x, "x wrong number of pulses"
+    assert iy / STEPPER_PULSES_PER_MM_Y == delta.y, "y wrong number of pulses"
+    assert iz / STEPPER_PULSES_PER_MM_Z == delta.z, "z wrong number of pulses"
+    assert ie / STEPPER_PULSES_PER_MM_E == delta.e, "e wrong number of pulses"
     assert max(mx, my, mz, me) <= generator.total_time_s(), "interpolation time or pulses wrong"
-    logging.debug("Did {}, {}, {}, {} iterations".format(ix, iy, iz, ie))
+    logging.debug("Moved {}, {}, {}, {} iterations".format(ix, iy, iz, ie))
     logging.info("prepared in " + str(round(pt - st, 2)) \
                  + "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.
     """

cnc/pulses.py

@@ -3,9 +3,10 @@ import math
 import logging
 
 from cnc.config import *
+from cnc.enums import *
 from cnc.coordinates import Coordinates
 
-SECONDS_IN_MINUTE = 60
+SECONDS_IN_MINUTE = 60.0
 
 
 class PulseGenerator(object):
@@ -33,10 +34,11 @@ class PulseGenerator(object):
         It's not implemented yet.
     """
 
-    def __init__(self):
+    def __init__(self, delta):
         """ Create object. Do not create directly this object, inherit this class
             and implement interpolation function and related methods.
             All child have to call this method ( super().__init__() ).
+            :param delta: overall movement delta in mm, uses for debug purpose.
         """
         self._iteration_x = 0
         self._iteration_y = 0
@@ -46,20 +48,21 @@ class PulseGenerator(object):
         self._acceleration_time_s = 0.0
         self._linear_time_s = 0.0
         self._2Vmax_per_a = 0.0
+        self._delta = delta
 
     def _get_movement_parameters(self):
-        """ Get for interpolation. This method have to be reimplemented
-            in parent classes and should calculate 3 parameters.
+        """ Get parameters for interpolation. This method have to be
+            reimplemented in parent classes and should calculate 3 parameters.
         :return: Tuple of three values:
                 acceleration_time_s: time for accelerating and breaking motors
                                      during movement
                 linear_time_s: time for uniform movement, it is total movement
                                time minus acceleration and braking time
-                max_axis_velocity_mm_per_sec: maximum velocity of any of axis
-                                              during movement. Even if whole
-                                              movement is accelerated, this
-                                              value should be calculated as top
-                                              velocity.
+                max_axis_velocity_mm_per_sec: maximum axis velocity of all
+                                              axises during movement. Even if
+                                              whole movement is accelerated,
+                                              this value should be calculated
+                                              as top velocity.
         """
         raise NotImplemented
 
@@ -121,9 +124,12 @@ class PulseGenerator(object):
         # Vmax * Tpseudo = Vlinear * t - a * t^2 / 2
         # V on start braking is Vlinear = Taccel * a = Tbreaking * a
         # Vmax * Tpseudo = Tbreaking * a * t - a * t^2 / 2
-        return 2.0 * self._acceleration_time_s + self._linear_time_s \
-               - math.sqrt(self._acceleration_time_s ** 2
-                           - self._2Vmax_per_a * bt)
+        d = self._acceleration_time_s ** 2 - self._2Vmax_per_a * bt
+        if d > 0:
+            d = math.sqrt(d)
+        else:
+            d = 0
+        return 2.0 * self._acceleration_time_s + self._linear_time_s - d
 
     def __next__(self):
         # for python3
@@ -193,6 +199,12 @@ class PulseGenerator(object):
         acceleration_time_s, linear_time_s, _ = self._get_movement_parameters()
         return acceleration_time_s * 2.0 + linear_time_s
 
+    def delta(self):
+        """ Get overall movement distance.
+        :return: Movement distance for each axis in millimeters.
+        """
+        return self._delta
+
 
 class PulseGeneratorLinear(PulseGenerator):
     def __init__(self, delta_mm, velocity_mm_per_min):
@@ -200,7 +212,7 @@ class PulseGeneratorLinear(PulseGenerator):
         :param delta_mm: movement distance of each axis.
         :param velocity_mm_per_min: desired velocity.
         """
-        super(PulseGeneratorLinear, self).__init__()
+        super(PulseGeneratorLinear, self).__init__(delta_mm)
         # this class doesn't care about direction
         self._distance_mm = abs(delta_mm)
         # velocity of each axis
@@ -231,7 +243,7 @@ class PulseGeneratorLinear(PulseGenerator):
         self._direction = math.copysign(1, delta_mm.x), \
                           math.copysign(1, delta_mm.y), \
                           math.copysign(1, delta_mm.z), \
-                          math.copysign(1, delta_mm.e) \
+                          math.copysign(1, delta_mm.e)
 
     def _get_movement_parameters(self):
         """ Return movement parameters, see super class for details.
@@ -262,3 +274,286 @@ class PulseGeneratorLinear(PulseGenerator):
         t_e = self.__linear(ie / STEPPER_PULSES_PER_MM_E, self._distance_mm.e,
                             self.max_velocity_mm_per_sec.e)
         return self._direction, (t_x, t_y, t_z, t_e)
+
+
+class PulseGeneratorCircular(PulseGenerator):
+    def __init__(self, delta, radius, plane, direction, velocity):
+        """ Create pulse generator for circular interpolation.
+            Position calculates based on formulas:
+            R^2 = x^2 + y^2
+            x = R * sin(phi)
+            y = R * cos(phi)
+            phi = omega * t,   2 * pi / omega = 2 * pi * R / V
+            phi = V * t / R
+            omega is angular_velocity.
+            so t = V / R * phi
+            phi can be calculated based on steps position.
+            Each axis can calculate circle phi base on iteration number, the
+            only one difference, that there is four quarters of circle and
+            signs for movement and solving expressions are different. So
+            we use additional variables to control it.
+            :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 direction: clockwise or counterclockwise.
+            :param velocity: velocity in mm per min.
+        """
+        super(PulseGeneratorCircular, self).__init__(delta)
+        self._plane = plane
+        self._direction = direction
+        velocity = velocity / SECONDS_IN_MINUTE
+        # Get circle start point and end point.
+        if self._plane == PLANE_XY:
+            sa = -radius.x
+            sb = -radius.y
+            ea = sa + delta.x
+            eb = sb + delta.y
+            apm = STEPPER_PULSES_PER_MM_X
+            bpm = STEPPER_PULSES_PER_MM_Y
+        elif self._plane == PLANE_YZ:
+            sa = -radius.y
+            sb = -radius.z
+            ea = sa + delta.y
+            eb = sb + delta.z
+            apm = STEPPER_PULSES_PER_MM_Y
+            bpm = STEPPER_PULSES_PER_MM_Z
+        elif self._plane == PLANE_ZX:
+            sa = -radius.z
+            sb = -radius.x
+            ea = sa + delta.z
+            eb = sb + delta.x
+            apm = STEPPER_PULSES_PER_MM_Z
+            bpm = STEPPER_PULSES_PER_MM_X
+        # adjust radius to fit into axises step.
+        self._radius = round(math.sqrt(sa * sa + sb * sb) * min(apm, bpm)) \
+                       / min(apm, bpm)
+        self._radius2 = self._radius * self._radius
+        self._start_a = sa
+        self._start_b = sb
+        assert round(math.sqrt(ea * ea + eb * eb) * min(apm, bpm)) \
+               / min(apm, bpm) == self._radius, "Wrong end point"
+
+        # Calculate angles and directions.
+        start_angle = self.__angle(sa, sb)
+        end_angle = self.__angle(ea, eb)
+        delta_angle = end_angle - start_angle
+        if delta_angle < 0 or (delta_angle == 0 and direction == CW):
+            delta_angle += 2 * math.pi
+        if direction == CCW:
+            delta_angle -= 2 * math.pi
+        if direction == CW:
+            if start_angle >= math.pi:
+                self._dir_b = 1
+            else:
+                self._dir_b = -1
+            if math.pi / 2 <= start_angle < 3 * math.pi / 2:
+                self._dir_a = -1
+            else:
+                self._dir_a = 1
+        elif direction == CCW:
+            if 0 < start_angle <= math.pi:
+                self._dir_b = 1
+            else:
+                self._dir_b = -1
+            if start_angle <= math.pi / 2 or start_angle > 3 * math.pi / 2:
+                self._dir_a = -1
+            else:
+                self._dir_a = 1
+        self._side_a = self._start_b < 0 or (self._start_b == 0 and self._dir_b < 0)
+        self._side_b = self._start_a < 0 or (self._start_a == 0 and self._dir_a < 0)
+        self._start_angle = start_angle
+        logging.debug("start angle {}, end angle {}, delta {}".format(
+                      start_angle * 180.0 / math.pi,
+                      end_angle * 180.0 / math.pi,
+                      delta_angle * 180.0 / math.pi))
+        delta_angle = abs(delta_angle)
+        self._delta_angle = delta_angle
+
+        # calculate values for interpolation.
+
+        # calculate travel distance for axis in circular move.
+        self._iterations_a = 0
+        self._iterations_b = 0
+        end_angle_m = end_angle
+        if start_angle >= end_angle:
+            end_angle_m += 2 * math.pi
+        rstart = int(start_angle / (math.pi / 2.0))
+        rend =  int(end_angle_m / (math.pi / 2.0))
+        if rend - rstart >= 4:
+            self._iterations_a = 4 * int(self._radius * apm)
+            self._iterations_b = 4 * int(self._radius * apm)
+        else:
+            if rstart == rend:
+                self._iterations_a = int(abs(sa - ea) * apm)
+                self._iterations_b = int(abs(sb - eb) * bpm)
+            else:
+                for r in range(rstart, rend + 1):
+                    i = r
+                    if i >= 4:
+                        i -= 4
+                    if r == rstart:
+                        if i == 0 or i == 2:
+                            self._iterations_a += int(self._radius * apm) - int(abs(sa) * apm)
+                        else:
+                            self._iterations_a += int(abs(sa) * apm)
+                        if i == 1 or i == 3:
+                            self._iterations_b += int(self._radius * bpm) - int(abs(sb) * bpm)
+                        else:
+                            self._iterations_b += int(abs(sb) * bpm)
+                    elif r == rend:
+                        if i == 0 or i == 2:
+                            self._iterations_a += int(abs(ea) * apm)
+                        else:
+                            self._iterations_a += int(self._radius * apm) - int(abs(ea) * apm)
+                        if i == 1 or i == 3:
+                            self._iterations_b += int(abs(eb) * bpm)
+                        else:
+                            self._iterations_b += int(self._radius * bpm) - int(abs(eb) * bpm)
+                    else:
+                        self._iterations_a += int(self._radius * apm)
+                        self._iterations_b += int(self._radius * bpm)
+            if direction == CCW:
+                self._iterations_a = 4 * int(self._radius * apm) - self._iterations_a
+                self._iterations_b = 4 * int(self._radius * bpm) - self._iterations_b
+
+        arc = delta_angle * self._radius
+        e2 = delta.e * delta.e
+        if self._plane == PLANE_XY:
+            self._iterations_3rd = abs(delta.z) * STEPPER_PULSES_PER_MM_Z
+            l = math.sqrt(arc * arc + delta.z * delta.z + e2)
+            self._velocity_3rd = abs(delta.z) / l * velocity
+            self._third_dir = math.copysign(1, delta.z)
+        elif self._plane == PLANE_YZ:
+            self._iterations_3rd = abs(delta.x) * STEPPER_PULSES_PER_MM_X
+            l = math.sqrt(arc * arc + delta.x * delta.x + e2)
+            self._velocity_3rd = abs(delta.x) / l * velocity
+            self._third_dir = math.copysign(1, delta.x)
+        elif self._plane == PLANE_ZX:
+            self._iterations_3rd = abs(delta.y) * STEPPER_PULSES_PER_MM_Y
+            l = math.sqrt(arc * arc + delta.y * delta.y + e2)
+            self._velocity_3rd = abs(delta.y) / l * velocity
+            self._third_dir = math.copysign(1, delta.y)
+        self._iterations_e = abs(delta.e) * STEPPER_PULSES_PER_MM_E
+        # Velocity splits with corresponding distance.
+        cV = arc / l * velocity
+        self._RdivV = self._radius / cV
+        self._e_velocity = abs(delta.e) / l * velocity
+        self._e_dir = math.copysign(1, delta.e)
+        self.max_velocity_mm_per_sec = max(cV, self._velocity_3rd,
+                                           self._e_velocity)
+        self.acceleration_time_s = self.max_velocity_mm_per_sec \
+                                   / STEPPER_MAX_ACCELERATION_MM_PER_S2
+        if STEPPER_MAX_ACCELERATION_MM_PER_S2 * self.acceleration_time_s ** 2 \
+                > l:
+            self.acceleration_time_s = math.sqrt(l /
+                                            STEPPER_MAX_ACCELERATION_MM_PER_S2)
+            self.linear_time_s = 0.0
+            self.max_velocity_mm_per_sec = l / self.acceleration_time_s
+        else:
+            linear_distance_mm = l - self.acceleration_time_s ** 2 \
+                                 * STEPPER_MAX_ACCELERATION_MM_PER_S2
+            self.linear_time_s = linear_distance_mm / velocity
+
+    def __angle(self, a, b):
+        # Calculate angle of entry point (a, b) of circle with center in (0,0)
+        angle = math.acos(b / math.sqrt(a * a + b * b))
+        if a < 0:
+            return 2 * math.pi - angle
+        return angle
+
+    def _get_movement_parameters(self):
+        """ Return movement parameters, see super class for details.
+        """
+        return self.acceleration_time_s, \
+               self.linear_time_s, \
+               self.max_velocity_mm_per_sec
+
+    def __circularHelper(self, start, i, pulses_per_mm, side, dir):
+        np = start + dir * i / pulses_per_mm
+        np = round(np, 10)
+        if np > self._radius:
+            np -= 2 * (np - self._radius)
+            np = round(np, 10)
+            dir = -dir
+            side = not side
+        if np < -self._radius:
+            np -= 2 * (np + self._radius)
+            np = round(np, 10)
+            dir = -dir
+            side = not side
+        if np > self._radius:
+            np -= 2 * (np - self._radius)
+            np = round(np, 10)
+            dir = -dir
+            side = not side
+        return np, dir, side
+
+    def __circularFindTime(self, a, b):
+        angle = self.__angle(a, b)
+        if self._direction == CW:
+            delta_angle = angle - self._start_angle
+        else:
+            delta_angle = self._start_angle - angle
+        if delta_angle <= 0:
+            delta_angle += 2 * math.pi
+        return self._RdivV * delta_angle
+
+    def __circularA(self, i, pulses_per_mm):
+        if i >= self._iterations_a:
+            return self._dir_a, None
+        a, dir, side = self.__circularHelper(self._start_a, i + 1,
+                                             pulses_per_mm, self._side_a,
+                                             self._dir_a)
+        # last item can be slightly more then end angle due to float precision
+        if i + 1 == self._iterations_a:
+            return dir, self._RdivV * self._delta_angle
+        b = math.sqrt(self._radius2 - a * a)
+        if side:
+            b = -b
+        return dir, self.__circularFindTime(a, b)
+
+    def __circularB(self, i, pulses_per_mm):
+        if i >= self._iterations_b:
+            return self._dir_b, None
+        b, dir, side = self.__circularHelper(self._start_b, i + 1,
+                                             pulses_per_mm, self._side_b,
+                                             self._dir_b)
+        # last item can be slightly more then end angle due to float precision
+        if i + 1 == self._iterations_b:
+            return dir, self._RdivV * self._delta_angle
+        a = math.sqrt(self._radius2 - b * b)
+        if side:
+            a = -a
+        return dir, self.__circularFindTime(a, b)
+
+    def __linear(self, i, total_i, pulses_per_mm, velocity):
+        if i >= total_i:
+            return None
+        return i / pulses_per_mm / velocity
+
+    def _interpolation_function(self, ix, iy, iz, ie):
+        """ Calculate interpolation values for linear movement, see super class
+            for details.
+        """
+        if self._plane == PLANE_XY:
+            dx, tx = self.__circularA(ix, STEPPER_PULSES_PER_MM_X)
+            dy, ty = self.__circularB(iy, STEPPER_PULSES_PER_MM_Y)
+            tz = self.__linear(iz, self._iterations_3rd, STEPPER_PULSES_PER_MM_Z,
+                               self._velocity_3rd)
+            dz = self._third_dir
+        elif self._plane == PLANE_YZ:
+            dy, ty = self.__circularA(iy, STEPPER_PULSES_PER_MM_Y)
+            dz, tz = self.__circularB(iz, STEPPER_PULSES_PER_MM_Z)
+            tx = self.__linear(ix, self._iterations_3rd, STEPPER_PULSES_PER_MM_X,
+                               self._velocity_3rd)
+            dx = self._third_dir
+        elif self._plane == PLANE_ZX:
+            dz, tz = self.__circularA(iz, STEPPER_PULSES_PER_MM_Z)
+            dx, tx = self.__circularB(ix, STEPPER_PULSES_PER_MM_X)
+            ty = self.__linear(iy, self._iterations_3rd, STEPPER_PULSES_PER_MM_Y,
+                               self._velocity_3rd)
+            dy = self._third_dir
+        te = self.__linear(ie, self._iterations_e, STEPPER_PULSES_PER_MM_E,
+                           self._e_velocity)
+        return (dx, dy, dz, self._e_dir), (tx, ty, tz, te)

tests/test_coordinates.py

@@ -139,4 +139,4 @@ class TestCoordinates(unittest.TestCase):
 
 
 if __name__ == '__main__':
-    unittest.main()
+    unittest.main()

tests/test_gmachine.py

@@ -91,14 +91,34 @@ class TestGMachine(unittest.TestCase):
                           m.do_command, GCode.parse_line("G2X4Y4I2J2"))
         self.assertRaises(GMachineException,
                           m.do_command, GCode.parse_line("G3X4Y4I2J2"))
+        m.do_command(GCode.parse_line("G17"))
         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, 0))
-        m.do_command(GCode.parse_line("G1X5Y5"))
+        m.do_command(GCode.parse_line("G1X10Y10"))
+        m.do_command(GCode.parse_line("G2X9I1"))
+        self.assertEqual(m.position(), Coordinates(9, 10, 0, 0))
+        m.do_command(GCode.parse_line("G19"))
+        m.do_command(GCode.parse_line("G1X10Y10Z10"))
+        m.do_command(GCode.parse_line("G3Y8K1"))
+        self.assertEqual(m.position(), Coordinates(10, 8, 10, 0))
+        m.do_command(GCode.parse_line("G17"))
+        m.do_command(GCode.parse_line("G1X5Y5Z0"))
         m.do_command(GCode.parse_line("G2X0Y0Z5I-2J-2"))
         self.assertEqual(m.position(), Coordinates(0, 0, 5, 0))
-
+        m.do_command(GCode.parse_line("G17"))
+        m.do_command(GCode.parse_line("G1X90Y90"))
+        m.do_command(GCode.parse_line("G2X90Y70I-5J-5"))
+        self.assertEqual(m.position(), Coordinates(90, 70, 5, 0))
+        m.do_command(GCode.parse_line("G18"))
+        m.do_command(GCode.parse_line("G1X90Y90Z20E0"))
+        m.do_command(GCode.parse_line("G2Z20X70I-5K-5E22"))
+        self.assertEqual(m.position(), Coordinates(70, 90, 20, 22))
+        m.do_command(GCode.parse_line("G19"))
+        m.do_command(GCode.parse_line("G1X90Y90Z20"))
+        m.do_command(GCode.parse_line("G2Y90Z0J-5K-5E27"))
+        self.assertEqual(m.position(), Coordinates(90, 90, 0, 27))
 
     def test_g4(self):
         m = GMachine()

tests/test_pulses.py

@@ -19,8 +19,17 @@ class TestPulses(unittest.TestCase):
         self.assertRaises(ZeroDivisionError,
                           PulseGeneratorLinear,
                           Coordinates(0, 0, 0, 0), self.v)
+        self.assertRaises(ZeroDivisionError, PulseGeneratorCircular,
+                          Coordinates(0, 0, 0, 0), Coordinates(0, 0, 9, 9),
+                          PLANE_XY, CW, self.v)
+        self.assertRaises(ZeroDivisionError, PulseGeneratorCircular,
+                          Coordinates(0, 0, 0, 0), Coordinates(9, 0, 0, 9),
+                          PLANE_YZ, CW, self.v)
+        self.assertRaises(ZeroDivisionError, PulseGeneratorCircular,
+                          Coordinates(0, 0, 0, 0), Coordinates(0, 9, 0, 9),
+                          PLANE_ZX, CW, self.v)
 
-    def test_step(self):
+    def test_step_linear(self):
         # Check if PulseGenerator returns correctly single step movement.
         g = PulseGeneratorLinear(Coordinates(1.0 / STEPPER_PULSES_PER_MM_X,
                                              0, 0, 0),
@@ -52,19 +61,121 @@ class TestPulses(unittest.TestCase):
             self.assertEqual(pe, 0)
         self.assertEqual(i, 1)
 
-    def test_linear_with_hal_virtual(self):
+
+    def __check_circular(self, delta, radius, plane, direction = CW):
+        g = PulseGeneratorCircular(delta, radius, plane, direction, self.v)
+        x, y, z, e = 0, 0, 0, 0
+        dx, dy, dz, de = None, None, None, None
+        dir_changed = 0
+        dir_requested = False
+        t = -1
+        for dir, px, py, pz, pe in g:
+            if dir:
+                dx, dy, dz, de = px, py, pz, pe
+                dir_requested = True
+                continue
+            if dir_requested: # ignore last change
+                dir_requested = False
+                dir_changed += 1
+            if px is not None:
+                x += dx
+            if py is not None:
+                y += dy
+            if pz is not None:
+                z += dz
+            if pe is not None:
+                e += de
+            v = list(i for i in (px, py, pz, pe) if i is not None)
+            self.assertEqual(min(v), max(v))
+            self.assertLessEqual(t, min(v))
+            t = max(v)
+        return dir_changed, Coordinates(x / STEPPER_PULSES_PER_MM_X,
+                                        y / STEPPER_PULSES_PER_MM_Y,
+                                        z / STEPPER_PULSES_PER_MM_Z,
+                                        e / STEPPER_PULSES_PER_MM_E)
+
+    def test_single_radius_circles(self):
+        # Check if PulseGenerator returns correctly single radius movement in
+        # both direction.
+        _, pos = self.__check_circular(Coordinates(0, 0, 0, 0),
+                                       Coordinates(1.0 / STEPPER_PULSES_PER_MM_X, 0, 0, 0),
+                                       PLANE_XY, CW)
+        self.assertEqual(pos, Coordinates(0, 0, 0, 0))
+        _, pos = self.__check_circular(Coordinates(0, 0, 0, 0),
+                                       Coordinates(-1.0 / STEPPER_PULSES_PER_MM_X, 0, 0, 0),
+                                       PLANE_XY, CW)
+        self.assertEqual(pos, Coordinates(0, 0, 0, 0))
+        _, pos = self.__check_circular(Coordinates(0, 0, 0, 0),
+                                       Coordinates(0, 1.0 / STEPPER_PULSES_PER_MM_Y, 0, 0),
+                                       PLANE_YZ, CW)
+        self.assertEqual(pos, Coordinates(0, 0, 0, 0))
+        _, pos = self.__check_circular(Coordinates(0, 0, 0, 0),
+                                       Coordinates(0, -1.0 / STEPPER_PULSES_PER_MM_Y, 0, 0),
+                                       PLANE_YZ, CW)
+        self.assertEqual(pos, Coordinates(0, 0, 0, 0))
+        _, pos = self.__check_circular(Coordinates(0, 0, 0, 0),
+                                       Coordinates(0, 0, 1.0 / STEPPER_PULSES_PER_MM_Z, 0),
+                                       PLANE_ZX, CW)
+        self.assertEqual(pos, Coordinates(0, 0, 0, 0))
+        _, pos = self.__check_circular(Coordinates(0, 0, 0, 0),
+                                       Coordinates(0, 0, -1.0 / STEPPER_PULSES_PER_MM_Z, 0),
+                                       PLANE_ZX, CW)
+        self.assertEqual(pos, Coordinates(0, 0, 0, 0))
+        _, pos = self.__check_circular(Coordinates(0, 0, 0, 0),
+                                       Coordinates(1.0 / STEPPER_PULSES_PER_MM_X, 0, 0, 0),
+                                       PLANE_XY, CCW)
+        self.assertEqual(pos, Coordinates(0, 0, 0, 0))
+        _, pos = self.__check_circular(Coordinates(0, 0, 0, 0),
+                                       Coordinates(-1.0 / STEPPER_PULSES_PER_MM_X, 0, 0, 0),
+                                       PLANE_XY, CCW)
+        self.assertEqual(pos, Coordinates(0, 0, 0, 0))
+        _, pos = self.__check_circular(Coordinates(0, 0, 0, 0),
+                                       Coordinates(0, 1.0 / STEPPER_PULSES_PER_MM_Y, 0, 0),
+                                       PLANE_YZ, CCW)
+        self.assertEqual(pos, Coordinates(0, 0, 0, 0))
+        _, pos = self.__check_circular(Coordinates(0, 0, 0, 0),
+                                       Coordinates(0, -1.0 / STEPPER_PULSES_PER_MM_Y, 0, 0),
+                                       PLANE_YZ, CCW)
+        self.assertEqual(pos, Coordinates(0, 0, 0, 0))
+        _, pos = self.__check_circular(Coordinates(0, 0, 0, 0),
+                                       Coordinates(0, 0, 1.0 / STEPPER_PULSES_PER_MM_Z, 0),
+                                       PLANE_ZX, CCW)
+        self.assertEqual(pos, Coordinates(0, 0, 0, 0))
+        _, pos = self.__check_circular(Coordinates(0, 0, 0, 0),
+                                       Coordinates(0, 0, -1.0 / STEPPER_PULSES_PER_MM_Z, 0),
+                                       PLANE_ZX, CCW)
+        self.assertEqual(pos, Coordinates(0, 0, 0, 0))
+
+    def test_with_hal_virtual(self):
         # Using hal_virtual module for this test, it already contains plenty
         # of asserts for wrong number of pulses, pulse timing issues etc
-        hal_virtual.move_linear(Coordinates(1, 0, 0, 0), self.v)
-        hal_virtual.move_linear(Coordinates(25.4, 0, 0, 0), self.v)
-        hal_virtual.move_linear(Coordinates(25.4, 0, 0, 0), self.v)
-        hal_virtual.move_linear(Coordinates(25.4, 0, 0, 0), self.v)
-        hal_virtual.move_linear(Coordinates(TABLE_SIZE_X_MM,
+        hal_virtual.move(PulseGeneratorLinear(Coordinates(1, 0, 0, 0),
+                                              self.v))
+        hal_virtual.move(PulseGeneratorLinear(Coordinates(25.4, 0, 0, 0),
+                                              self.v))
+        hal_virtual.move(PulseGeneratorLinear(Coordinates(25.4, 0, 0, 0),
+                                              self.v))
+        hal_virtual.move(PulseGeneratorLinear(Coordinates(25.4, 0, 0, 0),
+                                              self.v))
+        hal_virtual.move(PulseGeneratorLinear(Coordinates(TABLE_SIZE_X_MM,
                                             TABLE_SIZE_Y_MM,
                                             TABLE_SIZE_Z_MM,
-                                            100.0), self.v)
+                                            100.0), self.v))
+        hal_virtual.move(PulseGeneratorCircular(Coordinates(0, 20, 0, 0),
+                                                Coordinates(-10, 10, 0, 0),
+                                                PLANE_XY, CW, self.v))
+        hal_virtual.move(PulseGeneratorCircular(Coordinates(-4, -4, 0, 0),
+                                                Coordinates(-2, -2, 0, 0),
+                                                PLANE_XY, CW, self.v))
+        hal_virtual.move(PulseGeneratorCircular(Coordinates(- 2.0 / STEPPER_PULSES_PER_MM_X,
+                                                            - 2.0 / STEPPER_PULSES_PER_MM_Y,
+                                                            0, 0),
+                                                Coordinates(- 1.0 / STEPPER_PULSES_PER_MM_X,
+                                                            - 1.0 / STEPPER_PULSES_PER_MM_Y,
+                                                            0, 0),
+                                                PLANE_XY, CW, self.v))
 
-    def test_twice_faster(self):
+    def test_twice_faster_linear(self):
         # Checks if one axis moves exactly twice faster, pulses are correct.
         m = Coordinates(2, 4, 0, 0)
         g = PulseGeneratorLinear(m, self.v)
@@ -112,10 +223,23 @@ class TestPulses(unittest.TestCase):
         self.assertEqual(m.z * STEPPER_PULSES_PER_MM_Z, iz)
         self.assertEqual(m.e * STEPPER_PULSES_PER_MM_E, ie)
         self.assertLessEqual(t, g.total_time_s())
+        _, pos = self.__check_circular(Coordinates(0, 8, 0, 7),
+                                       Coordinates(1, 0, 1, 0),
+                                       PLANE_ZX, CCW)
+        self.assertEqual(pos, Coordinates(0, 8, 0, 7))
+        _, pos = self.__check_circular(Coordinates(5, 0, 0, 6),
+                                       Coordinates(0, 1, -1, 0),
+                                       PLANE_YZ, CW)
+        self.assertEqual(pos, Coordinates(5, 0, 0, 6))
+        _, pos = self.__check_circular(Coordinates(-2, -2, 3, 2),
+                                       Coordinates(-1, -1, 0, 0),
+                                       PLANE_XY, CCW)
+        self.assertEqual(pos, Coordinates(-2, -2, 3, 2))
 
     def test_acceleration_velocity(self):
         # Check if acceleration present in pulses sequence and if velocity
-        # is correct
+        # is correct, since PulseGenerator is responsible for this, check only
+        # one child class.
         m = Coordinates(TABLE_SIZE_X_MM, 0, 0, 0)
         g = PulseGeneratorLinear(m, self.v)
         i = 0
@@ -156,6 +280,11 @@ class TestPulses(unittest.TestCase):
                 dir_found = True
                 # check dirs
                 self.assertTrue(px < 0 and py > 0 and pz < 0 and pe > 0)
+        # check for circle, full circle
+        dir_changed, _ = self.__check_circular(Coordinates(0, 0, 0, 0),
+                                               Coordinates(1.0, 1.0, 0, 0),
+                                               PLANE_ZX, CCW)
+        self.assertEqual(dir_changed, 4)
 
 
 if __name__ == '__main__':