add direction control in pulsegenerator

cnc/hal_raspberry/hal.py

@@ -31,7 +31,7 @@ STEP_PIN_MASK_E = 1 << STEPPER_STEP_PIN_E
 
 def init():
     """ Initialize GPIO pins and machine itself, including callibration if
-        needed. Do not return till all procedure is completed.
+        needed. Do not return till all procedures are completed.
     """
     gpio.init(STEPPER_STEP_PIN_X, rpgpio.GPIO.MODE_OUTPUT)
     gpio.init(STEPPER_STEP_PIN_Y, rpgpio.GPIO.MODE_OUTPUT)
@@ -120,30 +120,33 @@ def move_linear(delta, velocity):
     while dma.is_active():
         time.sleep(0.001)
 
-    # set direction pins
-    if delta.x > 0.0:
-        gpio.clear(STEPPER_DIR_PIN_X)
-    else:
-        gpio.set(STEPPER_DIR_PIN_X)
-    if delta.y > 0.0:
-        gpio.clear(STEPPER_DIR_PIN_Y)
-    else:
-        gpio.set(STEPPER_DIR_PIN_Y)
-    if delta.z > 0.0:
-        gpio.clear(STEPPER_DIR_PIN_Z)
-    else:
-        gpio.set(STEPPER_DIR_PIN_Z)
-    if delta.e > 0.0:
-        gpio.clear(STEPPER_DIR_PIN_E)
-    else:
-        gpio.set(STEPPER_DIR_PIN_E)
-
     # prepare and run dma
     dma.clear()
     prev = 0
     is_ran = False
     st = time.time()
-    for tx, ty, tz, te in generator:
+    for dir, tx, ty, tz, te in generator:
+        if dir:  # set up directions
+            pins_to_set = 0
+            pins_to_clear = 0
+            if tx > 0:
+                pins_to_clear |= 1 << STEPPER_DIR_PIN_X
+            elif tx < 0:
+                pins_to_set |= 1 << STEPPER_DIR_PIN_X
+            if ty > 0:
+                pins_to_clear |= 1 << STEPPER_DIR_PIN_Y
+            elif ty < 0:
+                pins_to_set |= 1 << STEPPER_DIR_PIN_Y
+            if tz > 0:
+                pins_to_clear |= 1 << STEPPER_DIR_PIN_Z
+            elif tz < 0:
+                pins_to_set |= 1 << STEPPER_DIR_PIN_Z
+            if te > 0:
+                pins_to_clear |= 1 << STEPPER_DIR_PIN_E
+            elif te < 0:
+                pins_to_set |= 1 << STEPPER_DIR_PIN_E
+            dma.add_set_clear(pins_to_set, pins_to_clear)
+            continue
         pins = 0
         k = int(round(min(x for x in (tx, ty, tz, te) if x is not None)
                       * US_IN_SECONDS))

cnc/hal_raspberry/rpgpio.py

@@ -166,6 +166,24 @@ class DMAGPIO(DMAProto):
         self._physmem.write(self.__current_address, "8I", data)
         self.__current_address = next_cb
 
+    def add_set_clear(self, pins_to_set, pins_to_clear):
+        """ Change state of gpio pins.
+        :param pins_to_set: bitwise mask which pins should be set.
+        :param pins_to_clear: bitwise mask which pins should be clear.
+        """
+        next_cb = self.__current_address + self._DMA_CONTROL_BLOCK_SIZE
+        if next_cb > self._physmem.get_size():
+            raise MemoryError("Out of allocated memory.")
+        next1 = self._physmem.get_bus_address() + next_cb
+        source = next1 - 8  # last 8 bytes are padding, use it to store data
+        data = (
+                self._pulse_info, source, self._pulse_destination,
+                    self._pulse_length,
+                self._pulse_stride, next1, pins_to_set, pins_to_clear
+               )
+        self._physmem.write(self.__current_address, "8I", data)
+        self.__current_address = next_cb
+
     def finalize_stream(self):
         """ Mark last added block as the last one.
         """

cnc/hal_virtual.py

@@ -37,7 +37,19 @@ def move_linear(delta, velocity):
     dx, dy, dz, de = 0, 0, 0, 0
     mx, my, mz, me = 0, 0, 0, 0
     st = time.time()
-    for tx, ty, tz, te in generator:
+    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
+            continue
         if tx is not None:
             if tx > mx:
                 mx = tx
@@ -87,6 +99,7 @@ def move_linear(delta, velocity):
         f = list(x for x in (tx, ty, tz, te) if x is not None)
         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"

cnc/pulses.py

@@ -42,6 +42,7 @@ class PulseGenerator(object):
         self._iteration_y = 0
         self._iteration_z = 0
         self._iteration_e = 0
+        self._iteration_direction = None
         self._acceleration_time_s = 0.0
         self._linear_time_s = 0.0
         self._2Vmax_per_a = 0.0
@@ -54,10 +55,11 @@ class PulseGenerator(object):
                                      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
+                max_axis_velocity_mm_per_sec: maximum velocity of any of axis
-                                              movement. Even if whole movement is
+                                              during movement. Even if whole
-                                              accelerated, this value should be
+                                              movement is accelerated, this
-                                              calculated as top velocity.
+                                              value should be calculated as top
+                                              velocity.
         """
         raise NotImplemented
 
@@ -67,10 +69,13 @@ class PulseGenerator(object):
             must be expressed in terms of position, i.e. t = S / V for linear,
             where S - distance would be increment on motor minimum step.
         :param ix: number of pulse for X axis.
-        :param iy: number of pulse for X axis.
+        :param iy: number of pulse for Y axis.
-        :param iz: number of pulse for X axis.
+        :param iz: number of pulse for Z axis.
-        :param ie: number of pulse for X axis.
+        :param ie: number of pulse for E axis.
-        :return: time for each axis or None if movement for axis is finished.
+        :return: Two tuples. First is tuple is directions for each axis,
+                 positive means forward, negative means reverse. Second is
+                 tuple of times for each axis in us or None if movement for
+                 axis is finished.
         """
         raise NotImplemented
 
@@ -87,6 +92,7 @@ class PulseGenerator(object):
         self._iteration_y = 0
         self._iteration_z = 0
         self._iteration_e = 0
+        self._iteration_direction = None
         logging.debug(', '.join("%s: %s" % i for i in vars(self).items()))
         return self
 
@@ -116,7 +122,8 @@ class PulseGenerator(object):
         # 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)
+               - math.sqrt(self._acceleration_time_s ** 2
+                           - self._2Vmax_per_a * bt)
 
     def __next__(self):
         # for python3
@@ -124,14 +131,26 @@ class PulseGenerator(object):
 
     def next(self):
         """ Iterate pulses.
-        :return: Tuple of three values for each axis which represent time for
+        :return: Tuple of five values:
-                 the next pulse. If there is no pulses left  None will be
+                    - first is boolean value, if it is True, motors direction
-                 returned.
+                        should be changed and next pulse should performed in
+                        this direction.
+                    - values for all machine axises. For direction update,
+                        positive values means forward movement, negative value
+                        means reverse movement. For normal pulse, values are
+                        represent time for the next pulse in microseconds.
+                 This iteration strictly guarantees that next pulses time will
+                 not be earlier in time then current. If there is no pulses
+                 left StopIteration will be raised.
         """
-        tx, ty, tz, te = self._interpolation_function(self._iteration_x,
+        dir, (tx, ty, tz, te) = self._interpolation_function(self._iteration_x,
-                                                      self._iteration_y,
+                                                             self._iteration_y,
-                                                      self._iteration_z,
+                                                             self._iteration_z,
-                                                      self._iteration_e)
+                                                             self._iteration_e)
+        # check if direction update:
+        if dir != self._iteration_direction:
+            self._iteration_direction = dir
+            return (True,) + dir
         # check condition to stop
         if tx is None and ty is None and tz is None and te is None:
             raise StopIteration
@@ -165,7 +184,7 @@ class PulseGenerator(object):
                 te = am
                 self._iteration_e += 1
 
-        return tx, ty, tz, te
+        return False, tx, ty, tz, te
 
     def total_time_s(self):
         """ Get total time for movement.
@@ -190,23 +209,29 @@ class PulseGeneratorLinear(PulseGenerator):
             velocity_mm_per_min / SECONDS_IN_MINUTE / distance_total_mm)
         # acceleration time
         self.acceleration_time_s = self.max_velocity_mm_per_sec.find_max() \
-                              / STEPPER_MAX_ACCELERATION_MM_PER_S2
+                                   / STEPPER_MAX_ACCELERATION_MM_PER_S2
         # check if there is enough space to accelerate and brake, adjust time
         # S = a * t^2 / 2
         if STEPPER_MAX_ACCELERATION_MM_PER_S2 * self.acceleration_time_s ** 2 \
                 > distance_total_mm:
             self.acceleration_time_s = math.sqrt(distance_total_mm /
-                                                  STEPPER_MAX_ACCELERATION_MM_PER_S2)
+                                            STEPPER_MAX_ACCELERATION_MM_PER_S2)
             self.linear_time_s = 0.0
-            # V = a * t -> V = 2 * S / t, take half of total distance for acceleration and braking
+            # V = a * t -> V = 2 * S / t, take half of total distance for
-            self.max_velocity_mm_per_sec = self._distance_mm / self.acceleration_time_s
+            # acceleration and braking
+            self.max_velocity_mm_per_sec = self._distance_mm \
+                                           / self.acceleration_time_s
         else:
             # calculate linear time
-            linear_distance_mm = distance_total_mm\
+            linear_distance_mm = distance_total_mm \
-                                      - self.acceleration_time_s ** 2 \
+                                 - self.acceleration_time_s ** 2 \
-                                      * STEPPER_MAX_ACCELERATION_MM_PER_S2
+                                 * STEPPER_MAX_ACCELERATION_MM_PER_S2
             self.linear_time_s = linear_distance_mm \
                                  / self.max_velocity_mm_per_sec.length()
+        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) \
 
     def _get_movement_parameters(self):
         """ Return movement parameters, see super class for details.
@@ -236,4 +261,4 @@ class PulseGeneratorLinear(PulseGenerator):
                             self.max_velocity_mm_per_sec.z)
         t_e = self.__linear(ie / STEPPER_PULSES_PER_MM_E, self._distance_mm.e,
                             self.max_velocity_mm_per_sec.e)
-        return t_x, t_y, t_z, t_e
+        return self._direction, (t_x, t_y, t_z, t_e)

tests/test_pulses.py

@@ -26,7 +26,9 @@ class TestPulses(unittest.TestCase):
                                              0, 0, 0),
                                  self.v)
         i = 0
-        for px, py, pz, pe in g:
+        for dir, px, py, pz, pe in g:
+            if dir:
+                continue
             i += 1
             self.assertEqual(px, 0)
             self.assertEqual(py, None)
@@ -40,7 +42,9 @@ class TestPulses(unittest.TestCase):
                                              1.0 / STEPPER_PULSES_PER_MM_E),
                                  self.v)
         i = 0
-        for px, py, pz, pe in g:
+        for dir, px, py, pz, pe in g:
+            if dir:
+                continue
             i += 1
             self.assertEqual(px, 0)
             self.assertEqual(py, 0)
@@ -65,7 +69,9 @@ class TestPulses(unittest.TestCase):
         m = Coordinates(2, 4, 0, 0)
         g = PulseGeneratorLinear(m, self.v)
         i = 0
-        for px, py, pz, pe in g:
+        for dir, px, py, pz, pe in g:
+            if dir:
+                continue
             if i % 2 == 0:
                 self.assertNotEqual(px, None)
             else:
@@ -86,7 +92,9 @@ class TestPulses(unittest.TestCase):
         iz = 0
         ie = 0
         t = -1
-        for px, py, pz, pe in g:
+        for dir, px, py, pz, pe in g:
+            if dir:
+                continue
             if px is not None:
                 ix += 1
             if py is not None:
@@ -112,7 +120,9 @@ class TestPulses(unittest.TestCase):
         g = PulseGeneratorLinear(m, self.v)
         i = 0
         lx = 0
-        for px, py, pz, pe in g:
+        for dir, px, py, pz, pe in g:
+            if dir:
+                continue
             if i == 2:
                 at = px - lx
             if i == TABLE_SIZE_X_MM * STEPPER_PULSES_PER_MM_X / 2:
@@ -124,5 +134,29 @@ class TestPulses(unittest.TestCase):
         self.assertGreater(at, lt)
         self.assertGreater(bt, lt)
 
+    def test_directions(self):
+        # Check if directions are set up correctly.
+        m = Coordinates(1, -2, 3, -4)
+        g = PulseGeneratorLinear(m, self.v)
+        dir_found = False
+        for dir, px, py, pz, pe in g:
+            if dir:
+                # should be once
+                self.assertFalse(dir_found)
+                dir_found = True
+                # check dirs
+                self.assertTrue(px > 0 and py < 0 and pz > 0 and pe < 0)
+        m = Coordinates(-1, 2, -3, 4)
+        g = PulseGeneratorLinear(m, self.v)
+        dir_found = False
+        for dir, px, py, pz, pe in g:
+            if dir:
+                # should be once
+                self.assertFalse(dir_found)
+                dir_found = True
+                # check dirs
+                self.assertTrue(px < 0 and py > 0 and pz < 0 and pe > 0)
+
+
 if __name__ == '__main__':
     unittest.main()