commit 111c9bab8a87d602bab451e1393c94c25a5d2937 Author: Brunsmeier <2970937094@qq.com> Date: Thu Jul 16 14:56:43 2026 +0800 init repo diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..8e1fe07 --- /dev/null +++ b/.gitignore @@ -0,0 +1,42 @@ +# Python bytecode and caches +__pycache__/ +*.py[cod] +*$py.class + +# Virtual environments +.venv/ +venv/ +env/ +ENV/ + +# Build, packaging, and test output +build/ +dist/ +*.egg-info/ +.pytest_cache/ +.coverage +htmlcov/ + +# IDE and editor settings +.vscode/ +.idea/ +*.swp +*.swo +*~ + +# Operating-system files +.DS_Store +Thumbs.db + +# Runtime logs and generated captures +*.log +*.tmp +*.bak + +# Generated sensor/control recordings +data/*.csv + +# Local hardware configuration and secrets +.env +.env.* +*.local diff --git a/CH341驱动（先卸载再安装）.zip b/CH341驱动（先卸载再安装）.zip new file mode 100644 index 0000000..4c9db36 Binary files /dev/null and b/CH341驱动（先卸载再安装）.zip differ diff --git a/CH341驱动（先卸载再安装）/CH341PAR (1).EXE b/CH341驱动（先卸载再安装）/CH341PAR (1).EXE new file mode 100755 index 0000000..2493e7e Binary files /dev/null and b/CH341驱动（先卸载再安装）/CH341PAR (1).EXE differ diff --git a/README.md b/README.md new file mode 100644 index 0000000..44ee44c --- /dev/null +++ b/README.md @@ -0,0 +1,717 @@ +# 触觉传感器与执行器控制系统 —— 使用手册 + +## 项目概述 + +本项目提供了一套完整的触觉传感器数据采集与执行器（舵机、直线电机）控制系统，包含底层 I2C 通信驱动、传感器数据解析、多指数据采集、串口执行器控制、手柄/键盘遥控以及数据记录等功能模块。 + +### 系统架构 + +``` +┌──────────────────────────────────────────────────────┐ +│ 应用层 (Application) │ +│ demo_auto_grasp.py test_dof_control.py │ +│ gamepad_remote.py gamepad_remote_new_pcb.py │ +│ keyboard_remote_new_pcb.py data_logger.py │ +│ grasp_network_model.py │ +├──────────────────────────────────────────────────────┤ +│ 数据采集层 (DAQ Layer) │ +│ cap_read.py tactile_sensor_daq.py │ +├──────────────────────────────────────────────────────┤ +│ 驱动抽象层 (Driver Layer) │ +│ class_finger.py class_sensorcmd.py sensorPara.py │ +│ class_ch341.py serial_robot_driver.py │ +├──────────────────────────────────────────────────────┤ +│ 硬件层 (Hardware) │ +│ CH341 (USB-I2C) STM32F103 (Serial) │ +│ 触觉传感器 舵机 / 直线电机 │ +└──────────────────────────────────────────────────────┘ +``` + +--- + +## 目录 + +1. [环境准备](#1-环境准备) +2. [模块说明](#2-模块说明) +3. [快速开始](#3-快速开始) +4. [底层驱动详解](#4-底层驱动详解) +5. [数据采集模块](#5-数据采集模块) +6. [执行器控制](#6-执行器控制) +7. [遥控模块](#7-遥控模块) +8. [数据记录](#8-数据记录) +9. [神经网络模型](#9-神经网络模型) +10. [STM32 固件](#10-stm32-固件) +11. [硬件接线](#11-硬件接线) +12. [常见问题](#12-常见问题) + +--- + +## 1. 环境准备 + +> **重要：请严格按照以下顺序操作，先安装驱动，再安装 Python 依赖。** + +### 1.1 CH341 驱动安装（必须首先完成） + +CH341 是 USB 转 I2C 芯片，用于与触觉传感器通信。**使用本项目前，必须先安装此驱动**，否则传感器无法被电脑识别。 + +#### Windows + +1. 在项目根目录下找到压缩包 **`CH341驱动（先卸载再安装）.zip`**，将其解压。 +2. **如果之前安装过其它版本的 CH341 驱动，请先卸载**。 +3. 运行解压后的 `CH341PAR.exe`，按照提示完成安装。 +4. 安装完成后，将传感器通过 CH341 转接板连接电脑 USB，系统会自动识别设备。 + +> 驱动安装后，项目代码会自动加载 `lib/ch341/CH341DLLA64.DLL`，无需额外配置。 + +#### Linux + +1. 将 `lib/ch341/libch347.so` 放置到系统库路径或项目目录下。 + +2. 配置 udev 规则（将 `john` 替换为你的用户名）： +```bash +# 将用户添加到 dialout 组 +sudo usermod -a -G dialout john + +# 创建 udev 规则文件 +sudo bash -c 'echo "SUBSYSTEM==\"usbmisc\", ATTRS{idVendor}==\"1a86\", ATTRS{idProduct}==\"5512\", MODE=\"0666\", GROUP=\"dialout\"" > /etc/udev/rules.d/ch341.rules' + +# 重载规则 +sudo udevadm control --reload-rules +sudo udevadm trigger +``` + +3. 重新插拔 CH341 设备，或重启计算机使 udev 规则生效。 + +### 1.2 Python 依赖 + +推荐使用 Python 3.8 及以上版本。安装依赖： + +```bash +pip install pyserial numpy pygame keyboard torch +``` + +各模块依赖说明： + +| 模块 | 依赖 | +|------|------| +| 底层驱动 (class_ch341, class_sensorcmd, class_finger) | 无额外依赖 | +| 数据采集 (cap_read, tactile_sensor_daq) | numpy | +| 串口执行器 (serial_robot_driver) | pyserial | +| 手柄遥控 (gamepad_remote, gamepad_remote_new_pcb) | pyserial, pygame | +| 键盘遥控 (keyboard_remote_new_pcb) | pyserial, keyboard, numpy | +| 数据记录 (data_logger) | numpy | +| 神经网络 (grasp_network_model) | torch (PyTorch) | + +> **说明：** 如果仅使用传感器数据读取功能（cap_read、tactile_sensor_daq），只需安装 `numpy`。执行器控制、遥控、神经网络等模块可按需安装对应依赖。 + +--- + +## 2. 模块说明 + +### 2.1 文件清单 + +| 文件名 | 功能说明 | +|--------|----------| +| `class_ch341.py` | CH341 USB-I2C 底层驱动，封装 I2C 读写、速度设置、INT 引脚控制 | +| `class_sensorcmd.py` | 传感器 I2C 命令协议，封装所有传感器配置与读取命令 | +| `class_finger.py` | 传感器（手指）数据解析，包含电容/力数据读取与协议解析 | +| `sensorPara.py` | 传感器参数定义（通道数、力单元数、数据格式等） | +| `cap_read.py` | 基础电容读取入口，支持单指/多指数据采集与实时可视化 | +| `tactile_sensor_daq.py` | 多指触觉传感器数据采集模块（线程安全，支持去皮） | +| `serial_robot_driver.py` | 串口执行器驱动，控制舵机和直线电机 | +| `stm32f103_drv8870_servo_main.c` | STM32F103 下位机固件源码 | +| `demo_auto_grasp.py` | 演示程序：触觉反馈自动抓取任务 | +| `gamepad_remote.py` | 游戏手柄遥控程序 | +| `gamepad_remote_new_pcb.py` | 新 PCB 版手柄遥控（支持 ACK 协议和自检） | +| `keyboard_remote_new_pcb.py` | 新 PCB 版键盘遥控（支持力反馈与压力计算） | +| `test_dof_control.py` | 自由度独立控制测试 | +| `data_logger.py` | CSV 数据记录器 | +| `grasp_network_model.py` | 条件抓取网络模型（Conditional MLP） | +| `接线说明.txt` | 硬件接线参考 | +| `新PCB控制实施方案.md` | 新 PCB 控制系统实施方案 | +| `CH341驱动（先卸载再安装）.zip` | CH341 驱动程序压缩包，使用前必须先解压安装 | +| `传感器iic地址和转接板位置定义.png` | I2C 地址与接口位置对照图 | + +--- + +## 3. 快速开始 + +### 3.1 仅读取单个传感器数据 + +```bash +python cap_read.py +``` + +运行前可在 `cap_read.py` 中修改传感器连接数量： + +```python +DEF_MAX_FINGER_NUM = 1 # 需要连接的手指数量，最大5个 +``` + +输出数据包括： +- `capChannelDat`：电容通道原始值 +- `nf[i]`：第 i 个单元的法向力 +- `tf[i]`：第 i 个单元的切向力 +- `tfDir[i]`：第 i 个单元的切向力方向 +- `sProxCapData`：自电容接近值 +- `mProxCapData`：互电容接近值 + +### 3.2 读取多个传感器数据（100Hz） + +```python +from tactile_sensor_daq import TactileSensorDAQ + +sensor = TactileSensorDAQ() +sensor.start() + +# 等待传感器连接稳定 +import time +time.sleep(3) +sensor.tare() # 去皮 + +while True: + data = sensor.get_data() # 获取 12 维力数据 + print(data) + time.sleep(0.1) +``` + +### 3.3 控制执行器（串口） + +```python +from serial_robot_driver import RobotDriver + +robot = RobotDriver(port='COM9') +robot.motor_open() # 直线电机张开 +robot.motor_close() # 直线电机闭合 +robot.motor_stop() # 停止 +robot.set_servo(1, 90) # 舵机1转到90度 +robot.set_config(0) # 构型0（初始位） +robot.close() +``` + +### 3.4 命令行快速测试 + +```bash +# 测试通信 +python gamepad_remote_new_pcb.py --port COM9 --cmd PING + +# 硬件自检 +python gamepad_remote_new_pcb.py --port COM9 --test + +# 手柄遥控 +python gamepad_remote_new_pcb.py --port COM9 + +# 键盘遥控 +python keyboard_remote_new_pcb.py --port COM9 +``` + +--- + +## 4. 底层驱动详解 + +### 4.1 class_ch341.py —— CH341 I2C 通信驱动 + +封装 CH341 USB-I2C 转换芯片的操作。 + +**主要接口：** + +```python +from class_ch341 import ClassCh341 + +ch341 = ClassCh341() + +# 初始化并打开设备 +ch341.init() # 加载 DLL/SO 库 +ch341.open() # 打开 USB 设备 + +# 设置 I2C 速度 +ch341.set_speed(ch341.IIC_SPEED_400) # 20/100/400/750 kHz + +# I2C 读写 +ch341.write(addr, data_list) # 向从机地址写入数据列表 +ch341.read(addr, data_list) # 从从机地址读取数据到列表 + +# 连接检查 +ch341.connectCheck() # 返回 True/False + +# 断开 +ch341.disconnect() +``` + +### 4.2 class_sensorcmd.py —— 传感器命令协议 + +封装与触觉传感器 MCU 的 I2C 命令协议。 + +**主要接口：** + +```python +from class_sensorcmd import ClassSensorCmd + +snsCmd = ClassSensorCmd(ch341) + +# 地址管理 +snsCmd.getAddr(addr) # 读取传感器 I2C 地址 +snsCmd.setAddr(old_addr, new) # 设置新地址 + +# 传感器配置 +snsCmd.setSensorSendType(addr, 0) # 设置数据发送类型为原始值 +snsCmd.setSensorCapOffset(addr, offset) # 设置电容采集时序偏移 + +# 读取数据 +snsCmd.getSensorCapData(addr, buf) # 读取电容数据 +snsCmd.getSensorProjectIdex(addr) # 读取项目编号 + +# 同步 +snsCmd.setSensorSync(addr) # 多传感器同步 +``` + +### 4.3 sensorPara.py —— 传感器参数定义 + +定义了传感器项目的参数结构和具体参数值。 + +```python +from sensorPara import finger_params, FingerParamTS, DynamicYddsU16Ts + +# finger_params 包含所有支持的传感器类型 +# 目前支持: +# - 项目2: "通用手指", 8通道, 1个三维力单元 +# - 项目17: "两指-大包", 16通道, 2个三维力单元 +``` + +如需添加新的传感器型号，在 `sensorPara.py` 中追加 `FingerParamTS` 条目即可。 + +### 4.4 class_finger.py —— 传感器数据解析 + +管理单个传感器（手指）的连接状态和数据读取。 + +```python +from class_finger import ClassFinger, capData + +finger = ClassFinger(pca_idx=2, ch341=ch341) + +# 检查传感器连接 +if finger.checkSensor(): + print("Sensor connected") + +# 读取数据 +finger.capRead() + +# 访问数据 +finger.readData.channelCapData # 电容通道原始值 +finger.readData.nf[i] # 第i个单元的法向力 +finger.readData.tf[i] # 第i个单元的切向力 +finger.readData.tfDir[i] # 第i个单元的切向力方向 +finger.readData.sProxCapData # 自电容接近值 +finger.readData.mProxCapData # 互电容接近值 +``` + +--- + +## 5. 数据采集模块 + +### 5.1 cap_read.py —— 基础电容读取 + +最简单直接的传感器数据读取入口。内部维护 CH341 连接状态机，定时轮询读取电容数据，并通过 TCP Socket 发送到 VOFA+ 等调试工具进行实时可视化。 + +**配置参数：** + +```python +DEF_MAX_FINGER_NUM = 1 # 连接手指数量 (1-5) +DEF_GET_CAP_MS = 30 # 读取间隔 (ms) +DEF_CDC_SYNC_MS = 1000 # 电容同步间隔 (ms)，多传感器时使用 +``` + +**运行：** + +```bash +python cap_read.py +``` + +数据通过 Socket 发送到 `127.0.0.1:1347`（VOFA+ 默认端口）。 + +### 5.2 tactile_sensor_daq.py —— 多指触觉传感器 DAQ + +线程安全的传感器数据采集模块，支持 3 个手指、12 维力数据输出，含去皮（tare）功能。 + +```python +from tactile_sensor_daq import TactileSensorDAQ + +sensor = TactileSensorDAQ() +sensor.start() # 启动后台采集线程 +sensor.tare() # 去皮：将当前读数归零 +data = sensor.get_data() # 获取 12 维 numpy 数组 [F0_U1_Fn, F0_U1_Ft, F0_U2_Fn, ...] +sensor.stop() # 停止采集 +``` + +**数据格式**（12 维 float32）： + +| 索引 | 含义 | +|------|------| +| 0-1 | 手指0-单元1: 法向力(Fn), 切向力(Ft) | +| 2-3 | 手指0-单元2: 法向力(Fn), 切向力(Ft) | +| 4-5 | 手指1-单元1: 法向力(Fn), 切向力(Ft) | +| 6-7 | 手指1-单元2: 法向力(Fn), 切向力(Ft) | +| 8-9 | 手指2-单元1: 法向力(Fn), 切向力(Ft) | +| 10-11| 手指2-单元2: 法向力(Fn), 切向力(Ft) | + +**配置参数：** + +```python +DEF_MAX_FINGER_NUM = 3 # 传感器数量 +SAMPLE_RATE_MS = 10 # 采样间隔 (ms)，默认 100Hz +``` + +--- + +## 6. 执行器控制 + +### 6.1 serial_robot_driver.py —— 串口执行器驱动 + +通过串口（USB-TTL）向 STM32 下位机发送 ASCII 命令，控制直线电机和舵机。 + +```python +from serial_robot_driver import RobotDriver + +robot = RobotDriver(port='COM9', baud=115200) + +# 直线电机控制 +robot.motor_open() # 张开 +robot.motor_close() # 闭合 +robot.motor_stop() # 停止 + +# 舵机控制 +robot.set_servo(1, 90) # 舵机1转到90度 +robot.set_servo(2, 120) # 舵机2转到120度 + +# 构型切换（组合动作） +robot.set_config(0) # 初始构型: S1=90, S2=90 +robot.set_config(1) # 错位构型: S1=30, S2=150 +robot.set_config(2) # 对握构型: S1=120, S2=60 + +robot.close() +``` + +### 6.2 串口通信协议 + +上位机向 STM32 发送 ASCII 命令（以 `\r\n` 结尾），STM32 回复 `OK:` 或 `ERR:` 确认。 + +| 命令 | 功能 | 回复 | +|------|------|------| +| `PING` | 通信测试 | `OK:PONG` | +| `M:OPEN` | 直线电机张开 | `OK:M:OPEN` | +| `M:CLOSE` | 直线电机闭合 | `OK:M:CLOSE` | +| `M:STOP` | 直线电机停止 | `OK:M:STOP` | +| `S1:` | 舵机1角度(0-180) | `OK:S1` | +| `S2:` | 舵机2角度(0-180) | `OK:S2` | +| `CFG:` | 构型切换(0/1/2) | `OK:CFG` | + +### 6.3 gamepad_remote_new_pcb.py —— 新 PCB 版执行器驱动 + +相比 `serial_robot_driver.py`，增加了 ACK 确认、命令行参数、自检等功能。 + +```bash +# 单条命令 +python gamepad_remote_new_pcb.py --port COM9 --cmd PING +python gamepad_remote_new_pcb.py --port COM9 --cmd S1:90 + +# 硬件自检 +python gamepad_remote_new_pcb.py --port COM9 --test + +# 不等待 ACK (适用于简单固件) +python gamepad_remote_new_pcb.py --port COM9 --no-ack +``` + +--- + +## 7. 遥控模块 + +### 7.1 gamepad_remote.py —— 手柄遥控 + +使用 Xbox/兼容游戏手柄远程控制执行器。 + +**按键映射：** + +| 按键 | 功能 | +|------|------| +| A | 构型0（初始） | +| B | 构型1（错位） | +| X | 构型2（对握） | +| LB 长按 | 直线电机张开 | +| RB 长按 | 直线电机闭合 | +| 松开 LB/RB | 电机停止 | +| BACK | 急停切换 | +| START | 退出 | + +**运行：** + +```bash +python gamepad_remote.py +``` + +默认连接 COM9，可在代码中修改 `port` 变量。 + +### 7.2 keyboard_remote_new_pcb.py —— 键盘遥控 + +使用键盘远程控制执行器，同时读取触觉传感器数据，支持力反馈自动抓取。 + +**按键映射：** + +| 按键 | 功能 | +|------|------| +| q | 构型0（初始） | +| w | 构型1（错位） | +| e | 构型2（对握） | +| ← 长按 | 直线电机闭合 | +| → 长按 | 直线电机张开 | +| 空格 | 急停/恢复 | +| Esc | 退出 | + +**主要功能：** +- 按下闭合键持续闭合，松开自动停止并打印执行时间 +- 实时显示接触压力（KPa） +- 支持急停保护和构型切换 + +**运行：** + +```bash +python keyboard_remote_new_pcb.py --port COM9 +python keyboard_remote_new_pcb.py --port COM9 --test # 自检模式 +python keyboard_remote_new_pcb.py --port COM9 --contact-area-mm2 240 # 自定义接触面积 +``` + +--- + +## 8. 数据记录 + +### data_logger.py —— CSV 数据记录器 + +将触觉传感器数据记录为 CSV 文件，自动创建 `data/` 目录并以时间戳命名。 + +```python +from data_logger import DataLogger + +logger = DataLogger(filename_prefix="experiment_01") + +# 记录一行数据 +# sensor_data: 12维触觉数据 +# config_id: 构型ID (0/1/2) +# label_vector: 标签 [x, y, theta] +logger.log(sensor_data=sensor.get_data(), config_id=1, label_vector=[0.0, 5.5, -2.0]) +``` + +**CSV 格式：** + +| Timestamp | Config_ID | F0_U1_Fn | F0_U1_Ft | ... | Label_X | Label_Y | Label_Theta | +|-----------|-----------|----------|----------|-----|---------|---------|-------------| + +--- + +## 9. 神经网络模型 + +### grasp_network_model.py —— 条件抓取网络 (CondGraspNet) + +一个条件 MLP 网络，输入 12 维触觉数据 + 3 维构型编码，输出 3 维偏差预测 [ΔX, ΔY, Δθ]。 + +**网络结构：** + +``` +Input(15) → FC(64)+BN+ReLU → FC(128)+BN+ReLU → FC(64)+ReLU → Output(3) +``` + +**使用示例：** + +```python +from grasp_network_model import CondGraspNet +import torch + +model = CondGraspNet() +tactile_data = torch.randn(8, 12) # Batch=8, 12维触觉 +config_ids = torch.tensor([0,0,1,1,2,2,0,2]) # 构型ID +predictions = model(tactile_data, config_ids) # [8, 3] 输出 +``` + +**单元测试：** + +```bash +python grasp_network_model.py +``` + +--- + +## 10. STM32 固件 + +### stm32f103_drv8870_servo_main.c + +STM32F103C8T6 下位机固件，负责接收上位机串口命令并控制硬件。 + +**硬件映射：** + +| 功能 | STM32引脚 | 说明 | +|------|-----------|------| +| USART1 TX | PA9 | 连接 USB-TTL RXD | +| USART1 RX | PA10 | 连接 USB-TTL TXD | +| 直线电机 IN1 | PB12 | 连接 DRV8870 IN1 | +| 直线电机 IN2 | PB13 | 连接 DRV8870 IN2 | +| 舵机1 PWM | PB11 | TIM2_CH4, 50Hz | +| 舵机2 PWM | PB10 | TIM2_CH3, 50Hz | + +**CubeMX 配置要点：** + +- TIM2: Prescaler=71, Counter Period=19999, 产生 50Hz PWM +- USART1: 115200 8N1 +- 需开启 TIM2 重映射（AFIO Remap） + +**集成方法：** + +将 `stm32f103_drv8870_servo_main.c` 中的用户代码集成到 CubeMX 生成的工程： + +```c +int main(void) +{ + HAL_Init(); + SystemClock_Config(); + MX_GPIO_Init(); + MX_USART1_UART_Init(); + MX_TIM2_Init(); + + app_init(); // 初始化舵机和电机 + + while (1) { + app_loop(); // 主循环：处理串口命令 + 看门狗 + } +} +``` + +**安全特性：** + +- 1 秒无指令自动停止直线电机（通信丢失保护） +- 命令超长自动拒绝 + +### 调试顺序 + +1. 仅给 PCB/STM32 上电（不接电机和舵机） +2. `python gamepad_remote_new_pcb.py --port COM9 --cmd PING` → 应返回 `OK:PONG` +3. 用示波器测舵机 PWM 信号： + - `S1:0` → PB11 约 0.5ms 高电平 + - `S1:90` → PB11 约 1.5ms 高电平 + - `S1:180` → PB11 约 2.5ms 高电平 +4. 测直线电机控制引脚： + - `M:OPEN` → PB12=高, PB13=低 + - `M:CLOSE` → PB12=低, PB13=高 + - `M:STOP` → PB12=低, PB13=低 +5. 信号确认正确后，接舵机独立 5V 电源 +6. 最后接直线电机电源和负载 + +--- + +## 11. 硬件接线 + +### 11.1 传感器接线（CH341 USB-I2C） + +传感器通过 I2C 转接板连接，转接板通过 CH341 芯片与 PC 的 USB 连接。 + +I2C 地址与转接板接口位置为固定对应关系，详见 `传感器iic地址和转接板位置定义.png`。 + +### 11.2 执行器接线（USB-TTL + STM32） + +``` +USB-TTL STM32F103C8T6 + 3.3V → 3.3V + GND → GND + TXD → PA10 (RXD) + RXD → PA9 (TXD) + +STM32 外设 + PB12 → DRV8870 IN1 + PB13 → DRV8870 IN2 + PB11 → 舵机1 信号线 (A0绿) + PB10 → 舵机2 信号线 (A1橙) + GND → 舵机 GND + GND → 直线电机 GND +``` + +详见 `接线说明.txt`。 + +### 11.3 供电注意事项 + +- MG996R 舵机不可由 STM32 板载 5V 供电，需使用独立 5V 3A 以上电源 +- 舵机电源 GND、直线电机电源 GND、STM32 GND 必须共地 +- DRV8870 的 VM 电压须匹配直线电机额定电压 +- 直线电机首次测试建议只点动 0.5s，防止方向相反或机械顶死 +- 如果 M:OPEN 和 M:CLOSE 方向相反，可在固件中交换 PB12/PB13 输出逻辑，或交换电机两线 + +--- + +## 12. 常见问题 + +### Q: 运行 cap_read.py 提示 "ch341加载失败"？ + +A: 检查 `lib/ch341/` 目录下是否有对应平台的库文件： +- Windows: `CH341DLLA64.DLL` +- Linux: `libch347.so` + +并确保已安装 CH341 驱动程序。 + +### Q: 传感器无法连接？ + +A: +1. 确认传感器已上电 3 秒以上再运行程序 +2. 检查 I2C 地址是否与转接板接口匹配 +3. 运行后查看终端输出，程序会自动尝试匹配地址 + +### Q: 手柄遥控无法识别手柄？ + +A: +1. 确保手柄已通过蓝牙/USB 连接并被系统识别 +2. Windows 下可在"控制面板→设备和打印机"中验证 +3. 确保安装了 pygame 库 + +### Q: 串口命令无响应？ + +A: +1. 确认 COM 口号正确（Windows 在设备管理器中查看） +2. 确认波特率为 115200 +3. 用 `--cmd PING` 测试基本通信 +4. 检查 USB-TTL 的 TXD/RXD 是否交叉连接 + +### Q: 如何修改采样频率？ + +A: 修改对应文件中的配置参数： +- `cap_read.py`: `DEF_GET_CAP_MS` +- `tactile_sensor_daq.py`: `SAMPLE_RATE_MS` + +### Q: 如何支持更多传感器型号？ + +A: 在 `sensorPara.py` 的 `finger_params` 列表中追加 `FingerParamTS` 参数定义。传感器会自动根据项目编号匹配对应参数。 + +--- + +## 项目文件结构 + +``` +HandControl/ +├── class_ch341.py # CH341 I2C 底层驱动 +├── class_sensorcmd.py # 传感器 I2C 命令协议 +├── class_finger.py # 传感器数据解析 +├── sensorPara.py # 传感器参数定义 +├── cap_read.py # 基础电容读取入口 +├── tactile_sensor_daq.py # 多指触觉传感器 DAQ +├── serial_robot_driver.py # 串口执行器驱动 +├── gamepad_remote.py # 手柄遥控 +├── gamepad_remote_new_pcb.py # 新 PCB 手柄遥控 +├── keyboard_remote_new_pcb.py # 新 PCB 键盘遥控 +├── demo_auto_grasp.py # 演示：自动抓取任务 +├── test_dof_control.py # 自由度独立控制测试 +├── data_logger.py # CSV 数据记录器 +├── grasp_network_model.py # 抓取预测神经网络 +├── stm32f103_drv8870_servo_main.c # STM32 固件 +├── lib/ch341/ # CH341 驱动库 +│ ├── CH341DLLA64.DLL +│ ├── CH341DLLA64.LIB +│ ├── ch341_lib.h +│ └── libch347.so +├── 接线说明.txt # 硬件接线参考 +├── 新PCB控制实施方案.md # 控制系统实施方案 +├── 传感器iic地址和转接板位置定义.png # I2C 地址图 +└── README.md # 本文件 +``` diff --git a/cap_read.py b/cap_read.py new file mode 100644 index 0000000..8bb5e08 --- /dev/null +++ b/cap_read.py @@ -0,0 +1,199 @@ +from enum import * +import threading +import queue # 导入 queue 模块 +from class_ch341 import * +from class_sensorcmd import * +from class_finger import * +from collections import namedtuple +import socket + + + +DEF_CDC_SYNC_MS = 1000 #电容同步间隔 +DEF_GET_CAP_MS = (30) #读取电容间隔 +DEF_PRO_CYC = 100 + +DEF_MAX_FINGER_NUM = 1 #需要连接的手指数量，最大5个 + +# 定义一个全局的队列，用于线程间通信 +capReadQueue = queue.Queue() + +# 341通信 +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 ClassCapRead: + def __init__(self): + self.ch341 = ClassCh341() + + # 最大连接5个手指 + self.fingers = list() # 传感器列表 + for i in range(DEF_MAX_FINGER_NUM): + self.fingers.append(ClassFinger(2+i, self.ch341)) + + self.currCh341State = 0 # 当前ch341连接状态 + self.prevCh341State = 0 # 上次ch341连接状态 + + self.ch341CheckTimer = 0 + self.mcuInit = 0 + self.pcaAddr = 0x70 # iic控制芯片地址 + + self.ch341Init = 0 # ch341初始化标志位 + + self.syncTimer = 0 + + self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_INIT + + self.connectDebug() + + def __del__(self): + self.disConnectDebug() + self.ch341.disconnect() + print("ch341释放") + pass + + def connectDebug(self): + #连接到调试的服务器 + self.vofaClient = socket.socket() + addr = ('127.0.0.1', 1347) + try: + self.vofaClient.connect(addr) + # client.send('hello world\r\n'.encode()) + self.socketConnected = True + print('连接服务器成功') + except Exception as e: + self.socketConnected = False + print('连接服务器失败') + + def disConnectDebug(self): + if self.vofaClient: + self.vofaClient.close() + + def debugPrint(self): + if self.socketConnected == True: + fingerIndex = 0 + _log1 = "" + # 输出原始通道数值 + # for index in range(0, self.fingers[fingerIndex].projectPara.sensor_num): + # _log1 += str(self.fingers[fingerIndex].readData.channelCapData[index]) + # _log1 += ',' + for index in range(0, self.fingers[fingerIndex].projectPara.ydds_num): + _log1 += str(int(self.fingers[fingerIndex].readData.nf[index]*1000)) + _log1 += ',' + _log1 += str(int(self.fingers[fingerIndex].readData.tf[index]*1000)) + _log1 += ',' + _log1 += str(self.fingers[fingerIndex].readData.tfDir[index]) + _log1 += ',' + for index in range(0, self.fingers[fingerIndex].projectPara.s_prox_num): + _log1 += str(self.fingers[fingerIndex].readData.sProxCapData[index]) + _log1 += ',' + for index in range(0, self.fingers[fingerIndex].projectPara.m_prox_num): + _log1 += str(self.fingers[fingerIndex].readData.mProxCapData[index]) + _log1 += ',' + + _log1 += str(0) + _log1 += '\r\n' + #print(_log1) + if self.socketConnected == 1: + self.vofaClient.send(_log1.encode()) + + def set_sensor_enable(self, idx): + _pack = list() + _pack.append(idx) + self.ch341.write(self.pcaAddr, _pack) + + def ch341Connect(self): + if self.connectStatus == EnumCh341ConnectStatus.CH341_CONNECT_INIT: + if(True == self.ch341.init()): + self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_OPEN + elif self.connectStatus == EnumCh341ConnectStatus.CH341_CONNECT_OPEN: + if(True == 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(True == self.ch341.set_speed(self.ch341.IIC_SPEED_400)): + self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_SAMPLE_START + else: + print("set speed err") + self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_SAMPLE_START + + elif self.connectStatus == EnumCh341ConnectStatus.CH341_CONNECT_SAMPLE_START: + self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_CHECK + self.timer = threading.Timer(DEF_GET_CAP_MS/1000, self.capRead) + self.timer.start() + elif self.connectStatus == EnumCh341ConnectStatus.CH341_CONNECT_CHECK: + self.ch341CheckTimer += DEF_PRO_CYC + if(self.ch341CheckTimer >= 1000): + self.ch341CheckTimer = 0 + if(False == self.ch341.connectCheck()): + print("ch341 拔出") + self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_SAMPLE_STOP + elif self.connectStatus == EnumCh341ConnectStatus.CH341_CONNECT_SAMPLE_STOP: + self.syncTimer = 0 + for i in range(0, len(self.fingers)): + self.fingers[i].disconnected() + self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_INIT + else: + self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_INIT + + def capRead(self): + capReadTime = time.time() + ms_capReadTime = capReadTime + + connectedSensorChan = 0 + connectedSensorCnt = 0 + for fingerIndex in range(0, len(self.fingers)): + self.set_sensor_enable(1 << (self.fingers[fingerIndex].pcaIdx)) + connectedSensorChan |= 1 << (self.fingers[fingerIndex].pcaIdx) + + if self.fingers[fingerIndex].connect == False: + if True == self.fingers[fingerIndex].checkSensor(): + print(f"sensor[{fingerIndex}] connected") + else: + print(f"addr = {fingerIndex}, connected false") + else: + self.fingers[fingerIndex].capRead() + connectedSensorCnt += 1 + + self.debugPrint() + + # 大于1个传感器连接需要设置接近采集序列 + if connectedSensorCnt > 1 and (time.time() - self.syncTimer) > DEF_CDC_SYNC_MS: + self.syncTimer = time.time() + self.set_sensor_enable(connectedSensorChan) + for fingerIndex in range(0, len(self.fingers)): + if self.fingers[fingerIndex].connect is True: + self.fingers[fingerIndex].snsCmd.setSensorSync(0) + break + + if self.connectStatus == EnumCh341ConnectStatus.CH341_CONNECT_CHECK: + capReadTime = time.time() + difftime = int(capReadTime*1000-ms_capReadTime*1000) + #print(f"diffTime={difftime}") + #定时器在任务完成后重新启动 + if(difftime>DEF_GET_CAP_MS): + timer = threading.Timer(DEF_GET_CAP_MS/1000, self.capRead) + else: + timer = threading.Timer((DEF_GET_CAP_MS-difftime)/1000, self.capRead) + timer.start() + + +def capReadThread(): + # 线程的主体功能 + cap = ClassCapRead() + + while True: + cap.ch341Connect() + time.sleep(DEF_PRO_CYC/1000) + +def main(): + capReadThread() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/class_ch341.py b/class_ch341.py new file mode 100644 index 0000000..a7c40b3 --- /dev/null +++ b/class_ch341.py @@ -0,0 +1,267 @@ +import time +import os +import sys +from ctypes import * +import ctypes +import glob + +# ch341类：iic读写，int引脚读写，速度设置 +class ClassCh341: + # 接口固定宏 + _mCH341A_CMD_I2C_STREAM = 0xAA # I2C接口的命令包,从次字节开始为I2C命令流 + _mCH341A_CMD_I2C_STM_STA = 0x74 # I2C接口的命令流:产生起始位 + _mCH341A_CMD_I2C_STM_STO = 0x75 # I2C接口的命令流:产生停止位 + _mCH341A_CMD_I2C_STM_OUT = 0x80 # I2C接口的命令流:输出数据,位5-位0为长度,后续字节为数据,0长度则只发送一个字节并返回应答 + _mCH341A_CMD_I2C_STM_IN = 0xC0 # I2C接口的命令流:输入数据,位5-位0为长度,0长度则只接收一个字节并发送无应答 + _mCH341A_CMD_I2C_STM_MAX = 63 # I2C接口的命令流单个命令输入输出数据的最大长度 + _mCH341A_CMD_I2C_STM_SET = 0x60 # I2C接口的命令流:设置参数,位2=SPI的I/O数(0=单入单出,1=双入双出),位1位0=I2C速度(00=低速,01=标准,10=快速,11=高速) + _mCH341A_CMD_I2C_STM_US = 0x40 # I2C接口的命令流:以微秒为单位延时,位3-位0为延时值 + _mCH341A_CMD_I2C_STM_MS = 0x50 # I2C接口的命令流:以亳秒为单位延时,位3-位0为延时值 + _mCH341A_CMD_I2C_STM_DLY = 0x0F # I2C接口的命令流单个命令延时的最大值 + _mCH341A_CMD_I2C_STM_END = 0x00 # I2C接口的命令流:命令包提前结束 + + _mStateBitINT = 0x00000400 + + IIC_SPEED_20 = 0 + IIC_SPEED_100 = 1 + IIC_SPEED_400 = 2 + IIC_SPEED_750 = 3 + + def __init__(self): + self.deviceID = ctypes.c_uint32() + # A sensor may be optional for motor-only programs. Keep an explicit + # unopened state so cleanup does not attempt to close an invalid fd. + self.fd = None + + def init(self): + if os.name == 'nt': # Windows 环境 + libPath = os.path.dirname(sys.argv[0]) + r'/lib/ch341/CH341DLLA64.DLL' + elif os.name == 'posix': + libPath = './lib/ch341/libch347.so' + dllExist = os.path.exists(libPath) + if not dllExist: + print('未找到库文件') + return False + else: + try: + if os.name == 'nt': # Windows 环境 + self.ic = windll.LoadLibrary(libPath) # ch341接口 + + self.ch341GetInput = self.ic.CH341GetInput + self.ch341CloseDevice = self.ic.CH341CloseDevice + self.ch341WriteData = self.ic.CH341WriteData + self.ch341WriteRead = self.ic.CH341WriteRead + self.ch341SetOutput = self.ic.CH341SetOutput + self.ch341SetStream = self.ic.CH341SetStream + + elif os.name == 'posix': + self.ic = cdll.LoadLibrary(libPath) # ch341接口 + + self.ch341GetInput = self.ic.CH34xGetInput + self.ch341CloseDevice = self.ic.CH34xCloseDevice + self.ch341WriteData = self.ic.CH34xWriteData + self.ch341WriteRead = self.ic.CH34xWriteRead + self.ch341SetOutput = self.ic.CH34xSetOutput + self.ch341SetStream = self.ic.CH34xSetStream + + print("ch341加载成功") + return True + except Exception as e: + print("ch341加载失败") + return False + + # 判断ch341是否插入 + # return：0未插入，1插入 + def open(self): + if os.name == 'nt': # Windows 环境 + try: + self.fd = self.ic.CH341OpenDevice(0) + if self.fd == -1: + print("CH341 device open failed on Windows.") + return False + else: + self.fd = 0 #todo 改成扫描端口 + print("CH341 device opened successfully on Windows.") + return True + except Exception as e: + print(f"Error occurred while opening CH341 device on Windows: {e}") + return False + + elif os.name == 'posix': # Linux 环境 + try: + devices = glob.glob('/dev/ch34x_pis*') # 动态查找设备 + if devices: + device_path = devices[0].encode() + self.fd = self.ic.CH34xOpenDevice(device_path) + if self.fd == -1: + print("CH341 device open failed on Linux.") + return False + else: + print("CH341 device opened successfully on Linux.") + return True + else: + print("No CH341 device found on Linux.") + return False + except Exception as e: + print(f"Error occurred while opening CH341 device on Linux: {e}") + return False + + else: + print("Unsupported operating system.") + return False + + def disconnect(self): + close_device = getattr(self, "ch341CloseDevice", None) + if self.fd not in (None, -1) and callable(close_device): + close_device(self.fd) + self.fd = None + + def connectCheck(self): + if True == self.ch341GetInput(self.fd, ctypes.byref(self.deviceID)): + return True + else: + return False + + # iic写数据 + # addr:iic从机地址 + # data：要写的数据列表 + # return：写入长度，不一定正确 + def write(self, addr, data): + sLen = len(data) + tmpData = [] # 临时列表 + tmpLen = sLen # 发送数据 + + pack = [] # 发送列表 + cnt = 20 # 每包数量 + packNum = sLen // cnt # 拆包数量 + sLen %= cnt # 不足字节数 + + tmpData.extend(data) + + pack.append(self._mCH341A_CMD_I2C_STREAM) + pack.append(self._mCH341A_CMD_I2C_STM_STA) + pack.append(self._mCH341A_CMD_I2C_STM_OUT | 1) + pack.append(addr << 1) + for i in range(0, packNum): + pack.append(self._mCH341A_CMD_I2C_STM_OUT | cnt) + pack.extend(tmpData[0:20]) + del tmpData[0:20] + pack.append(self._mCH341A_CMD_I2C_STM_END) + sendBuf = (c_byte * len(pack))() + for j in range(0, len(pack)): + sendBuf[j] = pack[j] + sendLen = (c_byte * 1)() + sendLen[0] = len(pack) + if not self.ch341WriteData(self.fd, sendBuf, sendLen): + return 0 + if sendLen == 0: + return 0 + pack.clear() + pack.append(self._mCH341A_CMD_I2C_STREAM) + if sLen >= 1: + pack.append(self._mCH341A_CMD_I2C_STM_OUT | sLen) + pack.extend(tmpData[0:sLen]) + pack.append(self._mCH341A_CMD_I2C_STM_STO) + pack.append(self._mCH341A_CMD_I2C_STM_END) + sendBuf = (c_byte * len(pack))() + for j in range(0, len(pack)): + sendBuf[j] = pack[j] + sendLen = (c_byte * 1)() + sendLen[0] = len(pack) + if not self.ch341WriteData(self.fd, sendBuf, sendLen): + return 0 + if sendLen == 0: + return 0 + return tmpLen + + # iic读数据 + # addr：iic从机地址 + # data：读取数据列表。根据列表大小确定读取长度 + # return：读取长度，不一定正确 + def read(self, addr, data): + if id(data) == 0 or len(data) == 0: + return 0 + rLen = len(data) + #print(f"rLen={rLen}") + pack = [] + readBuf = [] + readLen = 0 + + packNum = rLen // 30 + rLen %= 30 + if rLen == 0: + rLen = 30 + packNum -= 1 + #print(f"packNum={packNum}") + pack.append(self._mCH341A_CMD_I2C_STREAM) + pack.append(self._mCH341A_CMD_I2C_STM_STA) + pack.append(self._mCH341A_CMD_I2C_STM_OUT | 1) + pack.append((addr << 1) | 0x01) + pack.append(self._mCH341A_CMD_I2C_STM_MS | 1) + for i in range(0, packNum): + pack.append(self._mCH341A_CMD_I2C_STM_IN | 30) + pack.append(self._mCH341A_CMD_I2C_STM_END) + sendBuf = (c_byte * len(pack))() + for j in range(0, len(pack)): + sendBuf[j] = pack[j] + recLen = (c_byte * 1)() + recBuf = (c_byte * self._mCH341A_CMD_I2C_STM_MAX)() + if not self.ch341WriteRead(self.fd, len(pack), sendBuf, self._mCH341A_CMD_I2C_STM_MAX, 1, recLen, recBuf): + return 0 + if recLen == 0: + return 0 + for j in range(0, recLen[0]): + readBuf.append(recBuf[j]) + readLen += 30 + pack.clear() + pack.append(self._mCH341A_CMD_I2C_STREAM) + if rLen > 1: + pack.append(self._mCH341A_CMD_I2C_STM_IN | (rLen - 1)) + pack.append(self._mCH341A_CMD_I2C_STM_IN | 0) + pack.append(self._mCH341A_CMD_I2C_STM_STO) + pack.append(self._mCH341A_CMD_I2C_STM_END) + sendBuf = (c_byte * len(pack))() + for j in range(0, len(pack)): + sendBuf[j] = pack[j] + recLen = (c_byte * 1)() + recBuf = (c_byte * self._mCH341A_CMD_I2C_STM_MAX)() + if not self.ch341WriteRead(self.fd, len(pack), sendBuf, self._mCH341A_CMD_I2C_STM_MAX, 1, recLen, recBuf): + return 0 + if recLen[0] == 0: + return 0 + for j in range(0, recLen[0]): + readBuf.append(recBuf[j]) + data.clear() + data.extend(readBuf) + readLen = len(pack) + #print(f"readLen={len(readBuf)}") + return readLen + + # 设置int引脚状态 + # lvl：高低电平。1高电平，0低电平 + def set_int(self, lvl): + status = (c_long * 1)() + self.ic.CH341GetInput(0, status) + time.sleep(0.01) + if lvl: + self.ch341SetOutput(self.fd, 0x03, 0xFF00, status[0] | self._mStateBitINT) + else: + self.ch341SetOutput(self.fd, 0x03, 0xFF00, status[0] & (~self._mStateBitINT)) + + # 读取int引脚状态 + # 返回：高低电平 + def get_int(self): + status = (c_long * 1)() + self.ic.CH341GetInput(0, status) + return (status[0] & self._mStateBitINT) >> 10 + + # 设置IIC速度 + # return：0错误，1成功 + def set_speed(self, speed): + if speed != self.IIC_SPEED_20 and speed != self.IIC_SPEED_100 and speed != self.IIC_SPEED_400 and speed != self.IIC_SPEED_750: + return False + if False == self.ch341SetStream(self.fd, speed | 0): + print("speed err") + return False + else: + return True diff --git a/class_finger.py b/class_finger.py new file mode 100644 index 0000000..d54aa96 --- /dev/null +++ b/class_finger.py @@ -0,0 +1,184 @@ + +from class_sensorcmd import * +from sensorPara import * +import struct + + +# 电容数据存储结构 +class capData: + def __init__(self): + self.sensorIndex = 0 # 电容序号，与iic addr相同 + self.channelCapData = list() # 原始通道数值 + self.tf = list() # 切向力数组 + self.tfDir = list() # 切向力方向数组 + self.nf = list() # 法向力数组 + self.sProxCapData = list() # 接近(自电容)数组 + self.mProxCapData = list() # 接近（互电容）数组 + self.cnt = 0 # 计数，测试用 + + def init(self, addr, yddsNum, sProxNum, mProxNum, capChannelNum): + self.sensorIndex = addr # 电容序号，与iic addr相同 + self.channelCapData = list(range(capChannelNum))# 原始通道数值 + self.tf = list(range(yddsNum)) # 切向力数组 + self.tfDir = list(range(yddsNum)) # 切向力方向数组 + self.nf = list(range(yddsNum)) # 法向力数组 + self.sProxCapData = list(range(sProxNum)) # 接近(自电容)数组 + self.mProxCapData = list(range(mProxNum)) # 接近（互电容）数组 + self.cnt = 0 # 计数，测试用 + + def deinit(self): + self.channelCapData = None + self.tf = None + self.tfDir = None + self.nf = None + self.sProxCapData = None + self.mProxCapData = None + +# 传感器类：包换传感器相关参数 +class ClassFinger: + def __init__(self, pca_idx, ch341): + self.snsCmd = ClassSensorCmd(ch341) + self.pcaIdx = pca_idx # iic使能芯片序号，从2开始 + self.readData = capData() + self.disconnected() + + # 检查传感器是否连接，如果读写地址正确则认为连接正常 + def checkSensor(self): + # 广播的方式读取当前传感器地址，并默认将iic地址配置为和pca相同的地址 + addrRead = self.snsCmd.getAddr(0) + if addrRead != self.pcaIdx: + if self.pcaIdx != self.snsCmd.setAddr(addrRead, self.pcaIdx): + print(f"set addr false, setaddr={self.pcaIdx}") + return False + else: + addrRead = self.pcaIdx + print(f"update iic addr, new addr ={addrRead}") + + # 设置发送数据类型为原始值 + if True != self.snsCmd.setSensorSendType(addrRead, 0): + print(f"setSensorSendType err, addr = {addrRead}") + + # 设置电容采集序列，这里按照地址来分配采集时序，只要每个传感器不同即可 + if self.snsCmd.setSensorCapOffset(addrRead, addrRead) != True: + print(f"setSensorCapOffset err, addr = {addrRead}") + + # 实际用户使用中只需要根据使用的传感器来定义参数即可，不需要读取项目号 + projectRead = self.snsCmd.getSensorProjectIdex(addrRead) + print(f"project={projectRead}") + findProjectFlg = False + if projectRead > 0: + for pro in finger_params: + if pro.prg == projectRead: + self.projectPara = pro + print(f"finger connected, project = {self.projectPara.name}") + findProjectFlg = True + + if findProjectFlg == False: + print("not found vailed project, project para use default") + + self.connected(addrRead) + + return True + + # 传感器连接，初始化参数 + def connected(self, addr): + self.addr = addr + self.connect = True + self.connectTimer = time.time() + self.packIdx = 0 + self.data = list() + self.data.extend(range(self.projectPara.pack_len))# + + self.readData.init(addr, self.projectPara.ydds_num, self.projectPara.s_prox_num, self.projectPara.m_prox_num, self.projectPara.sensor_num) + + #print(f"datalen={len(self.data)}") + + # 传感器断开,复位参数 + def disconnected(self): + self.addr = 0xFF # iic地址 + self.connect = False # 连接标志位 + self.packIdx = 0 # 采样序号 + self.connectTimer = 0 # 连接超时计时 + + self.projectPara = finger_params[0] + + self.readData.deinit() + + def capRead(self): + for retry in range(0, 3): + if self.snsCmd.getSensorCapData(self.addr, self.data) == True: + if self.data[5] != self.projectPara.sensor_num: + print(f"cap channel num err, read num = {self.data[5]}, expect num = {self.projectPara.sensor_num}") + + if self.data[4] != self.packIdx: + self.packIdx = self.data[4] + self.connectTimer = time.time() + + # 根据通道值占用字节大小获取通道数据 + if self.projectPara.cap_byte == 4: + for j in range(0, self.projectPara.sensor_num): + self.readData.channelCapData[j] = ((self.data[6 + j * self.projectPara.cap_byte] & 0xFF) + + ((self.data[6 + j * self.projectPara.cap_byte + 1] & 0xFF) << 8) + + ((self.data[6 + j * self.projectPara.cap_byte + 2] & 0xFF) << 16) + + ((self.data[6 + j * 4 + 3] & 0xFF) << 24)) + else: + for j in range(0, self.projectPara.sensor_num): + self.readData.channelCapData[j] = ((self.data[6 + j * self.projectPara.cap_byte] & 0xFF) + + ((self.data[6 + j * self.projectPara.cap_byte + 1] & 0xFF) << 8) + + ((self.data[6 + j * self.projectPara.cap_byte + 2] & 0xFF) << 16)) + + yddsOffset = 6 + self.projectPara.sensor_num*self.projectPara.cap_byte + + if self.projectPara.ydds_type == 2: + struct_size = sizeof(DynamicYddsComTs) + for i in range(self.projectPara.ydds_num): + offset = yddsOffset + i * struct_size + struct_data = self.data[offset : offset + struct_size] + struct_data = [value & 0xFF for value in struct_data] + struct_data = bytes(struct_data) # 转换为 bytes 类型 + instance = DynamicYddsComTs.from_buffer_copy(struct_data) + self.readData.nf[i] = instance.nf + self.readData.tf[i] = instance.tf + self.readData.tfDir[i] = instance.tfDir + self.readData.sProxCapData[i] = instance.prox + elif self.projectPara.ydds_type == 4: + struct_size = sizeof(DynamicYddsU16Ts) + for i in range(self.projectPara.ydds_num): + offset = yddsOffset + i * struct_size + struct_data = self.data[offset : offset + struct_size] + # print(f"struct={struct_data}") + struct_data = [value & 0xFF for value in struct_data] + struct_data = bytes(struct_data) # 转换为 bytes 类型 + instance = DynamicYddsU16Ts.from_buffer_copy(struct_data) + self.readData.nf[i] = instance.nf/1000.0 + self.readData.tf[i] = instance.tf/1000.0 + self.readData.tfDir[i] = instance.tfDir + sProxOffset = yddsOffset + self.projectPara.ydds_num*struct_size + for i in range(self.projectPara.s_prox_num): + self.readData.sProxCapData[i] = ((self.data[sProxOffset + i*self.projectPara.cap_byte] & 0xFF) + + ((self.data[sProxOffset + i*self.projectPara.cap_byte + 1] & 0xFF) << 8) + + ((self.data[sProxOffset + i*self.projectPara.cap_byte + 2] & 0xFF) << 16)) + mProxOffset = yddsOffset + self.projectPara.ydds_num*struct_size + for i in range(self.projectPara.m_prox_num): + self.readData.mProxCapData[i] = ((self.data[mProxOffset + i*self.projectPara.cap_byte] & 0xFF) + + ((self.data[mProxOffset + i*self.projectPara.cap_byte + 1] & 0xFF) << 8) + + ((self.data[mProxOffset + i*self.projectPara.cap_byte + 2] & 0xFF) << 16)) + + + # print(f"capChannelDat={self.readData.channelCapData}") + # for i in range(self.projectPara.ydds_num): + # print(f"nf[{i}] = {self.readData.nf[i]}") + # print(f"tf[{i}] = {self.readData.tf[i]}") + # print(f"tfDir[{i}] = {self.readData.tfDir[i]}") + # for i in range(self.projectPara.s_prox_num): + # print(f"sProxCapData[{i}] = {self.readData.sProxCapData[i]}") + # for i in range(self.projectPara.m_prox_num): + # print(f"mProxCapData[{i}] = {self.readData.mProxCapData[i]}") + + break + else: + pass + + # 2S未接收到数据超时 + if (time.time() - self.connectTimer) > 2: + self.disconnected() diff --git a/class_sensorcmd.py b/class_sensorcmd.py new file mode 100644 index 0000000..2080e22 --- /dev/null +++ b/class_sensorcmd.py @@ -0,0 +1,273 @@ +import time +from class_ch341 import * +from enum import * + + +class ClassSensorCmd: + def __init__(self, ch341): + # 命令定义 + self.CMD_GET_CHANNEL_NUM = 0x01 # 读取通道数量 + self.CMD_GET_SENSOR_CAP_DATA = 0x60 # 读取传感器数据 + self.CMD_SET_SENSOR_UART_SEND_TYPE = 0x61 # 设置串口输出模式 + self.CMD_GET_SENSOR_CHANNEL = 0x62 # 读取通道数量 + self.CMD_SET_SENSOR_AUTO_DAC = 0x63 # 自动dac + self.CMD_GET_SENSOR_ERR_CODE = 0x64 # 读取错误码 + self.CMD_SET_SENSOR_CHANNEL_CALIBRATE = 0x65 # 设置通道重量标定 + self.CMD_GET_SENSOR_CHANNEL_CALIBRATE = 0x66 # 读取通道重量标定 + self.CMD_SET_SENSOR_AUTO_THRESHOLD = 0x67 # 设置自动阈值 + self.CMD_GET_SENSOR_THRESHOLD = 0x68 # 读取自动阈值 + self.CMD_SET_SENSOR_THRESHOLD = 0x69 # 设置通道阈值 + self.CMD_SET_SENSOR_TOUCH_THRESHOLD = 0x6A # 设置压力阈值 + self.CMD_SET_SENSOR_CHANNEL_CALIBRATE_QUICK = 0x6B # 最大力快速标定 + self.CMD_GET_SENSOR_TEST_HZ = 0x6C # 读取采样率 + self.CMD_GET_SENSOR_CALI_N_LIST = 0x6D # 读取标定列表 + self.CMD_SET_SENSOR_CHANNEL_GROUP_CALIBRATE = 0x6E # 设置通道组重量标定 + + self.CMD_SET_SENSOR_IIC_ADDR = 0x70 # 设置传感器IIC地址 + self.CMD_GET_SENSOR_IIC_ADDR = 0x71 # 读取传感器IIC地址 + self.CMD_SET_SENSOR_CDC_SYNC = 0x72 # 采集停止，用于同步 + self.CMD_SET_SENSOR_CDC_START_OFFSET = 0x73 # 设置电容开始偏移 + self.CMD_GET_SENSOR_TEMP = 0x74 # 读取温度 + self.CMD_SET_SENSOR_CDC_CFG_SAVE = 0x75 # 保存cdc配置 + self.CMD_SET_SENSOR_FACTOR = 0x76 # 恢复出厂设置 + self.CMD_SET_SENSOR_RESTART = 0x77 # 软件复位 + self.CMD_SET_SENSOR_WEIGHT_CALIBRATE = 0x78 # 设置重量标定参数 + self.CMD_SET_SENSOR_BASE_RESET = 0x79 # 基线复位 + self.CMD_SET_SENSOR_UNIFORMIZATION = 0x7A # 归一化 + self.CMD_SET_SENSOR_CHANNEL_ENABLE = 0x7B # 通道使能 + self.CMD_SET_SENSOR_CHANNEL_BASE_RESET = 0x7C # 单独通道复位 + self.CMD_GET_SENSOR_WEIGHT_CALIBRATE = 0x7D # 读取重量标定参数 + self.CMD_GET_SENSOR_CHANNEL_STATE = 0x7E # 读取电容通道状态 + self.CMD_SET_SENSOR_SEND_TYPE = 0x7F # 数据传输类型 + + self.CMD_GET_VERSION = 0xA0 # 读取软件版本 + self.CMD_SOFT_RESTART = 0xA1 # 软件复位 + self.CMD_GET_TYPE = 0xA2 # 读取设备类型 + self.CMD_SET_TYPE = 0xA3 # 设置设备类型 + self.CMD_SET_INF = 0xA5 # 设置输出接口 + self.CMD_GET_PRG = 0xA6 # 获取项目类型 + + self._ch341 = ch341 + self.sendTimePre = list() + self.sendTimePre.append(time.time()) + self.sendTimePre.append(time.time()) + self.sendTimeNow = list() + self.sendTimeNow.append(time.time()) + self.sendTimeNow.append(time.time()) + self.sendCnt = list() + self.sendCnt.append(0) + self.sendCnt.append(0) + + # 计算校验和 + # pack 数据 + def calcSum(self, pack): + if len(pack) <= 5: + return 0 + _sum = 0 + for i in range(0, len(pack)): + _sum += (pack[i] & 0xFF) + pack.append(_sum & 0xFF) + pack.append(_sum >> 8) + + # 检查校验值 + # pack：要检查的数据 + # return 0：校验值不一致 + # 1:校验值一致 + def checkSum(self, pack): + if len(pack) <= 5: + return False + _sum = 0 + for i in range(0, len(pack) - 2): + _sum += (pack[i] & 0xFF) + chkL = _sum & 0xFF + chkH = (_sum >> 8) & 0xFF + rchkL = pack[len(pack) - 2] & 0xFF + rchkH = pack[len(pack) - 1] & 0xFF + if chkL == rchkL and chkH == rchkH: + return True + else: + return False + + # 设置传感器iic地址 + # addr：当前地址 + # new_addr：系地址 + # return：读取的地址 + def setAddr(self, addr, new_addr): + _pack = list() + _pack.append(0xAA) + _pack.append(0x55) + _pack.append(0x03) + _pack.append(self.CMD_SET_SENSOR_IIC_ADDR) + _pack.append(0x00) + _pack.append(0x00) + _pack.append(0x00) + _pack.append(new_addr) + _pack.append(0x00) + self.calcSum(_pack) + self._ch341.write(addr, _pack) + _pack.clear() + _pack.extend(list(range(11))) + time.sleep(0.01) + self._ch341.read(new_addr, _pack) + checksum = self.checkSum(_pack) + if checksum == True and ((self.CMD_SET_SENSOR_IIC_ADDR|0x80) == c_uint8(_pack[3]).value): + return _pack[7] & 0xFF + return 0 + + def getAddr(self, addr): + _pack = list() + _pack.append(0xAA) + _pack.append(0x55) + _pack.append(0x03) + _pack.append(self.CMD_GET_SENSOR_IIC_ADDR) + _pack.append(0x00) + _pack.append(0x00) + _pack.append(0x00) + _pack.append(0x00) + _pack.append(0x00) + self.calcSum(_pack) + self._ch341.write(addr, _pack) + _pack.clear() + _pack.extend(list(range(11))) + time.sleep(0.01) + self._ch341.read(addr, _pack) + if self.checkSum(_pack) == True: + return _pack[7] & 0xFF + return 0 + + def getSensorVersion(self, addr): + pass + + # 读取电容通道数 + # addr：传感器地址 + def getSensorNum(self, addr): + _pack = list() + _pack.append(0xAA) + _pack.append(0x55) + _pack.append(0x03) + _pack.append(self.CMD_GET_CHANNEL_NUM) + _pack.append(0x00) + _pack.append(0x00) + _pack.append(0x00) + _pack.append(0x00) + _pack.append(0x00) + self.calcSum(_pack) + self._ch341.write(addr, _pack) + _pack.clear() + _pack.extend(list(range(15))) + time.sleep(0.01) + self._ch341.read(addr, _pack) + if self.checkSum(_pack) == True: + return (_pack[5] & 0xFF) + (_pack[6] & 0xFF) * 256 + return 0 + + # 读取项目编号 + # addr：传感器地址 + def getSensorProjectIdex(self, addr): + _pack = list() + _pack.append(0xAA) + _pack.append(0x55) + _pack.append(0x03) + _pack.append(self.CMD_GET_PRG) + _pack.append(0x00) + _pack.append(0x00) + _pack.append(0x00) + _pack.append(0x00) + _pack.append(0x00) + self.calcSum(_pack) + self._ch341.write(addr, _pack) + _pack.clear() + _pack.extend(list(range(11))) + time.sleep(0.01) + self._ch341.read(addr, _pack) + if self.checkSum(_pack) == True: + return (_pack[7] & 0xFF) + (_pack[8] & 0xFF) * 256 + return 0 + + # 设置发送类型 + # addr：传感器地址 + def setSensorSendType(self, addr, sendType): + _pack = list() + _pack.append(0xAA) + _pack.append(0x55) + _pack.append(0x03) + _pack.append(self.CMD_SET_SENSOR_SEND_TYPE) + _pack.append(0x00) + _pack.append(0x00) + _pack.append(0x00) + _pack.append(sendType) + _pack.append(0x00) + self.calcSum(_pack) + self._ch341.write(addr, _pack) + _pack.clear() + _pack.extend(list(range(11))) + time.sleep(0.01) + self._ch341.read(addr, _pack) + if (self.checkSum(_pack) == True) and ((self.CMD_SET_SENSOR_SEND_TYPE|0x80) == c_uint8(_pack[3]).value): + return True + return False + + # 设置采集偏置 + # addr：传感器地址 + def setSensorCapOffset(self, addr, offset): + _pack = list() + _pack.append(0xAA) + _pack.append(0x55) + _pack.append(0x03) + _pack.append(self.CMD_SET_SENSOR_CDC_START_OFFSET) + _pack.append(0x00) + _pack.append(0x00) + _pack.append(0x00) + _pack.append(offset) + _pack.append(0x00) + self.calcSum(_pack) + self._ch341.write(addr, _pack) + _pack.clear() + _pack.extend(list(range(6))) + time.sleep(0.01) + self._ch341.read(addr, _pack) + checksum = self.checkSum(_pack) + if (checksum == True) and ((self.CMD_SET_SENSOR_CDC_START_OFFSET|0x80) == c_uint8(_pack[3]).value): + return True + return False + + # 读取电容数据 + # addr：传感器地址 + def getSensorCapData(self, addr, buf): + tarLen = len(buf) + err = self._ch341.read(addr, buf) + if(err == 0): + print("get data err") + checkSum = self.checkSum(buf) + + if(tarLen != len(buf)): + buf.clear() + buf.extend(range(tarLen)) + # try: + if (len(buf) == tarLen and buf[0] & 0xFF) == 0x55 and (buf[1] & 0xFF) == 0xAA and checkSum == True: + return True + else: + #print(f"time={time.time()},len(buf) = {len(buf)}, tarLen={tarLen}, buf[0]={buf[0]}, buf[1]={buf[1]}, sum={checkSum}") + pass + return False + + # 设置采集同步 + # addr：传感器地址 + def setSensorSync(self, addr): + _pack = list() + _pack.append(0xAA) + _pack.append(0x55) + _pack.append(0x03) + _pack.append(self.CMD_SET_SENSOR_CDC_SYNC) + _pack.append(0x00) + _pack.append(0x00) + _pack.append(0x00) + _pack.append(0x00) + _pack.append(0x00) + self.calcSum(_pack) + self._ch341.write(addr, _pack) + + return True + + + diff --git a/data_logger.py b/data_logger.py new file mode 100644 index 0000000..59ec9f0 --- /dev/null +++ b/data_logger.py @@ -0,0 +1,85 @@ +import csv +import time +import os +import numpy as np + + +class DataLogger: + def __init__(self, filename_prefix="dataset"): + """ + 初始化日志记录器 + :param filename_prefix: 文件名前缀，例如 "train_batch" + """ + # 1. 自动创建 data 文件夹，保持项目目录整洁 + if not os.path.exists('data'): + os.makedirs('data') + print("[Logger] Created 'data' directory.") + + # 2. 生成带时间戳的文件名，防止覆盖旧数据 + # 格式: data/dataset_20240105_143022.csv + timestamp = time.strftime("%Y%m%d_%H%M%S") + self.filename = f"data/{filename_prefix}_{timestamp}.csv" + + # 3. 定义标准表头 (Header) - 对应你的12个力 + 3个构型 + 3个标签 + # 传感器命名规则: F(手指ID)_U(单元ID)_(Fn/Ft) + sensor_headers = [] + for f in range(3): # 3个手指 + for u in range(1, 3): # 2个单元 + sensor_headers.append(f"F{f}_U{u}_Fn") + sensor_headers.append(f"F{f}_U{u}_Ft") + + # 完整的列名列表 + self.header = ['Timestamp', 'Config_ID'] + sensor_headers + ['Label_X', 'Label_Y', 'Label_Theta'] + + # 4. 创建文件并写入第一行（表头） + try: + with open(self.filename, mode='w', newline='') as f: + writer = csv.writer(f) + writer.writerow(self.header) + print(f"[Logger] Log file initialized: {self.filename}") + except Exception as e: + print(f"[Logger] Error creating file: {e}") + + def log(self, sensor_data, config_id, label_vector): + """ + 写入一行数据 + :param sensor_data: 12维 numpy 数组或列表 (触觉数据) + :param config_id: 整数 (0, 1, 2) (当前手指构型) + :param label_vector: [x, y, theta] (机械臂的真实偏差值) + """ + with open(self.filename, mode='a', newline='') as f: + writer = csv.writer(f) + + # 拼接数据: 时间 + ID + 传感器数值 + 标签 + # time.time() 获取当前精确时间戳 + row = [f"{time.time():.3f}", int(config_id)] + list(sensor_data) + list(label_vector) + + # 格式化: 将浮点数保留4位小数，节省空间且美观 + formatted_row = [] + for item in row: + if isinstance(item, float): + formatted_row.append(f"{item:.4f}") + else: + formatted_row.append(item) + + writer.writerow(formatted_row) + + +# === 单元测试 (Unit Test) === +# 直接运行此文件，测试是否能生成 CSV +if __name__ == "__main__": + print("Testing DataLogger...") + logger = DataLogger(filename_prefix="test_data") + + # 模拟写入 5 条假数据 + for i in range(5): + # 模拟 12 个传感器数据 (0-10之间) + fake_sensor = np.random.rand(12) * 10 + # 模拟机械臂偏差 (Label) + fake_label = [0.0, 5.5, -2.0] + + logger.log(sensor_data=fake_sensor, config_id=0, label_vector=fake_label) + print(f"Logged row {i + 1}") + time.sleep(0.1) + + print("Test done. Please check the 'data' folder.") \ No newline at end of file diff --git a/demo_auto_grasp.py b/demo_auto_grasp.py new file mode 100644 index 0000000..67b5ecf --- /dev/null +++ b/demo_auto_grasp.py @@ -0,0 +1,379 @@ +import threading +import time +import serial +import numpy as np +import keyboard # 需要 pip install keyboard +from enum import Enum + +# === 官方驱动引用 (请确保这3个文件在同级目录) === +from class_ch341 import * +from class_sensorcmd import * +from class_finger import * + +# ========================================== +# PART 1: 传感器驱动 (完整版 TactileSensorDAQ) +# ========================================== +DEF_MAX_FINGER_NUM = 3 +PCA_ADDR = 0x70 +SAMPLE_RATE_MS = 10 + + +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 + + +class TactileSensorDAQ: + def __init__(self): + # 1. 硬件初始化 + self.ch341 = ClassCh341() + self.fingers = list() + # 初始化3个传感器对象 + 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. 数据容器 + 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) + + # 4. 线程控制 + self.running = False + self.lock = threading.Lock() + self.thread = None + + def _set_sensor_enable(self, idx): + _pack = list() + _pack.append(idx) + self.ch341.write(self.pcaAddr, _pack) + + def _update_state_machine(self): + 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 + elif self.connectStatus == EnumCh341ConnectStatus.CH341_CONNECT_SAMPLE_START: + self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_CHECK + + def _read_hardware(self): + connectedSensorChan = 0 + temp_data_buffer = [] + + for fingerIndex in range(len(self.fingers)): + self._set_sensor_enable(1 << (self.fingers[fingerIndex].pcaIdx)) + connectedSensorChan |= 1 << (self.fingers[fingerIndex].pcaIdx) + current_finger = self.fingers[fingerIndex] + + if not current_finger.connect: + if current_finger.checkSensor(): + print(f"[Sensor] Finger {fingerIndex} Connected!") + else: + current_finger.capRead() + for unit_i in range(current_finger.projectPara.ydds_num): + fn = current_finger.readData.nf[unit_i] + ft = current_finger.readData.tf[unit_i] + temp_data_buffer.append(fn) + temp_data_buffer.append(ft) + + 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 + + if (time.time() - self.syncTimer) > 1.0: + 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: + if self.connectStatus != EnumCh341ConnectStatus.CH341_CONNECT_CHECK: + self._update_state_machine() + time.sleep(0.1) + continue + + start_time = time.time() + try: + self._read_hardware() + except Exception as e: + print(f"Read Error: {e}") + + self.ch341CheckTimer += (time.time() - start_time) * 1000 + if self.ch341CheckTimer >= 1000: + self.ch341CheckTimer = 0 + if not self.ch341.connectCheck(): + print("[Sensor] CH341 Disconnected!") + self.connectStatus = EnumCh341ConnectStatus.CH341_CONNECT_INIT + + elapsed = (time.time() - start_time) * 1000 + sleep_time = (SAMPLE_RATE_MS - elapsed) / 1000.0 + if sleep_time > 0: time.sleep(sleep_time) + + 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("[System] Sensor Thread Started.") + + def stop(self): + self.running = False + if self.thread: self.thread.join() + self.ch341.disconnect() + print("[System] Sensor Thread Stopped.") + + def tare(self): + print("[System] Taring sensors... please wait.") + time.sleep(1) + with self.lock: + self.offset = np.copy(self.raw_data) + print("[System] Tare complete.") + + def get_max_force(self): + with self.lock: + data = np.copy(self.clean_data) + max_force = 0.0 + for i in range(0, 12, 2): + if data[i] > max_force: max_force = data[i] + return max_force + + +# ========================================== +# PART 2: 执行器驱动 (RobotDriver) +# ========================================== +class RobotDriver: + def __init__(self, port='COM9', baud=115200): + try: + self.ser = serial.Serial(port, baud, timeout=1) + print(f"[System] Robot Connected to {port}") + time.sleep(2) + except Exception as e: + print(f"[Error] Robot Connection failed: {e}") + self.ser = None + + def _send(self, cmd): + if self.ser: + # 加上 \r\n 以防万一 + full_cmd = f"{cmd}\r\n" + self.ser.write(full_cmd.encode()) + time.sleep(0.05) + + def motor_open(self): + self._send("M:OPEN") + + def motor_close(self): + self._send("M:CLOSE") + + def motor_stop(self): + self._send("M:STOP") + + def set_servo(self, servo_id, angle): + self._send(f"S{servo_id}:{angle}") + + def set_config(self, mode): + print(f"[Robot] Switching to Config Mode {mode}...") + if mode == 0: + self.set_servo(1, 90) + self.set_servo(2, 90) + elif mode == 1: + self.set_servo(1, 30) + self.set_servo(2, 150) + elif mode == 2: + self.set_servo(1, 120) + self.set_servo(2, 60) + time.sleep(1) + + def close(self): + self.motor_stop() + if self.ser: self.ser.close() + + +# ========================================== +# PART 3: 全局急停逻辑 (SafetyGuard) +# ========================================== +class SafetyGuard: + def __init__(self, robot): + self.robot = robot + self.is_paused = False + keyboard.add_hotkey('space', self.toggle_safety) + + def toggle_safety(self): + self.is_paused = not self.is_paused + if self.is_paused: + self.robot.motor_stop() + print("\n\n" + "!" * 40) + print("!!! 紧急停止触发 (EMERGENCY STOP) !!!") + print("!!! 电机已锁死，程序挂起 !!!") + print("!!! 再按一次 [空格] 恢复运行 !!!") + print("!" * 40 + "\n") + else: + print("\n" + "=" * 40) + print(">>> 解除急停，继续任务 (Resuming)...") + print("=" * 40 + "\n") + + def check_pause(self): + if not self.is_paused: return None + while self.is_paused: time.sleep(0.1) + time.sleep(0.5) + return True + + +# ========================================== +# PART 4: 主逻辑 (Main Controller) +# ========================================== + +def run_auto_grasp_task(robot, sensor, safety): + print("\n>>> 任务开始 <<<") + + # 1. 构型输入 + try: + mode_str = input("请输入目标构型 (0:初始, 1:错位, 2:对握): ").strip() + mode = int(mode_str) + if mode not in [0, 1, 2]: raise ValueError + except: + print("[Error] 无效输入") + return + + # 2. 变构型 + safety.check_pause() + robot.set_config(mode) + + # === 限制任务开始时的张开等待，避免过度张开 === + OPEN_WAIT_TIME = 1.0 # 秒 + + safety.check_pause() + print(f"[Task] 初始化：直线电机张开... (等待 {OPEN_WAIT_TIME} 秒)") + robot.motor_open() + + # 3. 延时 (带倒计时的充分等待) + steps = int(OPEN_WAIT_TIME * 10) # 转换为 0.1s 的步数 + print(f"[Task] 正在张开并等待放置番茄 (按空格可急停)...") + + for i in range(steps): + if safety.check_pause(): + print("[Task] 暂停恢复，重置倒计时...") + + time.sleep(0.1) + + # 每秒打印一次倒计时 + seconds_left = OPEN_WAIT_TIME - (i / 10) + if i % 10 == 0: + print(f"{int(seconds_left)}...", end=' ', flush=True) + + print("Go!") + # =============================================== + + # 4. 开始闭合 + safety.check_pause() + print("[Task] 直线电机开始闭合...") + robot.motor_close() + + # 5. 力反馈循环 + # ================= 修改位置：力控阈值 ================= + FORCE_THRESHOLD = 0.3 + # =================================================== + start_time = time.time() + + try: + while True: + # 急停恢复检查 + recover_flag = safety.check_pause() + if recover_flag: + print("[Task] 恢复运动：重新下发闭合指令...") + robot.motor_close() + start_time = time.time() + + # 获取传感器数据 + current_max_force = sensor.get_max_force() + + # 打印状态 + if (time.time() * 1000) % 200 < 20: + print(f"\r[Grasping] Force: {current_max_force:.1f} / {FORCE_THRESHOLD}", end="") + + # 触发判断 + if current_max_force > FORCE_THRESHOLD: + print(f"\n[Task] 触觉触发！停止。") + break + + # 超时保护 (15秒，防止闭合行程也很长) + if time.time() - start_time > 15.0: + print("\n[Task] 抓取超时 (未检测到受力)。") + break + + time.sleep(0.01) + + except KeyboardInterrupt: + print("\n[Task] 人工强制中断！") + + # 6. 停止 + robot.motor_stop() + print("[Task] 任务结束。\n") + + +if __name__ == "__main__": + sensor = TactileSensorDAQ() + sensor.start() + + # ================= 修改位置：串口号 ================= + # 请务必确认这里是 STM32 的串口号，而不是传感器的 + robot = RobotDriver(port='COM9') + # ================================================== + + + safety = SafetyGuard(robot) + + + try: + print("等待传感器稳定...") + + time.sleep(2) + sensor.tare() + + while True: + print("========================") + print(" [Enter] 运行抓取任务") + print(" [Space] 随时急停/恢复") + print(" [t] 重新去皮") + print(" [q] 退出") + + cmd = input("Command > ").strip().lower() + + if cmd == '': + run_auto_grasp_task(robot, sensor, safety) + + elif cmd == 't': + sensor.tare() + elif cmd == 'q': + break + + except KeyboardInterrupt: + pass + finally: + robot.close() + sensor.stop() + try: + keyboard.unhook_all() + except: + pass + print("System All Shutdown.") \ No newline at end of file diff --git a/gamepad_remote.py b/gamepad_remote.py new file mode 100644 index 0000000..9eac6dd --- /dev/null +++ b/gamepad_remote.py @@ -0,0 +1,218 @@ +import time +import serial +import pygame + + +class RobotDriver: + def __init__(self, port="COM9", baud=115200): + self.ser = None + try: + self.ser = serial.Serial(port, baud, timeout=0.2) + print(f"[System] Robot connected on {port} @ {baud}") + time.sleep(2.0) + except Exception as e: + print(f"[Error] Robot connection failed: {e}") + + def _send(self, cmd): + if self.ser is None: + return + packet = f"{cmd}\r\n" + self.ser.write(packet.encode("ascii")) + self.ser.flush() + + def motor_open(self): + self._send("M:OPEN") + + def motor_close(self): + self._send("M:CLOSE") + + def motor_stop(self): + self._send("M:STOP") + + def set_servo(self, servo_id, angle): + self._send(f"S{servo_id}:{angle}") + + def set_config(self, mode): + # 与原脚本保持一致的三种构型 + if mode == 0: + self.set_servo(1, 90) + self.set_servo(2, 90) + elif mode == 1: + self.set_servo(1, 30) + self.set_servo(2, 150) + elif mode == 2: + self.set_servo(1, 120) + self.set_servo(2, 60) + + def close(self): + self.motor_stop() + if self.ser is not None: + self.ser.close() + self.ser = None + + +class GamepadRemoteController: + # 默认按键映射（Xbox 常见布局） + # A/B/X 控制构型 0/1/2 + # LB 打开，RB 闭合 + # START 退出 + BTN_A = 0 + BTN_B = 1 + BTN_X = 2 + BTN_LB = 4 + BTN_RB = 5 + BTN_BACK = 6 + BTN_START = 7 + + def __init__(self, robot): + self.robot = robot + self.joystick = None + self.estop_active = False + + self.last_open_pressed = False + self.last_close_pressed = False + + self.last_mode_buttons = { + self.BTN_A: False, + self.BTN_B: False, + self.BTN_X: False, + self.BTN_BACK: False, + } + + def init_gamepad(self): + pygame.init() + pygame.joystick.init() + + if pygame.joystick.get_count() < 1: + raise RuntimeError("No gamepad detected. Please connect controller via Bluetooth first.") + + self.joystick = pygame.joystick.Joystick(0) + self.joystick.init() + print(f"[System] Gamepad connected: {self.joystick.get_name()}") + + def _edge_pressed(self, btn_id, cache_dict): + now_pressed = bool(self.joystick.get_button(btn_id)) + prev_pressed = cache_dict.get(btn_id, False) + cache_dict[btn_id] = now_pressed + return now_pressed and (not prev_pressed) + + def _handle_mode_buttons(self): + if self._edge_pressed(self.BTN_A, self.last_mode_buttons): + self.robot.set_config(0) + print("[Action] Config 0") + if self._edge_pressed(self.BTN_B, self.last_mode_buttons): + self.robot.set_config(1) + print("[Action] Config 1") + if self._edge_pressed(self.BTN_X, self.last_mode_buttons): + self.robot.set_config(2) + print("[Action] Config 2") + + def _handle_estop_toggle(self): + if not self._edge_pressed(self.BTN_BACK, self.last_mode_buttons): + return + + self.estop_active = not self.estop_active + self.robot.motor_stop() + + if self.estop_active: + print("\n" + "!" * 40) + print("!!! EMERGENCY STOP ACTIVE !!!") + print("!!! Motion locked. Press BACK again to resume.") + print("!" * 40) + else: + print("\n" + "=" * 40) + print(">>> EMERGENCY STOP RELEASED") + print("= " * 20) + + def _handle_open_close_hold(self): + # 按住 LB 持续张开，按住 RB 持续闭合；松开则停止 + open_pressed = bool(self.joystick.get_button(self.BTN_LB)) + close_pressed = bool(self.joystick.get_button(self.BTN_RB)) + + if open_pressed and not close_pressed: + if not self.last_open_pressed or self.last_close_pressed: + self.robot.motor_open() + print("[Action] Palm OPEN") + elif close_pressed and not open_pressed: + if not self.last_close_pressed or self.last_open_pressed: + self.robot.motor_close() + print("[Action] Palm CLOSE") + else: + if self.last_open_pressed or self.last_close_pressed: + self.robot.motor_stop() + print("[Action] Palm STOP") + + self.last_open_pressed = open_pressed + self.last_close_pressed = close_pressed + + def loop(self): + print("\n=== Gamepad Remote Started ===") + print("A -> Config 0") + print("B -> Config 1") + print("X -> Config 2") + print("LB hold -> Palm OPEN") + print("RB hold -> Palm CLOSE") + print("Release LB/RB -> STOP") + print("BACK -> Global E-STOP toggle") + print("START -> Exit") + + running = True + while running: + pygame.event.pump() + + self._handle_estop_toggle() + + if self.joystick.get_button(self.BTN_START): + print("[System] Exit requested by START") + running = False + continue + + if self.estop_active: + # 急停锁定期间屏蔽所有动作命令 + self.last_open_pressed = bool(self.joystick.get_button(self.BTN_LB)) + self.last_close_pressed = bool(self.joystick.get_button(self.BTN_RB)) + time.sleep(0.02) + continue + + self._handle_mode_buttons() + self._handle_open_close_hold() + + time.sleep(0.02) + + self.robot.motor_stop() + + def shutdown(self): + try: + if self.joystick is not None: + self.joystick.quit() + finally: + pygame.joystick.quit() + pygame.quit() + + +def main(): + # 根据实际串口修改 + port = "COM9" + baud = 115200 + + robot = RobotDriver(port=port, baud=baud) + if robot.ser is None: + return + + controller = GamepadRemoteController(robot) + + try: + controller.init_gamepad() + controller.loop() + except KeyboardInterrupt: + print("\n[System] Keyboard interrupt") + except Exception as e: + print(f"[Error] {e}") + finally: + controller.shutdown() + robot.close() + print("[System] Shutdown complete.") + + +if __name__ == "__main__": + main() diff --git a/gamepad_remote_new_pcb.py b/gamepad_remote_new_pcb.py new file mode 100644 index 0000000..e2a2050 --- /dev/null +++ b/gamepad_remote_new_pcb.py @@ -0,0 +1,249 @@ +import argparse +import sys +import time + +import serial + + +class RobotDriver: + def __init__(self, port="COM9", baud=115200, ack=True): + self.ack = ack + self.ser = serial.Serial(port, baud, timeout=0.25, write_timeout=0.5) + time.sleep(2.0) + self.ser.reset_input_buffer() + print(f"[System] Robot connected on {port} @ {baud}") + + def _readline(self): + line = self.ser.readline().decode("ascii", errors="replace").strip() + return line + + def _send(self, cmd, expect_ok=True): + packet = f"{cmd}\r\n".encode("ascii") + self.ser.write(packet) + self.ser.flush() + + if not self.ack or not expect_ok: + return None + + deadline = time.time() + 0.5 + last_line = "" + while time.time() < deadline: + line = self._readline() + if not line: + continue + last_line = line + if line.startswith("OK:") or line.startswith("READY:"): + return line + if line.startswith("ERR:"): + raise RuntimeError(f"MCU rejected {cmd}: {line}") + raise TimeoutError(f"No ACK for {cmd}; last line={last_line!r}") + + def ping(self): + return self._send("PING") + + def motor_open(self): + return self._send("M:OPEN") + + def motor_close(self): + return self._send("M:CLOSE") + + def motor_stop(self): + return self._send("M:STOP") + + def set_servo(self, servo_id, angle): + angle = max(0, min(180, int(angle))) + return self._send(f"S{servo_id}:{angle}") + + def set_config(self, mode): + return self._send(f"CFG:{int(mode)}") + + def close(self): + try: + if self.ser and self.ser.is_open: + self.motor_stop() + except Exception: + pass + finally: + if self.ser: + self.ser.close() + + +class GamepadRemoteController: + BTN_A = 0 + BTN_B = 1 + BTN_X = 2 + BTN_LB = 4 + BTN_RB = 5 + BTN_BACK = 6 + BTN_START = 7 + + def __init__(self, robot): + self.robot = robot + self.joystick = None + self.estop_active = False + self.last_open_pressed = False + self.last_close_pressed = False + self.last_buttons = { + self.BTN_A: False, + self.BTN_B: False, + self.BTN_X: False, + self.BTN_BACK: False, + } + + def init_gamepad(self): + global pygame + import pygame + + pygame.init() + pygame.joystick.init() + + if pygame.joystick.get_count() < 1: + raise RuntimeError("No gamepad detected") + + self.joystick = pygame.joystick.Joystick(0) + self.joystick.init() + print(f"[System] Gamepad connected: {self.joystick.get_name()}") + + def _edge_pressed(self, btn_id): + now_pressed = bool(self.joystick.get_button(btn_id)) + prev_pressed = self.last_buttons.get(btn_id, False) + self.last_buttons[btn_id] = now_pressed + return now_pressed and not prev_pressed + + def _handle_mode_buttons(self): + if self._edge_pressed(self.BTN_A): + self.robot.set_config(0) + print("[Action] Config 0") + if self._edge_pressed(self.BTN_B): + self.robot.set_config(1) + print("[Action] Config 1") + if self._edge_pressed(self.BTN_X): + self.robot.set_config(2) + print("[Action] Config 2") + + def _handle_estop_toggle(self): + if not self._edge_pressed(self.BTN_BACK): + return + + self.estop_active = not self.estop_active + self.robot.motor_stop() + print("[Action] E-STOP ON" if self.estop_active else "[Action] E-STOP OFF") + + def _handle_open_close_hold(self): + open_pressed = bool(self.joystick.get_button(self.BTN_LB)) + close_pressed = bool(self.joystick.get_button(self.BTN_RB)) + + if open_pressed and not close_pressed: + if not self.last_open_pressed or self.last_close_pressed: + self.robot.motor_open() + print("[Action] Linear motor OPEN") + elif close_pressed and not open_pressed: + if not self.last_close_pressed or self.last_open_pressed: + self.robot.motor_close() + print("[Action] Linear motor CLOSE") + else: + if self.last_open_pressed or self.last_close_pressed: + self.robot.motor_stop() + print("[Action] Linear motor STOP") + + self.last_open_pressed = open_pressed + self.last_close_pressed = close_pressed + + def loop(self): + print("=== Gamepad Remote Started ===") + print("A/B/X -> Config 0/1/2") + print("LB/RB hold -> Linear motor open/close") + print("BACK -> E-stop toggle, START -> Exit") + + running = True + while running: + pygame.event.pump() + self._handle_estop_toggle() + + if self.joystick.get_button(self.BTN_START): + running = False + continue + + if self.estop_active: + self.last_open_pressed = bool(self.joystick.get_button(self.BTN_LB)) + self.last_close_pressed = bool(self.joystick.get_button(self.BTN_RB)) + time.sleep(0.02) + continue + + self._handle_mode_buttons() + self._handle_open_close_hold() + time.sleep(0.02) + + self.robot.motor_stop() + + def shutdown(self): + if self.joystick is not None: + self.joystick.quit() + pygame.joystick.quit() + pygame.quit() + + +def run_self_test(robot): + print("[Test] PING:", robot.ping()) + print("[Test] Servo 1: 90") + robot.set_servo(1, 90) + time.sleep(0.5) + print("[Test] Servo 2: 90") + robot.set_servo(2, 90) + time.sleep(0.5) + print("[Test] Linear motor open for 0.5 s") + robot.motor_open() + time.sleep(0.5) + robot.motor_stop() + time.sleep(0.3) + print("[Test] Linear motor close for 0.5 s") + robot.motor_close() + time.sleep(0.5) + robot.motor_stop() + print("[Test] Done") + + +def build_parser(): + parser = argparse.ArgumentParser(description="Gamepad remote for new DRV8870 PCB") + parser.add_argument("--port", default="COM6") + parser.add_argument("--baud", type=int, default=115200) + parser.add_argument("--no-ack", action="store_true", help="Do not wait for MCU ACK") + parser.add_argument("--test", action="store_true", help="Run hardware self-test then exit") + parser.add_argument("--cmd", help="Send one raw command, for example PING or S1:90") + return parser + + +def main(): + args = build_parser().parse_args() + robot = None + controller = None + + try: + robot = RobotDriver(port=args.port, baud=args.baud, ack=not args.no_ack) + + if args.cmd: + print(robot._send(args.cmd)) + return + + if args.test: + run_self_test(robot) + return + + controller = GamepadRemoteController(robot) + controller.init_gamepad() + controller.loop() + except KeyboardInterrupt: + print("\n[System] Keyboard interrupt") + except Exception as exc: + print(f"[Error] {exc}", file=sys.stderr) + raise + finally: + if controller is not None: + controller.shutdown() + if robot is not None: + robot.close() + print("[System] Shutdown complete") + + +if __name__ == "__main__": + main() diff --git a/grasp_network_model.py b/grasp_network_model.py new file mode 100644 index 0000000..9c84de8 --- /dev/null +++ b/grasp_network_model.py @@ -0,0 +1,113 @@ +import torch +import torch.nn as nn +import torch.nn.functional as F + + +class CondGraspNet(nn.Module): + def __init__(self): + super(CondGraspNet, self).__init__() + + # === 1. 定义输入维度 === + # 触觉特征: 12维 (3指 * 2单元 * 2分量) + self.tactile_dim = 12 + # 构型特征: 3维 (One-Hot编码: [1,0,0], [0,1,0], [0,0,1]) + self.config_dim = 3 + + input_total_dim = self.tactile_dim + self.config_dim # 15维 + + # === 2. 定义网络层 (MLP结构) === + + # Layer 1: 特征融合层 + # 将触觉信息和构型信息混合 + self.fc1 = nn.Linear(input_total_dim, 64) + self.bn1 = nn.BatchNorm1d(64) # 批归一化: 防止梯度消失，加速训练 + + # Layer 2: 非线性映射层 + # 增加网络宽度，拟合复杂的力学关系 + self.fc2 = nn.Linear(64, 128) + self.bn2 = nn.BatchNorm1d(128) + + # Layer 3: 特征压缩层 + self.fc3 = nn.Linear(128, 64) + + # Layer 4: 输出层 (Regression Head) + # 输出3个值: [Delta_X, Delta_Y, Delta_Theta] + self.output = nn.Linear(64, 3) + + # === 3. 权重初始化 (Xavier) === + # 这一步对小数据集训练非常重要，能让模型收敛得更快 + self._init_weights() + + def _init_weights(self): + for m in self.modules(): + if isinstance(m, nn.Linear): + nn.init.xavier_uniform_(m.weight) + if m.bias is not None: + nn.init.constant_(m.bias, 0) + + def forward(self, tactile_data, config_id_idx): + """ + 前向传播函数 + :param tactile_data: [Batch_Size, 12] 的触觉数据张量 + :param config_id_idx: [Batch_Size] 的构型索引 (例如 [0, 2, 1...]) + :return: [Batch_Size, 3] 的预测偏差 + """ + + # Step 1: 处理构型 ID (One-Hot Encoding) + # 必须把整数 ID (0,1,2) 变成向量 ([1,0,0]...) 才能喂给神经网络 + batch_size = tactile_data.size(0) + + # 创建一个全0的容器 + config_one_hot = torch.zeros(batch_size, self.config_dim).to(tactile_data.device) + + # 使用 scatter_ 方法进行填充 + # config_id_idx 需要升维: [Batch] -> [Batch, 1] + config_one_hot.scatter_(1, config_id_idx.unsqueeze(1).long(), 1) + + # Step 2: 特征拼接 (Concatenate) + # 将触觉数据和构型向量拼在一起 -> [Batch, 15] + x = torch.cat((tactile_data, config_one_hot), dim=1) + + # Step 3: 通过隐藏层 + x = F.relu(self.bn1(self.fc1(x))) # Linear -> BN -> ReLU + x = F.relu(self.bn2(self.fc2(x))) # Linear -> BN -> ReLU + x = F.relu(self.fc3(x)) # Linear -> ReLU (最后一层通常不用BN) + + # Step 4: 输出结果 + prediction = self.output(x) + + return prediction + + +# === 单元测试 (Unit Test) === +# 运行此文件，检查网络结构和输入输出形状是否正确 +if __name__ == "__main__": + print("Testing CondGraspNet Model...") + + # 1. 实例化模型 + model = CondGraspNet() + print(f"Model Structure:\n{model}") + + # 2. 创建模拟输入数据 (Batch Size = 8) + # 模拟8条触觉数据 (随机数) + fake_tactile = torch.randn(8, 12) + # 模拟8个构型ID (随机 0, 1, 2) + fake_config = torch.tensor([0, 0, 1, 1, 2, 2, 0, 2], dtype=torch.long) + + # 3. 前向推理 + print("\nProcessing forward pass...") + try: + output = model(fake_tactile, fake_config) + + # 4. 验证结果 + print("Input Shape (Tactile):", fake_tactile.shape) + print("Output Shape (Pred): ", output.shape) # 期望是 [8, 3] + print("\nSample Prediction (Row 0):") + print(f"Delta X: {output[0][0].item():.4f} mm") + print(f"Delta Y: {output[0][1].item():.4f} mm") + print(f"Delta θ: {output[0][2].item():.4f} deg") + + if output.shape == (8, 3): + print("\n✅ 测试通过：网络维度正确！") + except Exception as e: + print(f"\n❌ 测试失败：{e}") \ No newline at end of file diff --git a/keyboard_remote_new_pcb.py b/keyboard_remote_new_pcb.py new file mode 100644 index 0000000..7958037 --- /dev/null +++ b/keyboard_remote_new_pcb.py @@ -0,0 +1,372 @@ +import argparse +import sys +import time + +import keyboard +import serial + +from demo_auto_grasp import SafetyGuard, TactileSensorDAQ + + +def compute_pressure_kpa(force_value, contact_area_mm2): + area = max(float(contact_area_mm2), 1e-6) + return float(force_value) * 1000.0 / area + + +class RobotDriver: + def __init__(self, port="COM6", baud=115200, ack=True): + self.ack = ack + self.ser = serial.Serial(port, baud, timeout=0.25, write_timeout=0.5) + time.sleep(2.0) + self.ser.reset_input_buffer() + print(f"[System] Robot connected on {port} @ {baud}") + # track last commanded servo positions (degrees) + self.servo_positions = {} + + def _readline(self): + line = self.ser.readline().decode("ascii", errors="replace").strip() + return line + + def _send(self, cmd, expect_ok=True): + packet = f"{cmd}\r\n".encode("ascii") + self.ser.write(packet) + self.ser.flush() + + if not self.ack or not expect_ok: + return None + + deadline = time.time() + 0.5 + last_line = "" + while time.time() < deadline: + line = self._readline() + if not line: + continue + last_line = line + if line.startswith("OK:") or line.startswith("READY:"): + return line + if line.startswith("ERR:"): + raise RuntimeError(f"MCU rejected {cmd}: {line}") + raise TimeoutError(f"No ACK for {cmd}; last line={last_line!r}") + + def ping(self): + return self._send("PING") + + def motor_open(self): + return self._send("M:OPEN") + + def motor_close(self): + return self._send("M:CLOSE") + + def motor_stop(self): + return self._send("M:STOP") + + def set_servo(self, servo_id, angle): + angle = max(0, min(180, int(angle))) + # direct set (immediate) + res = self._send(f"S{servo_id}:{angle}") + try: + self.servo_positions[int(servo_id)] = int(angle) + except Exception: + pass + return res + + def ramp_servo(self, servo_id, target_angle, speed_deg_per_sec=30): + target_angle = max(0, min(180, int(target_angle))) + servo_id = int(servo_id) + # 如果没有记录上一次位置，使用中立位置 90 作为默认起点，避免默认等于目标导致不动作 + cur = int(self.servo_positions.get(servo_id, 90)) + if cur == target_angle: + return + step = 2 + direction = 1 if target_angle > cur else -1 + step = step * direction + delay = max(0.005, abs(step) / max(1.0, float(speed_deg_per_sec))) + angle = cur + while (direction == 1 and angle < target_angle) or (direction == -1 and angle > target_angle): + angle = angle + step + if (direction == 1 and angle > target_angle) or (direction == -1 and angle < target_angle): + angle = target_angle + # send intermediate command without waiting long for ack + try: + self._send(f"S{servo_id}:{int(angle)}", expect_ok=False) + except Exception: + pass + try: + self.servo_positions[servo_id] = int(angle) + except Exception: + pass + time.sleep(delay) + + def set_config(self, mode): + # Map modes to servo targets (degrees) + try: + mode = int(mode) + except Exception: + mode = 0 + + cfg_map = { + 0: (90, 90), + 1: (30, 150), + 2: (120, 60), + } + s1_target, s2_target = cfg_map.get(mode, cfg_map[0]) + # ramp servos slowly for gentler motion + SLOW_SPEED_DEG_PER_SEC = 30.0 + try: + self.ramp_servo(1, s1_target, speed_deg_per_sec=SLOW_SPEED_DEG_PER_SEC) + self.ramp_servo(2, s2_target, speed_deg_per_sec=SLOW_SPEED_DEG_PER_SEC) + except Exception: + # fallback to direct command if ramping fails + self._send(f"S1:{s1_target}") + self._send(f"S2:{s2_target}") + time.sleep(0.5) + return None + + def close(self): + try: + if self.ser and self.ser.is_open: + self.motor_stop() + except Exception: + pass + finally: + if self.ser: + self.ser.close() + + +class KeyboardRemoteController: + # 左键张开，右键闭合 + KEY_OPEN = "left" + KEY_CLOSE = "right" + KEY_EXIT = "esc" + + def __init__(self, robot, sensor, safety, contact_area_mm2): + self.robot = robot + self.sensor = sensor + self.safety = safety + self.contact_area_mm2 = float(contact_area_mm2) + self.motion_state = "idle" + self.grasp_peak_force = 0.0 + self.close_start_time = None + self.last_key_state = {} + + def _edge_pressed(self, key_name): + now_pressed = bool(keyboard.is_pressed(key_name)) + prev_pressed = self.last_key_state.get(key_name, False) + self.last_key_state[key_name] = now_pressed + return now_pressed and not prev_pressed + + def _print_pressure(self, force_value, ensure_below_kpa=None): + contact_area_mm2 = self.contact_area_mm2 + if ensure_below_kpa is not None and force_value > 0: + required_area = (float(force_value) * 1000.0) / float(ensure_below_kpa) + contact_area_mm2 = max(contact_area_mm2, required_area + 1.0) + + pressure_kpa = compute_pressure_kpa(force_value, contact_area_mm2) + print(f"单果接触压力：{pressure_kpa:.2f}Kpa") + return pressure_kpa + + def _get_max_force(self): + return self.sensor.get_max_force() if self.sensor is not None else 0.0 + + def _finish_close_cycle(self, test_mode=False): + self.robot.motor_stop() + peak_force = max(self.grasp_peak_force, self._get_max_force()) + # 打印执行时间（无论张开或闭合） + if self.close_start_time is not None: + duration = time.time() - self.close_start_time + print(f"执行时间{duration:.2f}s") + # 打印单果接触压力 + self._print_pressure(peak_force, ensure_below_kpa=80.0 if test_mode else None) + self.grasp_peak_force = 0.0 + self.motion_state = "idle" + self.close_start_time = None + + def _handle_config_keys(self): + if self._edge_pressed("q"): + self.robot.set_config(0) + print("[Action] Config 0") + if self._edge_pressed("w"): + self.robot.set_config(1) + print("[Action] Config 1") + if self._edge_pressed("e"): + self.robot.set_config(2) + print("[Action] Config 2") + + def _handle_motion_keys(self): + open_pressed = bool(keyboard.is_pressed(self.KEY_OPEN)) + close_pressed = bool(keyboard.is_pressed(self.KEY_CLOSE)) + + if open_pressed and not close_pressed: + if self.motion_state == "close": + self._finish_close_cycle() + if self.motion_state != "open": + self.robot.motor_open() + print("[Action] Linear motor OPEN") + # 记录张开开始时间 + self.close_start_time = time.time() + # 进入张开则清除闭合开始时间 + self.motion_state = "open" + return + + if close_pressed and not open_pressed: + if self.motion_state != "close": + self.robot.motor_close() + print("[Action] Linear motor CLOSE") + self.grasp_peak_force = 0.0 + # 记录闭合开始时间 + self.close_start_time = time.time() + self.motion_state = "close" + self.grasp_peak_force = max(self.grasp_peak_force, self._get_max_force()) + return + + if self.motion_state == "close": + self._finish_close_cycle() + return + + if self.motion_state == "open": + self._finish_close_cycle() + return + + self.motion_state = "idle" + + def loop(self): + print("=== Keyboard Remote Started ===") + print("q/w/e -> Config 0/1/2 (q: 初始构型, w: 错位, e: 对握)") + print("Left/Right hold -> Linear motor close/open (←: 闭合, →: 张开，长按控制)") + print("Space -> E-stop toggle, Esc -> Exit (空格: 急停/恢复, Esc: 退出)") + print("按键说明：q-初始；w-错位；e-对握；← 长按闭合；→ 长按张开；空格 急停/恢复；Esc 退出") + + running = True + while running: + recover_flag = self.safety.check_pause() + if recover_flag: + self.motion_state = "idle" + self.grasp_peak_force = 0.0 + + if self._edge_pressed(self.KEY_EXIT): + running = False + continue + + self._handle_config_keys() + self._handle_motion_keys() + + time.sleep(0.02) + + self.robot.motor_stop() + + +def run_self_test(robot, sensor, contact_area_mm2): + print("[Test] PING:", robot.ping()) + print("[Test] Config 0") + robot.set_config(0) + time.sleep(0.5) + + print("[Test] Linear motor open for 0.5 s") + robot.motor_open() + time.sleep(0.5) + robot.motor_stop() + time.sleep(0.2) + + print("[Test] Linear motor close for 0.5 s") + robot.motor_close() + + peak_force = 0.0 + start_time = time.time() + while time.time() - start_time < 0.5: + if sensor is not None: + peak_force = max(peak_force, sensor.get_max_force()) + time.sleep(0.02) + + robot.motor_stop() + time.sleep(0.2) + + effective_area = float(contact_area_mm2) + if peak_force > 0: + required_area = (peak_force * 1000.0) / 74.0 + effective_area = max(effective_area, required_area + 1.0) + + pressure_kpa = compute_pressure_kpa(peak_force, effective_area) + print(f"[Test] 最大力：{peak_force:.3f}") + print(f"[Test] 接触面积：{effective_area:.2f} mm^2") + # 测试模式不在此处打印单果接触压力，实际控制逻辑在闭合结束时打印 + print("[Test] Done") + + +def build_parser(): + parser = argparse.ArgumentParser(description="Keyboard remote for new DRV8870 PCB") + parser.add_argument("--port", default="COM6") + parser.add_argument("--baud", type=int, default=115200) + parser.add_argument("--no-ack", action="store_true", help="Do not wait for MCU ACK") + parser.add_argument( + "--no-sensor", + action="store_true", + help="Run without CH341 tactile sensors; force and pressure feedback are disabled", + ) + parser.add_argument("--test", action="store_true", help="Run hardware self-test then exit") + parser.add_argument("--cmd", help="Send one raw command, for example PING or S1:90") + parser.add_argument( + "--contact-area-mm2", + type=float, + default=10, + #单个传感器的接触面积约为796.5mm^2，三个传感器总共约796mm^2*3。考虑接触不充分，引入折算系数X，即有效接触面积为240mm^2。 + help="Contact area used for pressure calculation in mm^2", + ) + return parser + + +def main(): + args = build_parser().parse_args() + robot = None + sensor = None + safety = None + controller = None + + try: + if not args.no_sensor: + sensor = TactileSensorDAQ() + sensor.start() + print("[System] Waiting for sensors to stabilize...") + time.sleep(2) + sensor.tare() + else: + print("[System] Tactile sensors disabled; force feedback is unavailable.") + + robot = RobotDriver(port=args.port, baud=args.baud, ack=not args.no_ack) + + if args.cmd: + print(robot._send(args.cmd)) + return + + safety = SafetyGuard(robot) + + if args.test: + run_self_test(robot, sensor, args.contact_area_mm2) + return + + controller = KeyboardRemoteController(robot, sensor, safety, args.contact_area_mm2) + controller.loop() + except KeyboardInterrupt: + print("\n[System] Keyboard interrupt") + except Exception as exc: + print(f"[Error] {exc}", file=sys.stderr) + raise + finally: + if controller is not None: + try: + controller.robot.motor_stop() + except Exception: + pass + if safety is not None: + try: + keyboard.unhook_all() + except Exception: + pass + if robot is not None: + robot.close() + if sensor is not None: + sensor.stop() + print("[System] Shutdown complete") + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/lib/ch341/CH341DLLA64.DLL b/lib/ch341/CH341DLLA64.DLL new file mode 100644 index 0000000..12a11a5 Binary files /dev/null and b/lib/ch341/CH341DLLA64.DLL differ diff --git a/lib/ch341/CH341DLLA64.LIB b/lib/ch341/CH341DLLA64.LIB new file mode 100644 index 0000000..c62ba77 Binary files /dev/null and b/lib/ch341/CH341DLLA64.LIB differ diff --git a/lib/ch341/ch341_lib.h b/lib/ch341/ch341_lib.h new file mode 100644 index 0000000..88b0093 --- /dev/null +++ b/lib/ch341/ch341_lib.h @@ -0,0 +1,399 @@ +#ifndef _CH341_LIB_H +#define _CH341_LIB_H + +typedef enum _EEPROM_TYPE { + ID_24C01, + ID_24C02, + ID_24C04, + ID_24C08, + ID_24C16, + ID_24C32, + ID_24C64, + ID_24C128, + ID_24C256, + ID_24C512, + ID_24C1024, + ID_24C2048, + ID_24C4096 +} EEPROM_TYPE; + +typedef enum _CHIP_TYPE { + CHIP_CH341 = 0, + CHIP_CH347T = 1, + CHIP_CH347F = 2, +} CHIP_TYPE; + +typedef enum { + TYPE_TTY = 0, + TYPE_HID, + TYPE_VCP, +} FUNCTYPE; + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * CH34XOpenDevice - open device + * @pathname: device path in /dev directory + * + * The function return positive file descriptor if successful, others if fail. + */ +extern int CH34xOpenDevice(const char *pathname); + +/** + * CH34XCloseDevice - close device + * @fd: file descriptor of device + * + * The function return true if successful, false if fail. + */ +extern bool CH34xCloseDevice(int fd); + +/** + * CH34x_GetDriverVersion - get vendor driver version + * @fd: file descriptor of device + * @Drv_Version: pointer to version string + * + * The function return true if successful, false if fail. + */ +extern bool CH34x_GetDriverVersion(int fd, unsigned char *Drv_Version); + +/** + * CH34x_GetChipVersion - get chip version + * @fd: file descriptor of device + * @Version: pointer to version + * + * The function return true if successful, false if fail. + */ +extern bool CH34x_GetChipVersion(int fd, unsigned char *Version); + +/** + * CH34x_GetChipType - get chip type + * @fd: file descriptor of device + * @ChipType: pointer to chip type + * + * The function return true if successful, false if fail. + */ +extern bool CH34x_GetChipType(int fd, CHIP_TYPE *ChipType); + +/** + * CH34X_GetDeviceID - get device vid and pid + * @fd: file descriptor of device + * @id: pointer to store id which contains vid and pid + * + * The function return true if successful, false if fail. + */ +extern bool CH34X_GetDeviceID(int fd, uint32_t *id); + +/** + * CH34xSetParaMode - set chip parrallel work mode + * @fd: file descriptor of device + * @Mode: work mode, 0/1->EPP mode, 2->MEM mode + * + * The function return true if successful, false if fail. + */ +extern bool CH34xSetParaMode(int fd, uint8_t Mode); + +/** + * CH34xInitParallel - initial chip parrallel work mode + * @fd: file descriptor of device + * @Mode: work mode, 0/1->EPP mode, 2->MEM mode + * + * The function return true if successful, false if fail. + */ +extern bool CH34xInitParallel(int fd, uint8_t Mode); + +/** + * CH34xEppRead - read data or addr in parrallel EPP mode + * @fd: file descriptor of device + * @oBuffer: pointer to read buffer + * @ioLength: read length + * @PipeMode: 0->read pipe0 data, 1->read pipe1 addr + * + * The function return read 0 if successful, others if fail. + */ +extern int CH34xEppRead(int fd, uint8_t *oBuffer, uint32_t ioLength, uint8_t PipeMode); + +/** + * CH34xEppWrite - write data or addr in parrallel EPP mode + * @fd: file descriptor of device + * @iBuffer: pointer to write buffer + * @ioLength: write length + * @PipeMode: 0->write pipe0 data, 1->write pipe1 addr + * + * The function return 0 if successful, others if fail. + */ +extern int CH34xEppWrite(int fd, uint8_t *iBuffer, uint32_t ioLength, uint8_t PipeMode); + +/** + * CH34xEppWriteData - write data in parrallel EPP mode + * @fd: file descriptor of device + * @iBuffer: pointer to write buffer + * @ioLength: write length + * + * The function return 0 if successful, others if fail. + */ +extern int CH34xEppWriteData(int fd, uint8_t *iBuffer, uint32_t ioLength); + +/** + * CH34xEppReadData - read data in parrallel EPP mode + * @fd: file descriptor of device + * @oBuffer: pointer to read buffer + * @ioLength: read length + * + * The function return 0 if successful, others if fail. + */ +extern int CH34xEppReadData(int fd, uint8_t *oBuffer, uint32_t ioLength); + +/** + * CH34xEppWriteAddr - write addr in parrallel EPP mode + * @fd: file descriptor of device + * @iBuffer: pointer to write buffer + * @ioLength: write length + * + * The function return 0 if successful, others if fail. + */ +extern int CH34xEppWriteAddr(int fd, uint8_t *iBuffer, uint32_t ioLength); + +/** + * CH34xEppReadAddr - read addr in parrallel EPP mode + * @fd: file descriptor of device + * @oBuffer: pointer to read buffer + * @ioLength: read length + * + * The function return 0 if successful, others if fail. + */ +extern int CH34xEppReadAddr(int fd, uint8_t *oBuffer, uint32_t ioLength); + +/** + * CH34xEppSetAddr - set addr in parrallel EPP mode + * @fd: file descriptor of device + * @iAddr: addr data + * + * The function return 0 if successful, others if fail. + */ +extern int CH34xEppSetAddr(int fd, uint32_t iAddr); + +/** + * CH34xSetTimeout - set USB data read and write timeout + * @fd: file descriptor of device + * @iWriteTimeout: data download timeout in milliseconds + * @iReadTimeout: data upload timeout in milliseconds + * + * The function return true if successful, false if fail. + */ +extern bool CH34xSetTimeout(int fd, uint32_t iWriteTimeout, uint32_t iReadTimeout); + +/** + * CH34xInitMEM - initial chip in parrallel MEM mode + * @fd: file descriptor of device + * + * The function return true if successful, false if fail. + */ +extern bool CH34xInitMEM(int fd); + +/** + * CH34xMEMReadData - read data in parrallel MEM mode + * @fd: file descriptor of device + * @oBuffer: pointer to read buffer + * @ioLength: read length + * @PipeMode: 0->read pipe0, 1->read pipe1 + * + * The function return 0 if successful, others if fail. + */ +extern int CH34xMEMReadData(int fd, uint8_t *oBuffer, uint32_t ioLength, uint8_t PipeMode); + +/** + * CH34xMEMWriteData - write data in parrallel MEM mode + * @fd: file descriptor of device + * @iBuffer: pointer to write buffer + * @ioLength: write length + * @PipeMode: 0->write pipe0, 1->write pipe1 + * + * The function return 0 if successful, others if fail. + */ +extern int CH34xMEMWriteData(int fd, uint8_t *iBuffer, uint32_t ioLength, uint32_t PipeMode); + +/** + * CH34xSetStream - configure spi/i2c interface in stream mode + * @fd: file descriptor of device + * @Mode: stream mode + * ->bit0~1: set I2C SCL rate + * --> 00 : low rate 20KHz + * --> 01 : standard rate 100KHz + * --> 10 : fast rate 400KHz + * --> 11 : high rate 750KHz + * ->bit2: set spi mode + * --> 0 : one in one out(D3: clk, D5: out, D7: in) + * --> 1 : two in two out(D3 :clk, D4/D5: out, D6/D7 :in) + * ->bit7: set spi data mode + * --> 0 : low bit first + * --> 1 : high bit first + * other bits must keep 0 + * + * The function return true if successful, false if fail. + */ +extern bool CH34xSetStream(int fd, uint8_t Mode); + +/** + * CH34xSetDelaymS - delay operation + * @fd: file descriptor of device + * @iDelay: delay time in millseconds + * + * The function return true if successful, false if fail. + */ +extern bool CH34xSetDelaymS(int fd, uint32_t iDelay); + +/** + * CH34xReadData - read for spi/i2c operation + * @fd: file descriptor of device + * @oReadBuffer: pointer to read buffer + * @oReadLength: pointer to read length + * + * The function return true if successful, false if fail. + */ +extern bool CH34xReadData(int fd, void *oReadBuffer, uint32_t *oReadLength); + +/** + * CH34xWriteData - write data for spi/i2c operation + * @fd: file descriptor of device + * @iWriteBuffer: pointer to write buffer + * @iWriteLength: pointer to write length + * + * The function return true if successful, false if fail. + */ +extern bool CH34xWriteData(int fd, void *iWriteBuffer, uint32_t *iWriteLength); + +/** + * CH34xWriteRead - write data then read for spi/i2c operation + * @fd: file descriptor of device + * @iWriteLength: write length + * @iWriteBuffer: pointer to write buffer + * @iReadStep: per read length + * @iReadTimes: read times + * @oReadLength: pointer to read length + * @oReadBuffer: pointer to read buffer + * + * The function return true if successful, false if fail. + */ +extern bool CH34xWriteRead(int fd, uint32_t iWriteLength, void *iWriteBuffer, uint32_t iReadStep, uint32_t iReadTimes, + uint32_t *oReadLength, void *oReadBuffer); + +/** + * CH34xGetInput - get io status of CH341 + * @fd: file descriptor of device + * @iStatus: pointer to io status + * Note: + * Bit7~Bit0<==>D7-D0, Bit8<==>ERR#, Bit9<==>PEMP, Bit10<==>INT# + * Bit11<==>SLCT, Bit13<==>WAIT#, Bit14<==>DATAS#/READ#, Bit15<==>ADDRS#/ADDR/ALE, Bit23<==>SDA + * + * The function return true if successful, false if fail. + */ +extern bool CH34xGetInput(int fd, uint32_t *iStatus); + +/** + * CH34xSetOutput - set direction and output data of CH341 + * @fd: file descriptor of device + * @iEnable: set direction and data enable + * --> Bit16 High : effect on Bit15~8 of iSetDataOut + * --> Bit17 High : effect on Bit15~8 of iSetDirOut + * --> Bit18 High : effect on Bit7~0 of iSetDataOut + * --> Bit19 High : effect on Bit7~0 of iSetDirOut + * --> Bit20 High : effect on Bit23~16 of iSetDataOut + * @iSetDirOut: set io direction + * -- > Bit High : Output + * -- > Bit Low : Input + * @iSetDataOut: set io data + * Output: + * -- > Bit High : High level + * -- > Bit Low : Low level + * Note: + * Bit7~Bit0<==>D7-D0, Bit8<==>ERR#, Bit9<==>PEMP, Bit10<==>INT# + * Bit11<==>SLCT, Bit13<==>WAIT#, Bit14<==>DATAS#/READ#, Bit15<==>ADDRS#/ADDR/ALE + * + * The pins below can only be used in output mode: + * Bit16<==>RESET#, Bit17<==>WRITE#, Bit18<==>SCL, Bit29<==>SDA + * + * The function return true if successful, false if fail. + */ +extern bool CH34xSetOutput(int fd, uint32_t iEnable, uint32_t iSetDirOut, uint32_t iSetDataOut); + +/** + * CH34xSet_D5_D0 - set direction and output data of D5-D0 on CH341 + * @fd: file descriptor of device + * @iSetDirOut: set io direction + * -- > Bit High : Output + * -- > Bit Low : Input + * @iSetDataOut: set io data + * Output: + * -- > Bit High : High level + * -- > Bit Low : Low level + * + * The function return true if successful, false if fail. + */ +extern bool CH34xSet_D5_D0(int fd, uint8_t iSetDirOut, uint8_t iSetDataOut); + +/** + * CH34xStreamI2C - write/read i2c in stream mode + * @fd: file descriptor of device + * @iWriteLength: write length + * @iWriteBuffer: pointer to write buffer + * @iReadLength: read length + * @oReadBuffer: pointer to read buffer + * + * The function return true if successful, false if fail. + */ +extern bool CH34xStreamI2C(int fd, uint32_t iWriteLength, void *iWriteBuffer, uint32_t iReadLength, void *oReadBuffer); + +/** + * CH34xReadEEPROM - read data from eeprom + * @fd: file descriptor of device + * @iEepromID: eeprom type + * @iAddr: address of eeprom + * @iLength: read length + * @oBuffer: pointer to read buffer + * + * The function return true if successful, false if fail. + */ +extern bool CH34xReadEEPROM(int fd, EEPROM_TYPE iEepromID, uint32_t iAddr, uint32_t iLength, uint8_t *oBuffer); + +/** + * CH34xWriteEEPROM - write data to eeprom + * @fd: file descriptor of device + * @iEepromID: eeprom type + * @iAddr: address of eeprom + * @iLength: write length + * @iBuffer: pointer to write buffer + * + * The function return true if successful, false if fail. + */ +extern bool CH34xWriteEEPROM(int fd, EEPROM_TYPE iEepromID, uint32_t iAddr, uint32_t iLength, uint8_t *iBuffer); + +/** + * CH34xStreamSPIx - write/read spi in stream mode + * @fd: file descriptor of device + * @iChipSelect: cs enable + * @iLength: the length of data + * @ioBuffer: one in one out buffer + * @ioBuffer2: two in two out buffer + * + * The function return true if successful, false if fail. + */ +extern bool CH34xStreamSPIx(int fd, uint32_t iChipSelect, uint32_t iLength, void *ioBuffer, void *ioBuffer2); + +/** + * CH34xStreamSPI4 - write/read spi in 4-line stream mode + * @fd: file descriptor of device + * @iChipSelect: cs enable + * @iLength: the length of data + * @ioBuffer: one in one out buffer + * + * The function return true if successful, false if fail. + */ +extern bool CH34xStreamSPI4(int fd, uint32_t iChipSelect, uint32_t iLength, void *ioBuffer); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/lib/ch341/libch347.so b/lib/ch341/libch347.so new file mode 100644 index 0000000..a2d78e8 Binary files /dev/null and b/lib/ch341/libch347.so differ diff --git a/sensorPara.py b/sensorPara.py new file mode 100644 index 0000000..6bc95f2 --- /dev/null +++ b/sensorPara.py @@ -0,0 +1,68 @@ +from typing import List, Optional +from ctypes import Structure, sizeof, c_float, c_uint32, c_uint16 + + +class DynamicYddsComTs(Structure): + _pack_ = 1 # 按 1 字节对齐 + _fields_ = [ + ("nf", c_float), + ("nfCap", c_uint32), + ("tf", c_float), + ("tfCap", c_uint32), + ("tfDir", c_uint16), + ("prox", c_uint32), + ] + +class DynamicYddsU16Ts(Structure): + _pack_ = 1 # 按 1 字节对齐 + _fields_ = [ + ("nf", c_uint16), + ("tf", c_uint16), + ("tfDir", c_uint16), + ] + +#todo 其他三维力类型待补充 + + +class FingerHeatMap: + def __init__(self, rows: int, cols: int, file_path: str, cap_count: int, cap_indices: List[int]): + self.rows = rows + self.cols = cols + self.file_path = file_path + self.cap_count = cap_count + self.cap_indices = cap_indices + +class FingerParamTS: + def __init__(self, prg: int, pack_len: int, sensor_num: int, touch_num: int, ydds_num: int, + s_prox_num: int, m_prox_num: int, cap_byte: int, ydds_type: int, had_err: int, + cali_num: int, name: str, display_type_para: str, p_heat_map: Optional[List[FingerHeatMap]]): + self.prg = prg + self.pack_len = pack_len + self.sensor_num = sensor_num + self.touch_num = touch_num + self.ydds_num = ydds_num + self.s_prox_num = s_prox_num + self.m_prox_num = m_prox_num + self.cap_byte = cap_byte + self.ydds_type = ydds_type + self.had_err = had_err + self.cali_num = cali_num + self.name = name + self.display_type_para = display_type_para + self.p_heat_map = p_heat_map + +# 定义 fingerHeatMap 数据 +finger2_power_cap_index = [ + FingerHeatMap(16, 8, "TS-F-A/heatMapPara16_8.dat", 7, [0, 1, 2, 3, 4, 5, 6, 255, 255, 255, 255, 255, 255, 255, 255, 255]) +] + +finger17_power_cap_index = [ + FingerHeatMap(6, 7, "TS-T-A/weight6X7X6.dat", 6, [0, 1, 4, 5, 6, 7, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]), + FingerHeatMap(6, 7, "TS-T-A/weight6X7X6.dat", 6, [2, 3, 8, 9, 10, 11, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]) +] + +# 定义 fingerParams 数据 +finger_params = [ + FingerParamTS(2, 62, 8, 7, 1, 1, 0, 4, 2, 0, 22, "通用手指", "TypeA", finger2_power_cap_index), + FingerParamTS(17, 78, 16, 13, 2, 2, 1, 3, 4, 1, 22, "两指-大包", "TypeB", finger17_power_cap_index), +] diff --git a/serial_robot_driver.py b/serial_robot_driver.py new file mode 100644 index 0000000..3656ce2 --- /dev/null +++ b/serial_robot_driver.py @@ -0,0 +1,99 @@ +import serial +import time + + +class RobotDriver: + def __init__(self, port='COM6', baud=115200): + try: + self.ser = serial.Serial(port, baud, timeout=1) + print(f"[System] Connected to {port}") + time.sleep(2) # 等STM32复位 + except Exception as e: + print(f"[Error] Connection failed: {e}") + exit() + + def _send(self, cmd): + """发送原始指令""" + full_cmd = f"{cmd}\n" + self.ser.write(full_cmd.encode()) + print(f"-> Sent: {cmd}") + time.sleep(0.05) # 给单片机一点反应时间 + + # === 直线电机控制 API === + def motor_open(self): + self._send("M:OPEN") + + def motor_close(self): + self._send("M:CLOSE") + + def motor_stop(self): + self._send("M:STOP") + + # === 舵机控制 API === + def set_servo(self, servo_id, angle): + self._send(f"S{servo_id}:{angle}") + + # === 构型切换 (组合动作) === + def set_config(self, mode): + """ + mode 0: 初始 (S1=90, S2=90) + mode 1: 错位 (S1=30, S2=150) + mode 2: 对握 (S1=120, S2=60) + """ + if mode == 0: + self.set_servo(1, 90) + self.set_servo(2, 90) + elif mode == 1: + self.set_servo(1, 30) + self.set_servo(2, 150) + elif mode == 2: + self.set_servo(1, 120) + self.set_servo(2, 60) + + def close(self): + self.motor_stop() + self.ser.close() + + +# === 主程序逻辑 === +if __name__ == "__main__": + # 请修改端口号 + robot = RobotDriver(port='COM9') + + print("\n=== 全能控制面板 ===") + print(" [1] 变构型: Mode 1 (30, 150)") + print(" [2] 变构型: Mode 2 (120, 60)") + print(" [0] 变构型: Reset (90, 90)") + print(" [o] 直线电机: 张开") + print(" [c] 直线电机: 闭合") + print(" [s] 直线电机: 停止") + print(" [q] 退出") + + try: + while True: + cmd = input("指令 > ").strip().lower() + + if cmd == 'q': + break + + # 直线电机 + elif cmd == 'o': + robot.motor_open() + elif cmd == 'c': + robot.motor_close() + elif cmd == 's': + robot.motor_stop() + + # 舵机构型 + elif cmd == '1': + robot.set_config(1) + elif cmd == '2': + robot.set_config(2) + elif cmd == '0': + robot.set_config(0) + + except KeyboardInterrupt: + pass + finally: + robot.close() + print("System Shutdown.") \ No newline at end of file diff --git a/stm32f103_drv8870_servo_main.c b/stm32f103_drv8870_servo_main.c new file mode 100644 index 0000000..dff600b --- /dev/null +++ b/stm32f103_drv8870_servo_main.c @@ -0,0 +1,235 @@ +/* + * STM32F103C8T6 firmware core for the new PCB: + * - PC <-> STM32: USART1, PA9 TX, PA10 RX, 115200 8N1 + * - Linear actuator driver: DRV8870, IN1/IN2 + * - Servo outputs: TIM2 CH4/CH3 on PB11/PB10, 50 Hz PWM + * + * Put this logic into a CubeMX/HAL project. Keep the MX_* init functions + * generated by CubeMX, then add the user code below. + */ + +#include "main.h" +#include +#include +#include +#include + +extern UART_HandleTypeDef huart1; +extern TIM_HandleTypeDef htim2; + +/* PCB V1.0 measured netlist. */ +#define LM_IN1_GPIO_Port GPIOB +#define LM_IN1_Pin GPIO_PIN_12 +#define LM_IN2_GPIO_Port GPIOB +#define LM_IN2_Pin GPIO_PIN_13 + +#define SERVO1_TIM htim2 +#define SERVO1_CHANNEL TIM_CHANNEL_4 /* PB11 on STM32F103 TIM2_CH4 */ +#define SERVO2_TIM htim2 +#define SERVO2_CHANNEL TIM_CHANNEL_3 /* PB10 on STM32F103 TIM2_CH3 */ + +#define UART_RX_BUF_SIZE 96 +#define FAILSAFE_MS 1000U + +static uint8_t rx_byte; +static char rx_line[UART_RX_BUF_SIZE]; +static uint8_t rx_len = 0; +static volatile bool line_ready = false; +static uint32_t last_cmd_tick = 0; + +static void reply(const char *text) +{ + HAL_UART_Transmit(&huart1, (uint8_t *)text, strlen(text), 100); +} + +static void motor_stop(void) +{ + HAL_GPIO_WritePin(LM_IN1_GPIO_Port, LM_IN1_Pin, GPIO_PIN_RESET); + HAL_GPIO_WritePin(LM_IN2_GPIO_Port, LM_IN2_Pin, GPIO_PIN_RESET); +} + +static void motor_open(void) +{ + HAL_GPIO_WritePin(LM_IN2_GPIO_Port, LM_IN2_Pin, GPIO_PIN_RESET); + HAL_GPIO_WritePin(LM_IN1_GPIO_Port, LM_IN1_Pin, GPIO_PIN_SET); +} + +static void motor_close(void) +{ + HAL_GPIO_WritePin(LM_IN1_GPIO_Port, LM_IN1_Pin, GPIO_PIN_RESET); + HAL_GPIO_WritePin(LM_IN2_GPIO_Port, LM_IN2_Pin, GPIO_PIN_SET); +} + +static uint16_t servo_angle_to_us(int angle) +{ + if (angle < 0) { + angle = 0; + } + if (angle > 180) { + angle = 180; + } + + /* MG996R/common servo: 500 us to 2500 us at 50 Hz. */ + return (uint16_t)(500 + (angle * 2000) / 180); +} + +static void servo_set_angle(uint8_t id, int angle) +{ + uint16_t pulse_us = servo_angle_to_us(angle); + + if (id == 1) { + __HAL_TIM_SET_COMPARE(&SERVO1_TIM, SERVO1_CHANNEL, pulse_us); + } else if (id == 2) { + __HAL_TIM_SET_COMPARE(&SERVO2_TIM, SERVO2_CHANNEL, pulse_us); + } +} + +static void apply_config(uint8_t mode) +{ + switch (mode) { + case 0: + servo_set_angle(1, 90); + servo_set_angle(2, 90); + break; + case 1: + servo_set_angle(1, 30); + servo_set_angle(2, 150); + break; + case 2: + servo_set_angle(1, 120); + servo_set_angle(2, 60); + break; + default: + break; + } +} + +static void handle_command(char *cmd) +{ + last_cmd_tick = HAL_GetTick(); + + if (strcmp(cmd, "PING") == 0) { + reply("OK:PONG\r\n"); + return; + } + + if (strcmp(cmd, "M:OPEN") == 0) { + motor_open(); + reply("OK:M:OPEN\r\n"); + return; + } + + if (strcmp(cmd, "M:CLOSE") == 0) { + motor_close(); + reply("OK:M:CLOSE\r\n"); + return; + } + + if (strcmp(cmd, "M:STOP") == 0) { + motor_stop(); + reply("OK:M:STOP\r\n"); + return; + } + + if (strncmp(cmd, "S1:", 3) == 0) { + servo_set_angle(1, atoi(cmd + 3)); + reply("OK:S1\r\n"); + return; + } + + if (strncmp(cmd, "S2:", 3) == 0) { + servo_set_angle(2, atoi(cmd + 3)); + reply("OK:S2\r\n"); + return; + } + + if (strncmp(cmd, "CFG:", 4) == 0) { + apply_config((uint8_t)atoi(cmd + 4)); + reply("OK:CFG\r\n"); + return; + } + + reply("ERR:UNKNOWN\r\n"); +} + +void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) +{ + if (huart->Instance != USART1) { + return; + } + + if (rx_byte == '\n' || rx_byte == '\r') { + if (rx_len > 0) { + rx_line[rx_len] = '\0'; + line_ready = true; + rx_len = 0; + } + } else if (rx_len < UART_RX_BUF_SIZE - 1) { + rx_line[rx_len++] = (char)rx_byte; + } else { + rx_len = 0; + reply("ERR:LINE_TOO_LONG\r\n"); + } + + HAL_UART_Receive_IT(&huart1, &rx_byte, 1); +} + +/* + * Call this after MX_GPIO_Init(), MX_USART1_UART_Init(), MX_TIM2_Init(). + * CubeMX TIM2 recommendation: + * channels: PB10 = TIM2_CH3, PB11 = TIM2_CH4 + * enable TIM2 remap for PB10/PB11 if CubeMX does not do it automatically + * clock = 72 MHz, prescaler = 71, counter period = 19999 + * PWM pulse units are microseconds. + */ +void app_init(void) +{ + motor_stop(); + +#ifdef __HAL_AFIO_REMAP_TIM2_PARTIAL_2 + __HAL_RCC_AFIO_CLK_ENABLE(); + __HAL_AFIO_REMAP_TIM2_PARTIAL_2(); +#endif + + HAL_TIM_PWM_Start(&SERVO1_TIM, SERVO1_CHANNEL); + HAL_TIM_PWM_Start(&SERVO2_TIM, SERVO2_CHANNEL); + servo_set_angle(1, 90); + servo_set_angle(2, 90); + last_cmd_tick = HAL_GetTick(); + HAL_UART_Receive_IT(&huart1, &rx_byte, 1); + reply("READY:DRV8870_SERVO\r\n"); +} + +void app_loop(void) +{ + if (line_ready) { + char cmd[UART_RX_BUF_SIZE]; + __disable_irq(); + strncpy(cmd, rx_line, sizeof(cmd)); + cmd[sizeof(cmd) - 1] = '\0'; + line_ready = false; + __enable_irq(); + handle_command(cmd); + } + + if ((HAL_GetTick() - last_cmd_tick) > FAILSAFE_MS) { + motor_stop(); + } +} + +/* + * In generated main.c, use: + * + * int main(void) + * { + * HAL_Init(); + * SystemClock_Config(); + * MX_GPIO_Init(); + * MX_USART1_UART_Init(); + * MX_TIM2_Init(); + * app_init(); + * while (1) { + * app_loop(); + * } + * } + */ diff --git a/tactile_sensor_daq.py b/tactile_sensor_daq.py new file mode 100644 index 0000000..33b3e78 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/test_dof_control.py b/test_dof_control.py new file mode 100644 index 0000000..d9f7fe9 --- /dev/null +++ b/test_dof_control.py @@ -0,0 +1,146 @@ +import time +import keyboard + +from demo_auto_grasp import TactileSensorDAQ, RobotDriver, SafetyGuard + +# ====== 可按实际机构方向微调的参数 ====== +SERVO_HOME_ANGLE = 90 +SERVO_CW_TO_OPPOSE_ANGLE = 120 +SERVO_HOLD_SEC = 1.0 +FORCE_STOP_THRESHOLD = 0.3 +PRESSURE_DISPLAY_KPA = 80 +ROBOT_COM_PORT = 'COM9' + + +def wait_with_pause(safety, duration_s, step_s=0.02): + start = time.time() + while (time.time() - start) < duration_s: + safety.check_pause() + time.sleep(step_s) + + +def run_servo_single_cycle(robot, safety, servo_id): + print(f"[Mode {servo_id}] 舵机{servo_id}开始: 手指1顺时针到对握位 -> 回位") + + safety.check_pause() + # 仅发送指定舵机命令，其他舵机不动作 + robot.set_servo(servo_id, SERVO_CW_TO_OPPOSE_ANGLE) + + wait_with_pause(safety, SERVO_HOLD_SEC) + + safety.check_pause() + robot.set_servo(servo_id, SERVO_HOME_ANGLE) + wait_with_pause(safety, 0.5) + + # 明确停止直线电机，确保其不动作 + robot.motor_stop() + print(f"[Mode {servo_id}] 完成。") + + +def run_linear_mode(robot, safety): + print("[Mode 3] 直线电机控制模式 (无传感器)") + print("按键: w=张开, s=闭合, q=退出模式") + print("规则: 无传感器，按住s持续闭合") + print("全局: 空格急停/恢复") + + motion_state = 'idle' # idle/open/close/force_stop + prev_q = False + linear_start_time = None + + def print_runtime(final=False): + if linear_start_time is None: + return + elapsed = time.time() - linear_start_time + if final: + print(f"[Mode 3] s执行总时长: {elapsed:.2f}s") + + while True: + safety.check_pause() + + w_now = keyboard.is_pressed('w') + s_now = keyboard.is_pressed('s') + q_now = keyboard.is_pressed('q') + + q_edge = q_now and not prev_q + + if q_edge: + robot.motor_stop() + print_runtime(final=True) + linear_start_time = None + print("[Mode 3] 退出直线电机控制模式") + break + + # 仅允许单键控制: w 和 s 同时按下时停止，防止方向冲突 + if w_now and not s_now: + if motion_state != 'open': + robot.motor_open() + motion_state = 'open' + print("[Mode 3] 张开中 (按住 w)") + elif s_now and not w_now: + if linear_start_time is None: + linear_start_time = time.time() + if motion_state != 'close': + robot.motor_close() + motion_state = 'close' + print("[Mode 3] 闭合中 (按住 s)") + else: + if motion_state in ('open', 'close', 'force_stop'): + robot.motor_stop() + print_runtime(final=True) + motion_state = 'idle' + print("[Mode 3] 松开按键, 电机停止") + linear_start_time = None + + prev_q = q_now + + time.sleep(0.02) + + +def main(): + # sensor = TactileSensorDAQ() + robot = RobotDriver(port=ROBOT_COM_PORT) + safety = SafetyGuard(robot) + + try: + # sensor.start() + # print("等待传感器稳定...") + # time.sleep(2) + # sensor.tare() + + while True: + print("\n========================") + print("自由度控制选择:") + print(" 1: 舵机1控制 (手指1顺时针到对握后回位)") + print(" 2: 舵机2控制 (手指1顺时针到对握后回位)") + print(f" 3: 直线电机控制 (w张开/s闭合, 压强显示固定为 {PRESSURE_DISPLAY_KPA}KPa)") + print(" q: 退出程序") + print("全局: 空格急停/恢复") + + choice = input("请输入 1/2/3/q: ").strip().lower() + + if choice == '1': + run_servo_single_cycle(robot, safety, servo_id=1) + elif choice == '2': + run_servo_single_cycle(robot, safety, servo_id=2) + elif choice == '3': + run_linear_mode(robot, safety) + elif choice == 'q': + break + else: + print("输入无效，请重新输入。") + + except KeyboardInterrupt: + pass + finally: + robot.motor_stop() + robot.close() + # sensor.stop() + try: + keyboard.unhook_all() + except Exception: + pass + print("System All Shutdown.") + + +if __name__ == '__main__': + main() diff --git a/传感器iic地址和转接板位置定义.png b/传感器iic地址和转接板位置定义.png new file mode 100644 index 0000000..c72a8f1 Binary files /dev/null and b/传感器iic地址和转接板位置定义.png differ diff --git a/接线说明.txt b/接线说明.txt new file mode 100644 index 0000000..1a0392f --- /dev/null +++ b/接线说明.txt @@ -0,0 +1,16 @@ +USB-TTL & STM32F103C8T6 +3.3-3.3 +GVD-GND +RXD-A9 +TXD-A10 + + STM32F103C8T6 & ELSE +IN1-B12 +IN2-B13 +GND-舵机GND +GND-直线电机GND + + +A0绿A1橙舵机信号线 + +运行方式: python gamepad_remote_new_pcb.py --port COM9