修复l20压感数据

Author: hejianxin79

LinkerHand/__pycache__/linker_hand_api.cpython-38.pyc

@@ -3,6 +3,7 @@
 import can
 import threading
 from enum import Enum
+import numpy as np
 
 class FrameProperty(Enum):
     INVALID_FRAME_PROPERTY = 0x00  # Invalid CAN frame property | No return
@@ -33,6 +34,28 @@ def __init__(self, can_channel='can0', baudrate=1000000, can_id=0x28):
         self.can_id = can_id
         self.running = True
         self.x05, self.x06, self.x07 = [],[],[]
+        # New pressure sensors
+        self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+        
+        self.thumb_matrix = np.full((12, 6), -1)
+        self.index_matrix = np.full((12, 6), -1)
+        self.middle_matrix = np.full((12, 6), -1)
+        self.ring_matrix = np.full((12, 6), -1)
+        self.little_matrix = np.full((12, 6), -1)
+        self.matrix_map = {
+            0: 0,
+            16: 1,
+            32: 2,
+            48: 3,
+            64: 4,
+            80: 5,
+            96: 6,
+            112: 7,
+            128: 8,
+            144: 9,
+            160: 10,
+            176: 11,
+        }
 
         # Initialize CAN bus according to operating system
         if sys.platform == "linux":
@@ -186,6 +209,48 @@ def process_response(self, msg):
             elif frame_type == 0x23:
                 d = list(response_data)
                 self.approach_inc = [float(i) for i in d]
+            elif frame_type == 0xb0:
+                self.xb0 = list(response_data)
+            elif frame_type == 0xb1:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb1 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.thumb_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb2:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb2 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.index_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb3:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb3 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.middle_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb4:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb4 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.ring_matrix[index] = d[1:]  # Remove the first flag bit
+            elif frame_type == 0xb5:
+                d = list(response_data)
+                if len(d) == 2:
+                    self.xb5 = d
+                elif len(d) == 7:
+                    index = self.matrix_map.get(d[0])
+                    if index is not None:
+                        self.little_matrix[index] = d[1:]  # Remove the first flag bit
     def pose_slice(self, p):
         """Slice the joint array into finger action arrays"""
         try:
@@ -243,14 +308,36 @@ def get_temperature(self):
     def clear_faults(self):
         '''Clear motor faults'''
         self.send_command(0x07, [1, 1, 1, 1, 1])
-
     def get_touch_type(self):
-        '''Get touch type, not supported'''
-        return [-1] * 5
+        '''Get touch type'''
+        self.send_command(0xb0,[],sleep=0.03)
+        self.send_command(0xb1,[],sleep=0.03)
+        t = []
+        for i in range(3):
+            t = self.xb1
+            time.sleep(0.01)
+        if len(t) == 2:
+            return 2
+        else:
+            return -1
 
     def get_touch(self):
-        '''Get touch data, not supported'''
-        return [-1] * 6
+        '''Get touch data'''
+        self.send_command(0xb1,[],sleep=0.03)
+        self.send_command(0xb2,[],sleep=0.03)
+        self.send_command(0xb3,[],sleep=0.03)
+        self.send_command(0xb4,[],sleep=0.03)
+        self.send_command(0xb5,[],sleep=0.03)
+        return [self.xb1[1],self.xb2[1],self.xb3[1],self.xb4[1],self.xb5[1],0] # The last digit is palm, currently not available
+
+    def get_matrix_touch(self):
+        self.send_command(0xb1,[0xc6],sleep=0.04)
+        self.send_command(0xb2,[0xc6],sleep=0.04)
+        self.send_command(0xb3,[0xc6],sleep=0.04)
+        self.send_command(0xb4,[0xc6],sleep=0.04)
+        self.send_command(0xb5,[0xc6],sleep=0.04)
+        return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
 
     def get_faults(self):
         '''Get motor fault codes'''

LinkerHand/core/can/linker_hand_l20_can.py

@@ -8,7 +8,7 @@
 from utils.open_can import OpenCan
 
 class LinkerHandApi:
-    def __init__(self, hand_type="left", hand_joint="L10", modbus = "None",can="can0"):  # Ubuntu:can0   win:PCAN_USBBUS1
+    def __init__(self, hand_type="left", hand_joint="L10", modbus = "None",can="can0"):
         self.last_position = []
         self.yaml = LoadWriteYaml()
         self.config = self.yaml.load_setting_yaml()
@@ -53,9 +53,9 @@ def __init__(self, hand_type="left", hand_joint="L10", modbus = "None",can="can0
         if sys.platform == "linux":
             self.open_can = OpenCan(load_yaml=self.yaml)
             self.open_can.open_can(self.can)
-            self.is_can = self.open_can.is_can_up_sysfs()
+            self.is_can = self.open_can.is_can_up_sysfs(interface=self.can)
             if not self.is_can:
-                ColorMsg(msg="CAN0 interface is not open", color="red")
+                ColorMsg(msg=f"{self.can} interface is not open", color="red")
                 sys.exit(1)
 
     # Five-finger movement
@@ -226,4 +226,4 @@ def close_can(self):
         self.open_can.close_can0()                         
 
 if __name__ == "__main__":
-    hand = LinkerHandApi(hand_type="right", hand_joint="L10")
+    hand = LinkerHandApi(hand_type="right", hand_joint="L10")