feat:1.blockly add holding registers check;2.gcode add H114

Author: SchoolMater

xarm/tools/blockly/_blockly_handler.py

@@ -29,11 +29,13 @@ def __init__(self, xml_path):
         self._listen_tgpio_analog = False
         self._listen_cgpio_state = False
         self._listen_count = False
+        self._listen_holding = False
         self._tgpio_digital_callbacks = []
         self._tgpio_analog_callbacks = []
         self._cgpio_digital_callbacks = []
         self._cgpio_analog_callbacks = []
         self._count_callbacks= []
+        self._holding_callbacks= []
 
         self.is_run_linear_track = False
         self._is_run_blockly = False
@@ -783,7 +785,34 @@ def __handle_gpio_event(self, gpio_type, block, indent=0, arg_map=None):
                 trigger = fields[2].text
                 self._cgpio_analog_callbacks.append(name)
                 self._append_main_code('self._cgpio_analog_callbacks.append({{\'io\': {}, \'trigger\': {}, \'op\': \'{}\', \'callback\': self.{}}})'.format(io, trigger, op, name), indent=indent+2)
+    
+    def _handle_event_get_holding_condition(self, block, indent=0, arg_map=None):
+        fields = self._get_nodes('field', root=block)
+        addr = int(fields[0].text.replace(' ', '').replace('0x','').replace(',','').replace('\xa0', ''), 16)
+        trigger = int(fields[1].text.replace(' ', '').replace('0x','').replace(',','').replace('\xa0', ''), 16)
 
+        num = 1
+
+        self._is_main_run_code = False
+        num = len(self._holding_callbacks) + 1
+        name = 'holding_is_changed_callback_{}'.format(num)
+        self._append_main_code('# Define Holding Registers {} Value is {} callback'.format(
+            addr, trigger), indent=1)
+
+        self._append_main_code('def {}(self):'.format(name), indent=1)
+        statement = self._get_node('statement', root=block)
+        if statement:
+            self._parse_block(statement, indent, arg_map=arg_map)
+        else:
+            self._append_main_code('pass', indent=indent + 3)
+        self._append_main_code('')
+
+        self._is_main_run_code = True
+        self._holding_callbacks.append(name)
+        self._append_main_code(
+            'self._holding_callbacks.append({{\'trigger\': \'{}\', \'addr\': \'{}\', \'callback\': self.{}}})'.format(
+                trigger, addr, name), indent=indent + 2)
+    
     def __handle_count_event(self, count_type, block, indent=0, arg_map=None):
         fields = self._get_nodes('field', root=block)
         op = fields[0].text

xarm/tools/blockly/_blockly_tool.py

@@ -91,7 +91,7 @@ def _init_robot_main_run_codes(self):
 
     def _finish_robot_main_run_codes(self, error_exit=True, stop_exit=True):
         if self._listen_tgpio_digital or self._listen_tgpio_analog or self._listen_cgpio_state \
-            or len(self._tgpio_digital_callbacks) or len(self._tgpio_analog_callbacks) or len(self._cgpio_digital_callbacks) or len(self._cgpio_analog_callbacks):
+            or len(self._tgpio_digital_callbacks) or len(self._tgpio_analog_callbacks) or len(self._cgpio_digital_callbacks) or len(self._cgpio_analog_callbacks) or len(self._count_callbacks) or len(self._holding_callbacks):
             self._append_main_code('            # Event Loop', indent=-1)
             self._append_main_code('            while self.is_alive:', indent=-1)
             self._append_main_code('                time.sleep(0.5)', indent=-1)
@@ -153,12 +153,16 @@ def _init_robot_main_class_codes(self, init=True, wait_seconds=1, mode=0, state=
             self._append_main_init_code('        self._cgpio_analog_callbacks = []')
         if len(self._count_callbacks):
             self._append_main_init_code('        self._count_callbacks = []')
+        if len(self._holding_callbacks):
+            self._append_main_init_code('        self._holding_callbacks = []')
         if self._listen_cgpio_state or len(self._cgpio_digital_callbacks) or len(self._cgpio_analog_callbacks):
             self._append_main_init_code('        self._cgpio_state = None')
 
         if self._listen_count or len(self._count_callbacks):
             self._append_main_init_code('        self._counter_val = None')
-
+        if self._listen_holding or len(self._holding_callbacks):
+            self._append_main_init_code('        self._holding_dict = {}')
+        
         if len(self._tgpio_digital_callbacks) or len(self._tgpio_analog_callbacks) or len(self._cgpio_digital_callbacks) or len(self._cgpio_analog_callbacks)\
                 or len(self._count_callbacks):
             self._append_main_init_code('        self._callback_in_thread = kwargs.get(\'callback_in_thread\', True)')
@@ -170,15 +174,20 @@ def _init_robot_main_class_codes(self, init=True, wait_seconds=1, mode=0, state=
             self._append_main_init_code('        gpio_t.start()')
 
         if len(self._tgpio_digital_callbacks) or len(self._tgpio_analog_callbacks) or len(self._cgpio_digital_callbacks) or len(self._cgpio_analog_callbacks)\
-                or len(self._count_callbacks):
+                or len(self._count_callbacks) or len(self._holding_callbacks):
             self._append_main_init_code('        callback_t = threading.Thread(target=self._event_callback_handle_thread, daemon=True)')
             self._append_main_init_code('        callback_t.start()')
 
         if self._listen_count or len(self._count_callbacks):
             self._append_main_init_code(
                 '        count_t = threading.Thread(target=self._listen_count_thread, daemon=True)')
             self._append_main_init_code('        count_t.start()')
-
+        
+        if self._holding_callbacks or len(self._holding_callbacks):
+            self._append_main_init_code(
+                '        holding_t = threading.Thread(target=self._listen_holding_thread, daemon=True)')
+            self._append_main_init_code('        holding_t.start()')
+        
         self._append_main_init_code('')
 
         self.__define_callback_thread_func()
@@ -238,7 +247,7 @@ def __define_property(self):
 
     def __define_callback_thread_func(self):
         # Define callback thread function
-        if  len(self._tgpio_digital_callbacks) or len(self._tgpio_analog_callbacks) or len(self._cgpio_digital_callbacks) or len(self._cgpio_analog_callbacks) or len(self._count_callbacks):
+        if  len(self._tgpio_digital_callbacks) or len(self._tgpio_analog_callbacks) or len(self._cgpio_digital_callbacks) or len(self._cgpio_analog_callbacks) or len(self._count_callbacks) or len(self._holding_callbacks):
             self._append_main_init_code('    def _event_callback_handle_thread(self):')
             self._append_main_init_code('        while self.alive:')
             self._append_main_init_code('            try:')
@@ -328,6 +337,23 @@ def __define_listen_count_thread_func(self):
                 self._append_main_init_code('                            self._callback_que.put(item[\'callback\'])')
             self._append_main_init_code('            self._counter_val = values')
             self._append_main_init_code('            time.sleep(0.01)\n')
+    
+    def __define_listen_holding_registers_thread_func(self):
+        # Define listen holding registers value thread function
+        if self._listen_holding or len(self._holding_callbacks):
+            self._append_main_init_code('    def _listen_holding_thread(self):')
+
+            self._append_main_init_code('        while self.alive:')
+            self._append_main_init_code('            for index, item in enumerate(self._holding_callbacks):')
+            self._append_main_init_code('                for i in range(2):')
+            self._append_main_init_code('                    _, values = self._arm.read_holding_registers(int(item[\'addr\']), 1)')
+            self._append_main_init_code(
+                '                    if _ == 0 and self._holding_dict.get(index, None) is not None:')
+            self._append_main_init_code(
+                '                        if eval(\'{} == {}\'.format(values[0], item[\'trigger\'])) and not eval(\'{} == {}\'.format(self._holding_dict[index], item[\'trigger\'])):')
+            self._append_main_init_code('                            self._callback_que.put(item[\'callback\'])')
+            self._append_main_init_code('                    self._holding_dict[index] = values[0] if _ == 0 else self._holding_dict.get(index, None)')
+            self._append_main_init_code('                    time.sleep(0.01)\n')
 
     def __define_run_blockly_func(self):
         if self._is_run_blockly and not self._is_exec:

xarm/version.py

@@ -1 +1 @@
-__version__ = '1.14.0'
+__version__ = '1.14.1'

xarm/x3/xarm.py

@@ -1259,6 +1259,10 @@ def __handle_gcode_h(num):
                 ret = self.set_servo_zero(servo_id=servo_id)
             elif num == 106:  # H106 get_servo_debug_msg, ex: H106
                 ret = self.get_servo_debug_msg()
+            elif num == 114:  # H114 servo_error_addr_r32, ex H114 I{id} D{addr}
+                servo_id = gcode_p.get_id_num(command, default=0)
+                addr = gcode_p.get_addr(command)
+                ret = self.arm_cmd.servo_error_addr_r32(axis=servo_id, addr=addr)
             else:
                 logger.debug('command {} is not exist'.format(command))
                 ret = APIState.CMD_NOT_EXIST, 'command {} is not exist'.format(command)