init repo
This commit is contained in:
Brunsmeier
42
.gitignore
vendored
Normal file
42
.gitignore
vendored
Normal file
@ -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
|
||||||
BIN
CH341驱动（先卸载再安装）.zip
Normal file
BIN
CH341驱动（先卸载再安装）.zip
Normal file
Binary file not shown.
BIN
CH341驱动（先卸载再安装）/CH341PAR (1).EXE
Executable file
BIN
CH341驱动（先卸载再安装）/CH341PAR (1).EXE
Executable file
Binary file not shown.
717
README.md
Normal file
717
README.md
Normal file
@ -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:<angle>` | 舵机1角度(0-180) | `OK:S1` |
|
||||||
|
| `S2:<angle>` | 舵机2角度(0-180) | `OK:S2` |
|
||||||
|
| `CFG:<mode>` | 构型切换(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 # 本文件
|
||||||
|
```
|
||||||
199
cap_read.py
Normal file
199
cap_read.py
Normal file
@ -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()
|
||||||
267
class_ch341.py
Normal file
267
class_ch341.py
Normal file
@ -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
|
||||||
184
class_finger.py
Normal file
184
class_finger.py
Normal file
@ -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()
|
||||||
273
class_sensorcmd.py
Normal file
273
class_sensorcmd.py
Normal file
@ -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
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
85
data_logger.py
Normal file
85
data_logger.py
Normal file
@ -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.")
|
||||||
379
demo_auto_grasp.py
Normal file
379
demo_auto_grasp.py
Normal file
@ -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.")
|
||||||
218
gamepad_remote.py
Normal file
218
gamepad_remote.py
Normal file
@ -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()
|
||||||
249
gamepad_remote_new_pcb.py
Normal file
249
gamepad_remote_new_pcb.py
Normal file
@ -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()
|
||||||
113
grasp_network_model.py
Normal file
113
grasp_network_model.py
Normal file
@ -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}")
|
||||||
372
keyboard_remote_new_pcb.py
Normal file
372
keyboard_remote_new_pcb.py
Normal file
@ -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()
|
||||||
BIN
lib/ch341/CH341DLLA64.DLL
Normal file
BIN
lib/ch341/CH341DLLA64.DLL
Normal file
Binary file not shown.
BIN
lib/ch341/CH341DLLA64.LIB
Normal file
BIN
lib/ch341/CH341DLLA64.LIB
Normal file
Binary file not shown.
399
lib/ch341/ch341_lib.h
Normal file
399
lib/ch341/ch341_lib.h
Normal file
@ -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
|
||||||
BIN
lib/ch341/libch347.so
Normal file
BIN
lib/ch341/libch347.so
Normal file
Binary file not shown.
68
sensorPara.py
Normal file
68
sensorPara.py
Normal file
@ -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),
|
||||||
|
]
|
||||||
99
serial_robot_driver.py
Normal file
99
serial_robot_driver.py
Normal file
@ -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.")
|
||||||
235
stm32f103_drv8870_servo_main.c
Normal file
235
stm32f103_drv8870_servo_main.c
Normal file
@ -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 <stdbool.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
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();
|
||||||
|
* }
|
||||||
|
* }
|
||||||
|
*/
|
||||||
228
tactile_sensor_daq.py
Normal file
228
tactile_sensor_daq.py
Normal file
@ -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()
|
||||||
146
test_dof_control.py
Normal file
146
test_dof_control.py
Normal file
@ -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()
|
||||||
BIN
传感器iic地址和转接板位置定义.png
Normal file
BIN
传感器iic地址和转接板位置定义.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 659 KiB |
Reference in New Issue
Block a user