init repo

tactile_sensor_daq.py

@@ -0,0 +1,228 @@
+import threading
+import time
+import numpy as np
+from enum import Enum
+# 导入官方例程的底层依赖
+from class_ch341 import *
+from class_sensorcmd import *
+from class_finger import *
+
+# === 配置区域 ===
+DEF_MAX_FINGER_NUM = 3  # 修改为 3 个传感器 (Finger 0, 1, 2)
+PCA_ADDR = 0x70  # I2C 多路复用器地址
+SAMPLE_RATE_MS = 10  # 采样间隔 (ms)，10ms = 100Hz
+
+
+class EnumCh341ConnectStatus(Enum):
+    CH341_CONNECT_INIT = 0
+    CH341_CONNECT_OPEN = 1
+    CH341_CONNECT_SET_SPEED = 2
+    CH341_CONNECT_SAMPLE_START = 3
+    CH341_CONNECT_CHECK = 4
+    CH341_CONNECT_SAMPLE_STOP = 5
+
+
+class TactileSensorDAQ:
+    def __init__(self):
+        # 1. 硬件初始化
+        self.ch341 = ClassCh341()
+        self.fingers = list()
+        # 初始化3个传感器对象，ID从2开始 (假设硬件拨码是 2,3,4)
+        for i in range(DEF_MAX_FINGER_NUM):
+            self.fingers.append(ClassFinger(4 + i, self.ch341))
+
+        # 2. 状态机变量
+        self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_INIT
+        self.ch341CheckTimer = 0
+        self.pcaAddr = PCA_ADDR
+        self.syncTimer = 0
+
+        # 3. 数据容器 (核心修改)
+        # 12维数据: [F0_U1_Fn, F0_U1_Ft, F0_U2_Fn, F0_U2_Ft, F1..., F2...]
+        self.raw_data = np.zeros(12, dtype=np.float32)  # 实时读取值
+        self.offset = np.zeros(12, dtype=np.float32)  # 去皮偏移量
+        self.clean_data = np.zeros(12, dtype=np.float32)  # 输出值 (Raw - Offset)
+
+        # 4. 线程控制
+        self.running = False
+        self.lock = threading.Lock()  # 线程锁，保证读取安全
+        self.thread = None
+
+    def _set_sensor_enable(self, idx):
+        """控制 I2C 多路复用器通道"""
+        _pack = list()
+        _pack.append(idx)
+        self.ch341.write(self.pcaAddr, _pack)
+
+    def _update_state_machine(self):
+        """维持 CH341 连接状态机 (原 logic 的简化版)"""
+        if self.connectStatus == EnumCh341ConnectStatus.CH341_CONNECT_INIT:
+            if self.ch341.init():
+                self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_OPEN
+
+        elif self.connectStatus == EnumCh341ConnectStatus.CH341_CONNECT_OPEN:
+            if self.ch341.open():
+                self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_SET_SPEED
+            else:
+                self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_INIT
+
+        elif self.connectStatus == EnumCh341ConnectStatus.CH341_CONNECT_SET_SPEED:
+            if self.ch341.set_speed(self.ch341.IIC_SPEED_400):
+                self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_SAMPLE_START
+            else:
+                self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_SAMPLE_START  # Retry
+
+        elif self.connectStatus == EnumCh341ConnectStatus.CH341_CONNECT_SAMPLE_START:
+            # 连接建立成功，进入读取循环
+            self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_CHECK
+
+        elif self.connectStatus == EnumCh341ConnectStatus.CH341_CONNECT_CHECK:
+            # 这里的检查逻辑放入主循环中执行
+            pass
+
+    def _read_hardware(self):
+        """读取所有传感器数据的核心函数"""
+        connectedSensorChan = 0
+
+        # 临时列表存储本轮读取的数据
+        temp_data_buffer = []
+
+        for fingerIndex in range(len(self.fingers)):
+            # 1. 切通道
+            self._set_sensor_enable(1 << (self.fingers[fingerIndex].pcaIdx))
+            connectedSensorChan |= 1 << (self.fingers[fingerIndex].pcaIdx)
+
+            current_finger = self.fingers[fingerIndex]
+
+            # 2. 检查连接与读取
+            if not current_finger.connect:
+                if current_finger.checkSensor():
+                    print(f"[System] Finger {fingerIndex} Connected!")
+            else:
+                current_finger.capRead()
+
+                # 3. 提取数据 (这是修改的关键！)
+                # 假设每个传感器有 ydds_num (通常是2) 个单元
+                # 这里的 nf 和 tf 应该是数组
+                for unit_i in range(current_finger.projectPara.ydds_num):
+                    # 提取法向力 Fn
+                    fn = current_finger.readData.nf[unit_i]
+                    # 提取切向力 Ft
+                    ft = current_finger.readData.tf[unit_i]
+
+                    temp_data_buffer.append(fn)
+                    temp_data_buffer.append(ft)
+
+        # 4. 更新共享内存
+        if len(temp_data_buffer) == 12:  # 确保数据完整
+            with self.lock:
+                self.raw_data = np.array(temp_data_buffer, dtype=np.float32)
+                # 计算去皮后的数据
+                self.clean_data = self.raw_data - self.offset
+                # 简单滤波：置零负值噪声
+                # self.clean_data[self.clean_data < 0] = 0
+
+        # 5. 同步逻辑 (保持原厂逻辑，防止电容漂移)
+        if (time.time() - self.syncTimer) > 1.0:  # 1秒同步一次
+            self.syncTimer = time.time()
+            self._set_sensor_enable(connectedSensorChan)
+            for f in self.fingers:
+                if f.connect:
+                    f.snsCmd.setSensorSync(0)
+                    break
+
+    def _thread_worker(self):
+        """后台线程主循环"""
+        while self.running:
+            # 1. 维护连接
+            if self.connectStatus != EnumCh341ConnectStatus.CH341_CONNECT_CHECK:
+                self._update_state_machine()
+                time.sleep(0.1)
+                continue
+
+            # 2. 读取数据
+            start_time = time.time()
+            try:
+                self._read_hardware()
+            except Exception as e:
+                print(f"Read Error: {e}")
+
+            # 3. 检查连接心跳 (保持原厂逻辑)
+            self.ch341CheckTimer += (time.time() - start_time) * 1000
+            if self.ch341CheckTimer >= 1000:
+                self.ch341CheckTimer = 0
+                if not self.ch341.connectCheck():
+                    print("CH341 Disconnected!")
+                    self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_INIT
+
+            # 4. 控制采样率
+            elapsed = (time.time() - start_time) * 1000
+            sleep_time = (SAMPLE_RATE_MS - elapsed) / 1000.0
+            if sleep_time > 0:
+                time.sleep(sleep_time)
+
+    # === 用户API ===
+
+    def start(self):
+        """启动采集"""
+        if self.running: return
+        self.running = True
+        self.thread = threading.Thread(target=self._thread_worker)
+        self.thread.daemon = True
+        self.thread.start()
+        print("Tactile Sensor System Started.")
+
+    def stop(self):
+        """停止采集"""
+        self.running = False
+        if self.thread:
+            self.thread.join()
+        self.ch341.disconnect()
+        print("Tactile Sensor System Stopped.")
+
+    def tare(self):
+        """去皮：将当前读数设为零点"""
+        print("Taring sensors... please wait.")
+        time.sleep(1)  # 等待数据稳定
+        with self.lock:
+            self.offset = np.copy(self.raw_data)
+        print("Tare complete.")
+
+    def get_data(self):
+        """获取最新的12维力数据"""
+        with self.lock:
+            return np.copy(self.clean_data)
+
+
+# === 调试代码 (直接运行此文件测试) ===
+if __name__ == "__main__":
+    sensor = TactileSensorDAQ()
+    sensor.start()
+
+    try:
+        # 等待连接稳定
+        print("Waiting for sensors to connect...")
+        time.sleep(3)
+
+        sensor.tare()  # 初始去皮
+
+        while True:
+            data = sensor.get_data()
+            log_str = ""
+            for i in range(3):  # 遍历 3 个手指
+                base_idx = i * 4
+                # 为了显示简洁，我们将 Unit1 和 Unit2 的 Fn 相加，作为一个总压力显示
+                # 你也可以根据需要显示全部细节
+                f_n_total = data[base_idx] + data[base_idx + 2]
+                f_t_total = data[base_idx + 1] + data[base_idx + 3]
+
+                log_str += f"F{i}: N={f_n_total:.1f} T={f_t_total:.1f} | "
+
+            print(log_str)
+            # === 修改结束 ===
+
+
+            time.sleep(0.1)  # 打印频率
+
+    except KeyboardInterrupt:
+        sensor.stop()