added RangeRead in dynamixel.loop, fixes #63

roboglia/base/thread.py

@@ -289,10 +289,10 @@ def run(self):
                 wait_time = self.__period - (end_time - start_time) + adjust
                 if wait_time > 0:
                     time.sleep(wait_time)
-                else:
-                    logger.debug(f'Loop "{self.name}" took longer to run '
-                                 f'{end_time - start_time:.5f} than '
-                                 f'loop period {self.period:.5f}; check')
+                # else:
+                #     logger.debug(f'Loop "{self.name}" took longer to run '
+                #                  f'{end_time - start_time:.5f} than '
+                #                  f'loop period {self.period:.5f}; check')
                 # statistics:
                 exec_counts += 1
                 if exec_counts >= self.__frequency * self.__review:
@@ -302,12 +302,13 @@ def run(self):
                     diff = self.__period - exec_time / exec_counts
                     # fine tune the frequency
                     adjust += diff * self.__throttle
+                    if adjust < - self.__period:
+                        adjust = - self.__period
                     if actual_freq < (self.__frequency * self.__warning):
                         logger.warning(
                             f'Loop "{self.name}" running under '
                             f'warning threshold at {actual_freq:.2f}[Hz] '
                             f'({rate*100:.0f}%)')
-                        logger.warning(f'adjust={adjust}')
                     else:
                         logger.info(
                             f'Loop "{self.name}" running at '

roboglia/dynamixel/__init__.py

@@ -14,6 +14,7 @@
 from .sync import DynamixelSyncWriteLoop
 from .sync import DynamixelBulkReadLoop
 from .sync import DynamixelBulkWriteLoop
+from .sync import DynamixelRangeReadLoop
 
 register_class(DynamixelAXBaudRateRegister)
 register_class(DynamixelAXComplianceSlopeRegister)
@@ -29,3 +30,4 @@
 register_class(DynamixelSyncWriteLoop)
 register_class(DynamixelBulkReadLoop)
 register_class(DynamixelBulkWriteLoop)
+register_class(DynamixelRangeReadLoop)

roboglia/dynamixel/sync.py

@@ -265,3 +265,82 @@ def atomic(self):
                         register.int_value = gbr.getData(device.dev_id,
                                                          register.address,
                                                          register.size)
+
+
+class DynamixelRangeReadLoop(BaseSync):
+    """Implements Read for a list of registers as specified in the frequency
+    parameter.
+
+    This method is provided as an alternative for AX devices that do not
+    support BulkRead or SyncRead and for which reading registers with
+    BaseReadSync would be extremely inefficient. With this method we still
+    have to send / recieive a communication packet for each device, but we
+    would get all the registers in one go.
+
+    The devices are provided in the `group` parameter and the registers
+    in the `registers` as a list of register names. The registers do not
+    need to be sequential.
+    It will update the `int_value` of each register in every device with
+    the result of the call.
+    Will raise exceptions if the BulkRead cannot be setup or fails to
+    execute.
+    """
+
+    def setup(self):
+        """Prepares to start the loop."""
+        self.start_address, self.length = self.get_register_range()
+
+    def atomic(self):
+        """Executes a RangeRead for all devices."""
+        # execute read
+        if not self.bus.can_use():
+            logger.error(f'Sync "{self.name}" '
+                         f'failed to acquire bus "{self.bus.name}"')
+            return
+
+        for device in self.devices:
+            # call the function
+            try:
+                res, cerr, derr = self.bus.packet_handler.readTxRx(
+                    self.bus.port_handler, device.dev_id,
+                    self.start_address, self.length)
+            except Exception as e:
+                logger.error(f'Exception raised while reading bus '
+                             f'"{self.name}" device "{device.name}"')
+                logger.error(str(e))
+                continue
+
+            # success call - log DEBUG
+            logger.debug(f'[RangeRead] dev={device.dev_id} '
+                         f'{res} (cerr={cerr}, derr={derr})')
+            # process result
+            if cerr != 0:
+                # communication error
+                err_desc = self.bus.packet_handler.getTxRxResult(cerr)
+                logger.error(f'[RangeRead "{self.name}"] '
+                             f'device "{device.name}", cerr={err_desc}')
+                continue
+
+            if derr != 0:
+                # device error
+                err_desc = self.bus.packet_handler.getRxPacketError(derr)
+                logger.warning(f'Device "{device.name}" responded with a '
+                               f'return error: {err_desc}')
+
+            # processs results
+            for reg_name in self.register_names:
+                reg = getattr(device, reg_name)
+                pos = reg.address - self.start_address
+                if reg.size == 1:
+                    value = res[pos]
+                elif reg.size == 2:
+                    value = res[pos] + res[pos + 1] * 256
+                elif reg.size == 4:
+                    value = res[pos] + res[pos + 1] * 256 + \
+                        res[pos + 2] * 65536 + \
+                        res[pos + 3] * 16777216
+                else:
+                    raise NotImplementedError
+                reg.int_value = value
+
+        self.bus.stop_using()       # !! as soon as possible