228 lines
8.3 KiB
Python
228 lines
8.3 KiB
Python
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() |