上传文件至 /
This commit is contained in:
@@ -0,0 +1,671 @@
|
||||
# 树莓派 5 圆柱运动控制系统
|
||||
|
||||
> 本项目运行在 **Raspberry Pi 5**（树莓派 5）上，通过多种通信协议控制三个圆柱在水槽中的运动，并实时测量受力情况。
|
||||
|
||||
---
|
||||
|
||||
## 目录
|
||||
|
||||
1. [项目简介](#1-项目简介)
|
||||
2. [整体架构](#2-整体架构)
|
||||
3. [什么是树莓派（Raspberry Pi）？](#3-什么是树莓派raspberry-pi)
|
||||
4. [GPIO 概念](#4-gpio-概念)
|
||||
5. [通信协议简介](#5-通信协议简介)
|
||||
6. [Python 环境搭建](#6-python-环境搭建)
|
||||
7. [项目文件结构](#7-项目文件结构)
|
||||
8. [如何运行](#8-如何运行)
|
||||
9. [三个驱动模块详解](#9-三个驱动模块详解)
|
||||
10. [signal_features.json 说明](#10-signal_featuresjson-说明)
|
||||
11. [常见问题 FAQ](#11-常见问题-faq)
|
||||
12. [参考资料](#12-参考资料)
|
||||
|
||||
---
|
||||
|
||||
## 1. 项目简介
|
||||
|
||||
这是一个在 **Raspberry Pi 5** 上运行的控制系统，用于在水槽中控制三个圆柱的运动并测量受力。系统通过以下方式工作：
|
||||
|
||||
- 三个**空心杯电机**（带编码器反馈）控制圆柱的**旋转**
|
||||
- 三个**步进电机**控制圆柱的**直线移动**
|
||||
- 一个**高精度 ADC**（模数转换器）测量应变片传来的电压信号，从而算出受力大小
|
||||
|
||||
---
|
||||
|
||||
## 2. 整体架构
|
||||
|
||||
```
|
||||
┌─────────────────────────────────────────────────────────────────────┐
|
||||
│ Raspberry Pi 5 │
|
||||
│ │
|
||||
│ ┌──────────────┐ ┌──────────────┐ ┌──────────────────────┐ │
|
||||
│ │ SPI 总线 │ │ I2C 总线 │ │ 硬件 PWM │ │
|
||||
│ │ /dev/spidev0.0│ │ /dev/i2c-1 │ │ GPIO12 输出 │ │
|
||||
│ └───────┬───────┘ └──────┬───────┘ └──────────┬───────────┘ │
|
||||
│ │ │ │ │
|
||||
└───────────┼───────────────────┼────────────────────────┼──────────────┘
|
||||
│ │ │
|
||||
▼ ▼ ▼
|
||||
┌──────────────┐ ┌──────────────────┐ ┌──────────────────┐
|
||||
│ ADS124S08 │ │ 4EncoderMotor │ │ StepMotor Driver │
|
||||
│ ADC 芯片 │ │ 电机模块 (I2C) │ │ (I2C + PWM) │
|
||||
│ (SPI 通信) │ │ 地址: 0x24 │ │ 地址: 0x27 │
|
||||
└──────┬───────┘ └────────┬─────────┘ └────────┬─────────┘
|
||||
│ │ │
|
||||
▼ ▼ ▼
|
||||
应变片信号 空心杯电机 (×4) 步进电机 (×3)
|
||||
(电压测量) (控制圆柱旋转) (控制圆柱平移)
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 3. 什么是树莓派（Raspberry Pi）？
|
||||
|
||||
树莓派是一台只有**信用卡大小**的 Linux 计算机。它虽然小，但具备普通电脑的所有功能——有 CPU、内存、USB 接口、HDMI 视频输出、网络接口等。树莓派最特别的地方是它有一排**GPIO 引脚**，可以通过这些引脚直接控制外部电子设备（比如电机、传感器、灯光等）。
|
||||
|
||||
### 本项目使用的型号：Raspberry Pi 5
|
||||
|
||||
### 树莓派引脚图
|
||||
|
||||
树莓派 5 的 40 针 GPIO 排针布局如下（从上方俯视，SD 卡槽朝上）：
|
||||
|
||||
```
|
||||
3.3V (1) (2) 5V
|
||||
GPIO2 (SDA) (3) (4) 5V
|
||||
GPIO3 (SCL) (5) (6) GND
|
||||
GPIO4 (7) (8) GPIO14 (TXD)
|
||||
GND (9) (10) GPIO15 (RXD)
|
||||
GPIO17 (11)(12) GPIO18
|
||||
GPIO27 (13)(14) GND
|
||||
GPIO22 (15)(16) GPIO23
|
||||
3.3V (17)(18) GPIO24
|
||||
GPIO10(MOSI)(19)(20) GND
|
||||
GPIO9(MISO)(21)(22) GPIO25
|
||||
GPIO11(SCLK)(23)(24) GPIO8 (CS)
|
||||
GND (25)(26) GPIO7 (CS1)
|
||||
GPIO0(IDSD)(27)(28) GPIO1 (IDSC)
|
||||
GPIO5 (29)(30) GND
|
||||
GPIO6 (31)(32) GPIO12 (PWM)
|
||||
GPIO13 (33)(34) GND
|
||||
GPIO19 (35)(36) GPIO16
|
||||
GPIO26 (37)(38) GPIO20
|
||||
GND (39)(40) GPIO21
|
||||
```
|
||||
|
||||
> 详细引脚功能说明请参考：[树莓派官方引脚文档](https://www.raspberrypi.com/documentation/computers/raspberry-pi.html)
|
||||
|
||||
---
|
||||
|
||||
## 4. GPIO 概念
|
||||
|
||||
### 什么是 GPIO？
|
||||
|
||||
GPIO 是 **General Purpose Input Output**（通用输入输出引脚）的缩写。可以把 GPIO 引脚想象成树莓派身上伸出来的"小触手"，每个触手可以：
|
||||
|
||||
- **输出高电平（3.3V）或低电平（0V）**：相当于开或关一个开关
|
||||
- **检测外部电平是高还是低**：相当于感应外部有没有电信号
|
||||
|
||||
### 高电平和低电平
|
||||
|
||||
- **高电平（HIGH / 1 / 3.3V）**：引脚输出 3.3 伏特的电压，相当于"开"
|
||||
- **低电平（LOW / 0 / 0V）**：引脚输出 0 伏特的电压，相当于"关"
|
||||
|
||||
简单来说，树莓派通过把 GPIO 引脚设置为高电平或低电平，来控制外部设备。比如把某个引脚设为高电平，就等于告诉连接的设备"开始工作"。
|
||||
|
||||
### 本项目中使用的 GPIO 引脚
|
||||
|
||||
| 引脚 (BCM) | 功能 | 连接设备 |
|
||||
|---|---|---|
|
||||
| GPIO12 | PWM 输出（产生步进脉冲） | StepMotor 模块 |
|
||||
| GPIO5 | 步进电机 0 方向控制 | StepMotor 模块 |
|
||||
| GPIO6 | 步进电机 1 方向控制 | StepMotor 模块 |
|
||||
| GPIO16 | 步进电机 2 方向控制 | StepMotor 模块 |
|
||||
| GPIO22 | 电源阶段 1 控制（先供电） | ADC (ADS124S08) |
|
||||
| GPIO17 | 电源阶段 1 控制（先供电） | ADC |
|
||||
| GPIO24 | 电源阶段 2 控制（后供电） | ADC |
|
||||
| GPIO27 | 电源阶段 2 控制（后供电） | ADC |
|
||||
| GPIO23 | 保持低电平（初始化期间） | ADC |
|
||||
| GPIO18 | 保持低电平（初始化期间） | ADC |
|
||||
| GPIO25 | DRDY 输入（检测ADC数据就绪） | ADC |
|
||||
| GPIO10 | SPI MOSI（主出从入） | ADC |
|
||||
| GPIO9 | SPI MISO（主入从出） | ADC |
|
||||
| GPIO11 | SPI SCLK（时钟线） | ADC |
|
||||
| GPIO8 | SPI CS（片选，低电平选中） | ADC |
|
||||
| GPIO2 | I2C SDA（数据线） | 电机模块 |
|
||||
| GPIO3 | I2C SCL（时钟线） | 电机模块 |
|
||||
|
||||
> BCM 编号是 Broadcom 芯片的 GPIO 编号方式，也是 Python 库（如 gpiozero）默认使用的编号方式。
|
||||
|
||||
---
|
||||
|
||||
## 5. 通信协议简介
|
||||
|
||||
### 5.1 SPI（Serial Peripheral Interface，串行外设接口）
|
||||
|
||||
#### 是什么
|
||||
|
||||
SPI 是一种**同步串行通信协议**，用 **4 根线**连接主设备（这里是树莓派）和从设备（这里是 ADC 芯片）。
|
||||
|
||||
#### 四根线的比喻
|
||||
|
||||
把 SPI 通信想象成老师和学生对话：
|
||||
|
||||
| 线名 | 全称 | 比喻 |
|
||||
|---|---|---|
|
||||
| **SCLK** | Serial Clock（串行时钟） | 老师用**节拍器**打拍子，所有人按这个节奏说话 |
|
||||
| **MOSI** | Master Out Slave In（主出从入） | **老师对学生说话**的通道 |
|
||||
| **MISO** | Master In Slave Out（主入从出） | **学生对老师说话**的通道 |
|
||||
| **CS** | Chip Select（片选） | 老师**点名**：喊谁的名字（拉低电平），谁就说话 |
|
||||
|
||||
> **CS 低电平有效**：当 CS 线从高电平变为低电平时，表示"我要跟你说话了"；当 CS 线回到高电平时，表示"对话结束"。
|
||||
|
||||
#### 为什么 ADC 用 SPI？
|
||||
|
||||
- **速度快**：SPI 时钟频率可达几十 MHz，适合高速数据读取
|
||||
- **全双工**：可以同时发送和接收数据（就像两个人可以同时说话和听）
|
||||
- 高精度 ADC（如 ADS124S08）需要快速、可靠地传输 24 位数据，SPI 非常适合
|
||||
|
||||
#### 本项目中的 SPI
|
||||
|
||||
- **主设备**：树莓派 5
|
||||
- **从设备**：ADS124S08 ADC 芯片
|
||||
- **总线路径**：`/dev/spidev0.0`（总线 0，设备 0）
|
||||
- **时钟频率**：1 MHz（每秒传输 1,000,000 位数据）
|
||||
- **SPI 模式**：模式 1（CPOL=0，CPHA=1）
|
||||
|
||||
---
|
||||
|
||||
### 5.2 I2C（Inter-Integrated Circuit，内部集成电路）
|
||||
|
||||
#### 是什么
|
||||
|
||||
I2C 是一种**同步串行通信协议**，只用 **2 根线**就可以连接多个设备。
|
||||
|
||||
#### 两根线的比喻
|
||||
|
||||
I2C 像学校的**广播系统**：
|
||||
|
||||
| 线名 | 全称 | 比喻 |
|
||||
|---|---|---|
|
||||
| **SDA** | Serial Data（串行数据线） | 广播的**话筒和喇叭**，说话和听话都用这一根 |
|
||||
| **SCL** | Serial Clock（串行时钟线） | 广播的**节拍信号**，控制说话的节奏 |
|
||||
|
||||
所有设备都接在同一对线上，但每个设备都有一个唯一的**地址**（就像一个班每个学生有**学号**）。主机喊学号，对应学号的设备才应答；其他设备保持安静。
|
||||
|
||||
#### 为什么电机模块用 I2C？
|
||||
|
||||
- **接线简单**：只需要两根线，可以挂多个设备（每个设备用不同地址区分）
|
||||
- **设备数量**：理论上一个 I2C 总线上最多可以连接 127 个设备
|
||||
- 电机控制不需要极快的数据传输速度，I2C 完全够用
|
||||
|
||||
#### 本项目中的 I2C
|
||||
|
||||
| 设备 | I2C 地址 | 用途 |
|
||||
|---|---|---|
|
||||
| 4EncoderMotor 模块 | **0x24** | 控制空心杯电机旋转 |
|
||||
| StepMotor Driver 模块 | **0x27** | 配置步进电机（使能/复位等） |
|
||||
|
||||
树莓派通过 `/dev/i2c-1`（I2C 总线 1）与这两个模块通信。
|
||||
|
||||
---
|
||||
|
||||
### 5.3 PWM（Pulse Width Modulation，脉冲宽度调制）
|
||||
|
||||
#### 是什么
|
||||
|
||||
PWM 是一种通过**快速开关信号**来模拟不同电压输出的技术。就像快速眨眼——如果你半睁半闭眼睛，看起来就像在"眯着眼"；PWM 也是类似的道理。
|
||||
|
||||
#### 两个重要参数
|
||||
|
||||
| 参数 | 含义 | 比喻 |
|
||||
|---|---|---|
|
||||
| **频率 (Frequency)** | 每秒开关多少次，单位 Hz（赫兹） | 每秒眨眼的次数 |
|
||||
| **占空比 (Duty Cycle)** | 高电平时间占一个周期的比例 | 每次眨眼时，眼睛闭着的时间比例——80% 闭着 = 20% 占空比 |
|
||||
|
||||
比如：频率 1000 Hz = 每秒开关 1000 次。占空比 50% = 一半时间通（高电平），一半时间断（低电平）。
|
||||
|
||||
#### 为什么步进电机用 PWM？
|
||||
|
||||
步进电机是一种特殊的电机，**每收到一个脉冲就走一步**。所以：
|
||||
|
||||
- PWM 的频率越高 → 脉冲来得越快 → 电机转速越快
|
||||
- PWM 的频率越低 → 脉冲来得越慢 → 电机转速越慢
|
||||
|
||||
树莓派的**硬件 PWM**（由专用硬件产生，不受 CPU 负载影响）通过 GPIO12 引脚输出脉冲，经过 StepMotor 模块驱动步进电机。
|
||||
|
||||
---
|
||||
|
||||
## 6. Python 环境搭建
|
||||
|
||||
### 6.1 检查环境
|
||||
|
||||
树莓派 OS（操作系统）自带 Python 3。打开终端（Terminal），输入以下命令检查版本：
|
||||
|
||||
```bash
|
||||
python3 --version
|
||||
```
|
||||
|
||||
如果显示类似 `Python 3.11.x` 或更高版本，说明 Python 已安装。
|
||||
|
||||
### 6.2 创建虚拟环境（推荐）
|
||||
|
||||
虚拟环境可以把本项目的 Python 依赖与你电脑上的其他项目隔离开，避免版本冲突。
|
||||
|
||||
```bash
|
||||
# 进入项目文件夹
|
||||
cd pinball_exp_rp5
|
||||
|
||||
# 创建虚拟环境（在项目根目录下生成 venv 文件夹）
|
||||
python3 -m venv venv
|
||||
|
||||
# 激活虚拟环境
|
||||
source venv/bin/activate
|
||||
|
||||
# 激活后，终端提示符前面会出现 (venv) 字样
|
||||
```
|
||||
|
||||
> 以后每次使用本项目，都先运行 `source venv/bin/activate` 来激活虚拟环境。
|
||||
|
||||
### 6.3 安装依赖
|
||||
|
||||
首先确保 `requirements.txt` 文件在项目根目录下，然后运行：
|
||||
|
||||
```bash
|
||||
pip install -r requirements.txt
|
||||
```
|
||||
|
||||
#### 各依赖库的作用
|
||||
|
||||
| 库名 | 作用 | 安装方式 |
|
||||
|---|---|---|
|
||||
| **spidev** | 操作 SPI 总线的 Python 库。树莓派通过它跟 ADC 芯片通信。 | `pip install spidev` |
|
||||
| **smbus2** | 操作 I2C 总线的 Python 库。树莓派通过它跟电机模块通信。 | `pip install smbus2` |
|
||||
| **RPi.GPIO** | 控制 GPIO 引脚的经典库（可选）。本项目中主要用 gpiozero 代替。 | `pip install RPi.GPIO` |
|
||||
| **gpiozero** | 简化版的 GPIO 控制库。Raspberry Pi OS 自带，无需额外安装。 | 系统自带 |
|
||||
| **rpi_hardware_pwm** | 控制树莓派硬件 PWM 输出的库，用于步进电机的脉冲产生。 | `pip install rpi_hardware_pwm` |
|
||||
| **jupyter** | 运行 `.ipynb` 笔记本文件的环境，可以在浏览器中交互式运行代码。 | `pip install jupyter` |
|
||||
| **matplotlib** | 绘图库，`test.ipynb` 中用它将 ADC 采集的数据画成图表。 | `pip install matplotlib` |
|
||||
|
||||
> **注意**：`rpi_hardware_pwm` 和 `matplotlib` 不在 `requirements.txt` 中，需要单独安装。
|
||||
|
||||
```bash
|
||||
# 如果上面的安装命令执行完后还缺少某些库，可以手动补装：
|
||||
pip install rpi_hardware_pwm matplotlib jupyter
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 7. 项目文件结构
|
||||
|
||||
```
|
||||
pinball_exp_rp5/
|
||||
│
|
||||
├── README.md # 本文件 — 项目说明（你正在读的这份文档）
|
||||
├── requirements.txt # Python 依赖列表（自动安装用）
|
||||
│
|
||||
├── drv_adc.py # ADC 驱动模块 — 采集电压信号（SPI 通信）
|
||||
├── drv_encodermotor.py # 编码器电机驱动模块 — 控制圆柱旋转（I2C 通信）
|
||||
├── drv_stepmotor.py # 步进电机驱动模块 — 控制圆柱平移（I2C + PWM）
|
||||
│
|
||||
├── test.ipynb # 教学演示 Notebook（Jupyter 笔记本）
|
||||
└── signal_features.json # 实验信号参数（已脱敏处理）
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 8. 如何运行
|
||||
|
||||
```bash
|
||||
# 1. 激活虚拟环境（如果之前创建了虚拟环境）
|
||||
source venv/bin/activate
|
||||
|
||||
# 2. 启动 Jupyter Notebook
|
||||
jupyter notebook
|
||||
|
||||
# 3. 终端会显示一个链接（类似 http://127.0.0.1:8888/），
|
||||
# 用浏览器打开这个链接
|
||||
|
||||
# 4. 在浏览器中点击 test.ipynb 文件
|
||||
|
||||
# 5. 在 Notebook 中按顺序从上到下运行每个 Cell（代码单元格）：
|
||||
# - 点击一个 Cell 选中它
|
||||
# - 按 Shift + Enter 运行当前 Cell 并自动跳转到下一个
|
||||
# - 也可以点工具栏上的 "Run" 按钮
|
||||
```
|
||||
|
||||
> **什么是 Jupyter Notebook？**
|
||||
> Jupyter Notebook 是一种交互式的编程环境，把代码、运行结果、文字说明放在一个文件（`.ipynb`）中。每个代码块叫做一个"Cell"，可以单独运行，非常适合教学和实验。
|
||||
|
||||
---
|
||||
|
||||
## 9. 三个驱动模块详解
|
||||
|
||||
### 9.1 ADC 模块（`drv_adc.py`）
|
||||
|
||||
#### 控制芯片
|
||||
|
||||
**TI ADS124S08** —— 这是一款 24 位高精度模数转换器（ADC）。
|
||||
|
||||
- **24 位**意味着它可以分辨非常微小的电压变化（最小可分辨约 0.3 微伏）
|
||||
- 可以把连续的模拟电压信号转换成计算机可以处理的数字信号
|
||||
- 本项目中用于读取应变片经放大后的电压信号，从而计算受力
|
||||
|
||||
#### 通信方式
|
||||
|
||||
SPI 总线，路径：`/dev/spidev0.0`
|
||||
|
||||
#### 为什么要分两步给 ADC 上电？
|
||||
|
||||
ADS124S08 芯片的模拟电路和数字电路需要按照特定顺序启动。如果同时上电，芯片内部状态可能不稳定。所以驱动程序先启动第一部分电源（Stage 1），等待 100ms 让电源稳定后，再启动第二部分电源（Stage 2）。
|
||||
|
||||
#### 典型调用流程
|
||||
|
||||
```python
|
||||
from drv_adc import ADS124S08
|
||||
|
||||
# 1. 创建 ADC 对象（自动完成上电和初始化）
|
||||
adc = ADS124S08()
|
||||
|
||||
# 2. 配置可编程增益放大器（PGA）
|
||||
# pga_en=1: 启用放大 gain=5: 放大 32 倍
|
||||
adc.set_pga(pga_en=1, gain=5)
|
||||
|
||||
# 3. 配置采样率
|
||||
# dr=8: 200 SPS（每秒采样 200 次） mode=1: 单次转换模式
|
||||
adc.set_datarate(dr=8, mode=1)
|
||||
|
||||
# 4. 选择要测量的输入通道（单端模式，以 AINCOM 为公共端）
|
||||
# 这里选择通道 0（AIN0 引脚）
|
||||
adc.set_input_mux(pos_channel=0)
|
||||
|
||||
# 5. 读取多个通道的电压原始值
|
||||
values = adc.request_channels([0, 1, 2])
|
||||
|
||||
# 6. 清理（关闭 SPI 总线，关掉 GPIO 电源）
|
||||
adc.close()
|
||||
```
|
||||
|
||||
#### 几个重要的配置
|
||||
|
||||
**PGA 增益倍数对照表：**
|
||||
|
||||
| gain 编码 | 实际放大倍数 | 适用信号 |
|
||||
|---|---|---|
|
||||
| 0 | ×1 | 强信号（>1V） |
|
||||
| 1 | ×2 | 中等信号 |
|
||||
| 2 | ×4 | 中等信号 |
|
||||
| 3 | ×8 | 较弱信号 |
|
||||
| 4 | ×16 | 弱信号 |
|
||||
| 5 | ×32 | 微弱信号（如应变片） |
|
||||
| 6 | ×64 | 极微弱信号 |
|
||||
| 7 | ×128 | 最微弱信号 |
|
||||
|
||||
**数据速率对照表：**
|
||||
|
||||
| dr 编码 | 实际速率（SPS） | 每次采样等待时间 |
|
||||
|---|---|---|
|
||||
| 0 | 2.5 | 400 ms |
|
||||
| 4 | 20 | 50 ms |
|
||||
| 8 | 200 | 5 ms |
|
||||
| 13 | 4000 | 0.25 ms |
|
||||
|
||||
> **SPS** = Samples Per Second（每秒采样次数）。数值越大，采样越快，但噪声也越大。
|
||||
|
||||
---
|
||||
|
||||
### 9.2 编码器电机模块（`drv_encodermotor.py`）
|
||||
|
||||
#### 控制硬件
|
||||
|
||||
**M5Stack Module 4EncoderMotor** —— 一个可以同时控制 4 个空心杯电机的模块。每个电机都带有编码器（可以测量电机转了多少圈）。
|
||||
|
||||
#### 通信方式
|
||||
|
||||
**I2C** 总线，模块地址：**0x24**
|
||||
|
||||
#### 什么是编码器？
|
||||
|
||||
编码器是安装在电机轴上的传感器。电机每转一个角度，编码器就产生一个脉冲。通过统计脉冲数量，计算机就知道电机转了多少圈、转到了哪个位置。
|
||||
|
||||
#### 三种控制模式
|
||||
|
||||
| 模式 | 名称 | 说明 |
|
||||
|---|---|---|
|
||||
| `NORMAL_MODE` (0x00) | 普通模式 | 直接设置 PWM 占空比控制转速，没有闭环控制 |
|
||||
| `POSITION_MODE` (0x01) | 位置模式 | 使用 PID 闭环控制，让电机转到指定位置并停住 |
|
||||
| `SPEED_MODE` (0x02) | 速度模式 | 使用 PID 闭环控制，让电机保持指定的转速 |
|
||||
|
||||
#### 什么是 PID 控制？
|
||||
|
||||
PID 是比例（P）-积分（I）-微分（D）控制的简称，是一种自动调节技术。打个比方：
|
||||
|
||||
你想让水龙头流出的水稳定在每分钟 1 升：
|
||||
|
||||
- **P（比例）**：看当前流量跟目标差多少，差得多就多拧一点，差得少就少拧一点
|
||||
- **I（积分）**：如果长时间有微小偏差（比如一直是 0.98 升），就慢慢增加拧开的幅度
|
||||
- **D（微分）**：如果流量突然变化（有人碰了水管），就快速反向调整一下
|
||||
|
||||
三个参数配合，就能让电机精确地达到并保持目标速度或位置。
|
||||
|
||||
#### 典型调用流程
|
||||
|
||||
```python
|
||||
from drv_encodermotor import EncoderMotorDriver, SPEED_MODE
|
||||
|
||||
# 使用 with 语句（自动打开和关闭 I2C 连接）
|
||||
with EncoderMotorDriver(bus=1) as driver:
|
||||
|
||||
# 1. 设置电机 0 为速度模式
|
||||
driver.set_mode(0, SPEED_MODE)
|
||||
|
||||
# 2. 设置速度模式的 PID 参数
|
||||
driver.set_speed_pid(0, kp=1, ki=100, kd=1)
|
||||
|
||||
# 3. 设置线速度（单位：米/秒）
|
||||
# 电机将以 0.01 米/秒的速度旋转
|
||||
driver.set_linear_speed_m_s(0, 0.01)
|
||||
```
|
||||
|
||||
> `with` 语句是 Python 的一种语法，确保无论代码是否出错，I2C 连接都会被正确关闭。等价于 `try: ... finally: driver.close()`。
|
||||
|
||||
---
|
||||
|
||||
### 9.3 步进电机模块（`drv_stepmotor.py`）
|
||||
|
||||
#### 控制硬件
|
||||
|
||||
**M5Stack StepMotor Driver** —— 一个可以驱动最多 3 个步进电机的模块。
|
||||
|
||||
#### 通信方式
|
||||
|
||||
- **I2C**（地址 0x27）：用于模块配置（使能、复位等）
|
||||
- **硬件 PWM**（GPIO12）：用于产生步进脉冲（控制电机速度）
|
||||
|
||||
#### 什么是步进电机？
|
||||
|
||||
步进电机是一种特殊的电机，**每收到一个电脉冲就转动一个固定的角度**（称为"步距角"）。本项目中电机的步距角是 1.8°（每圈 200 步），加上驱动器的微步进和减速器后，每圈需要 **89,600 个脉冲**。
|
||||
|
||||
#### 三个电机的关系
|
||||
|
||||
- 三个步进电机**共享同一路 PWM**（即同时以相同的频率步进）
|
||||
- 但每个电机可以**独立控制方向**（通过独立的 GPIO 引脚）
|
||||
- 这样三个圆柱可以同时移动，但方向可以不同
|
||||
|
||||
#### S 曲线加减速
|
||||
|
||||
电机突然启动或停止会产生机械冲击。本项目使用 **S 曲线加减速**来让电机平滑地加速和减速。名字来源于速度随时间变化的形状像字母 "S"。
|
||||
|
||||
具体做法是用**余弦函数**来计算每个时刻的速度：
|
||||
|
||||
- 启动时：速度从 0 慢慢增加（就像汽车起步）
|
||||
- 中间阶段：加速变快
|
||||
- 接近目标速度时：加速又变慢，最终平稳到达目标速度
|
||||
|
||||
减速时反过来，整个过程非常平滑，减少了机械冲击和电机失步的风险。
|
||||
|
||||
#### 典型调用流程
|
||||
|
||||
```python
|
||||
from drv_stepmotor import StepMotorDriver
|
||||
|
||||
# 1. 创建步进电机驱动对象
|
||||
drv = StepMotorDriver()
|
||||
|
||||
# 2. 使能电机（允许接收脉冲）
|
||||
drv.enable_motor()
|
||||
|
||||
# 3. 设置方向（True = 正向）
|
||||
drv.set_dir(True)
|
||||
|
||||
# 4. 启动电机并以 S 曲线加速到 0.01 米/秒
|
||||
drv.start(0.01)
|
||||
|
||||
# 5. 在运行中改变速度
|
||||
# drv.change_speed(0.02)
|
||||
|
||||
# 6. 以 S 曲线减速停止
|
||||
drv.stop()
|
||||
|
||||
# 7. 清理资源
|
||||
drv.close()
|
||||
|
||||
# 也支持 with 语句
|
||||
# with StepMotorDriver() as drv:
|
||||
# drv.enable_motor()
|
||||
# drv.set_dir(True)
|
||||
# drv.start(0.01)
|
||||
# time.sleep(2)
|
||||
# drv.stop()
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 10. `signal_features.json` 说明
|
||||
|
||||
这个文件存储了从 **LBM（格子玻尔兹曼方法）** 计算机仿真中提取的信号特征。LBM 是一种流体力学仿真方法，用于模拟水流绕过圆柱时的受力情况。
|
||||
|
||||
### 文件内容格式
|
||||
|
||||
```json
|
||||
{
|
||||
"signals": [
|
||||
{
|
||||
"name": "action1", // 第 1 个圆柱的信号
|
||||
"mean": 0.00166, // 信号的直流分量（平均值）
|
||||
"components": [ // 正弦波分量列表
|
||||
{
|
||||
"frequency": 0.135, // 频率 (Hz)
|
||||
"amplitude": 0.00137, // 幅值
|
||||
"phase": 0.187 // 相位 (弧度)
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
```
|
||||
|
||||
每个 action（对应一个圆柱）包含：
|
||||
- **mean（均值）**：信号的直流偏置
|
||||
- **多个正弦波分量**：每个分量有频率、幅值、相位
|
||||
|
||||
这些参数用于在主控制循环中合成周期运动信号，让圆柱按照仿真得出的最优方式运动。**数据已做脱敏处理**（数值做了随机偏移），仅供学习参考。
|
||||
|
||||
---
|
||||
|
||||
## 11. 常见问题 FAQ
|
||||
|
||||
#### Q: SPI 设备找不到
|
||||
|
||||
```bash
|
||||
ls -l /dev/spidev*
|
||||
```
|
||||
如果显示 `No such file or directory`，说明 SPI 接口未启用。
|
||||
|
||||
**解决方法**：
|
||||
```bash
|
||||
sudo raspi-config
|
||||
# 进入 Interface Options → SPI → Enable
|
||||
# 重启树莓派
|
||||
```
|
||||
|
||||
#### Q: I2C 设备找不到
|
||||
|
||||
```bash
|
||||
i2cdetect -y 1
|
||||
```
|
||||
如果命令不存在或没有显示设备地址，说明 I2C 接口未启用。
|
||||
|
||||
**解决方法**：
|
||||
```bash
|
||||
sudo raspi-config
|
||||
# 进入 Interface Options → I2C → Enable
|
||||
# 重启树莓派
|
||||
```
|
||||
|
||||
扫描到的设备地址应该显示为十六进制编号（如 `24` 和 `27`）。
|
||||
|
||||
#### Q: 电机不转
|
||||
|
||||
请按顺序检查：
|
||||
|
||||
1. **电源**：电机模块是否有外部电源供电？树莓派的 5V 引脚通常不足以驱动电机
|
||||
2. **使能信号**：是否调用了 `enable_motor()`（步进电机）或设置了正确的模式（编码器电机）
|
||||
3. **接线**：I2C 连线是否正确（SDA → GPIO2，SCL → GPIO3）
|
||||
4. **地址**：I2C 地址是否匹配（编码器电机 0x24，步进电机 0x27）
|
||||
|
||||
#### Q: ADC 读数为 0
|
||||
|
||||
1. 检查 PGA 配置是否正确（增益是否设得太低，信号太弱读不出来）
|
||||
2. 检查输入通道选择是否正确（是不是选对了 AIN 引脚）
|
||||
3. 检查 SPI 接线
|
||||
4. 用 `check_status()` 方法查看 ADC 状态寄存器
|
||||
|
||||
#### Q: ImportError: No module named xxx
|
||||
|
||||
运行以下命令安装缺失的库：
|
||||
|
||||
```bash
|
||||
pip install xxx
|
||||
```
|
||||
|
||||
常用的库安装命令：
|
||||
|
||||
```bash
|
||||
pip install spidev smbus2 rpi_hardware_pwm matplotlib jupyter
|
||||
```
|
||||
|
||||
#### Q: Permission denied（权限不足）
|
||||
|
||||
通常是因为当前用户没有访问 SPI 或 I2C 设备的权限。`pi` 用户默认有权限。如果使用其他用户，可以将用户加入相关组：
|
||||
|
||||
```bash
|
||||
sudo usermod -a -G spi,i2c,gpio 用户名
|
||||
# 然后注销并重新登录
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 12. 参考资料
|
||||
|
||||
- **ADS124S08 数据手册（TI 官方网站）**
|
||||
https://www.ti.com/product/ADS124S08
|
||||
|
||||
- **M5Stack 4EncoderMotor 模块文档**
|
||||
https://docs.m5stack.com/en/module/Module_4EncoderMotor_V1.1
|
||||
|
||||
- **M5Stack StepMotor Driver 模块文档**
|
||||
https://docs.m5stack.com/en/module/Stepmotor%20Driver%20Module13.2%20v1.1
|
||||
|
||||
- **树莓派官方文档**
|
||||
https://www.raspberrypi.com/documentation/
|
||||
|
||||
- **树莓派 GPIO 引脚说明**
|
||||
https://www.raspberrypi.com/documentation/computers/raspberry-pi.html
|
||||
|
||||
- **spidev Python 库**
|
||||
https://pypi.org/project/spidev/
|
||||
|
||||
- **smbus2 Python 库**
|
||||
https://pypi.org/project/smbus2/
|
||||
|
||||
- **gpiozero Python 库**
|
||||
https://gpiozero.readthedocs.io/
|
||||
+1006
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,673 @@
|
||||
"""
|
||||
树莓派 5 (Raspberry Pi 5) 上控制 M5Stack Module 4EncoderMotor 的 I2C 驱动程序。
|
||||
|
||||
模块功能：
|
||||
本驱动用于通过 I2C 总线控制 M5Stack 的 4EncoderMotor 模块。该模块可同时驱动
|
||||
4 个空心杯电机（带编码器反馈的小型直流电机），常用于旋转圆柱、机械臂关节等场景。
|
||||
每个电机都可以独立设置转速/位置模式、读取编码器数值、读取电流等。
|
||||
|
||||
I2C 通信简介：
|
||||
I2C（Inter-Integrated Circuit）是一种串行通信协议，仅需两根线（SDA 数据线 和
|
||||
SCL 时钟线）即可在多个设备之间传输数据。总线上每个设备都有一个唯一地址，主机
|
||||
（这里是树莓派）通过地址选择要通信的从设备。本模块的 I2C 地址固定为 0x24。
|
||||
|
||||
资源依赖：
|
||||
需要安装 smbus2 库（Python 的 I2C 操作库）。
|
||||
|
||||
参考文档：
|
||||
https://docs.m5stack.com/en/module/Module_4EncoderMotor_V1.1
|
||||
|
||||
用法示例：
|
||||
from drv_encodermotor import EncoderMotorDriver, NORMAL_MODE
|
||||
with EncoderMotorDriver(bus=1) as driver:
|
||||
driver.set_mode(0, NORMAL_MODE)
|
||||
driver.set_motor_speed(0, 50)
|
||||
"""
|
||||
|
||||
# 从 __future__ 模块导入 annotations 特性，允许在类型注解中使用字符串形式的类名
|
||||
# （例如在方法返回类型中引用自身类 "EncoderMotorDriver"），从而避免循环导入问题
|
||||
from __future__ import annotations
|
||||
|
||||
# 导入 struct 模块，用于将原始字节数据解析为整数、浮点数等 Python 数据类型
|
||||
# 本驱动中使用它来解析 I2C 读取回来的多字节数据（如编码器数值、电流值）
|
||||
import struct
|
||||
# 从 typing 模块导入 List 类型注解，用于提示函数参数是列表类型，提高代码可读性
|
||||
from typing import List
|
||||
|
||||
# 从 smbus2 库导入 SMBus 类，这是树莓派上操作 I2C 总线的 Python 封装库
|
||||
# SMBus 提供了读写单个字节、读写数据块等方法
|
||||
from smbus2 import SMBus
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# I2C 寄存器地址常量定义
|
||||
# 这些地址来源于 M5Stack 官方 Arduino 示例代码，每个地址对应模块内部的一个寄存器
|
||||
# 通过向这些寄存器写入或读取数据，即可控制电机或获取传感器信息
|
||||
# =============================================================================
|
||||
|
||||
# 模块的 I2C 从机地址，固定为 0x24（十六进制，十进制为 36）
|
||||
MODULE_4ENCODERMOTOR_ADDR = 0x24
|
||||
|
||||
# 舵机角度寄存器起始地址 = 0x00，用于设置舵机的目标角度
|
||||
# （第 1 个电机从 0x00 开始，第 2 个电机从 0x04 开始，依此类推）
|
||||
MODULE_4ENCODERMOTOR_SERVO_ANGLE_ADDR = 0x00
|
||||
# 舵机脉冲宽度寄存器起始地址 = 0x10，用于直接设置舵机的 PWM 脉冲宽度
|
||||
MODULE_4ENCODERMOTOR_SERVO_PULSE_ADDR = 0x10
|
||||
# PWM 占空比寄存器起始地址 = 0x20，用于设置电机的 PWM 占空比（即电机转速）
|
||||
# 第 1 个电机对应 0x20，第 2 个对应 0x21，以此类推（每个电机占 1 字节）
|
||||
MODULE_4ENCODERMOTOR_PWM_DUTY_ADDR = 0x20
|
||||
# 编码器数值寄存器起始地址 = 0x30，用于读取或设置电机的编码器计数值
|
||||
# 编码器是装在电机轴上的传感器，可以测量电机旋转的角度/圈数
|
||||
# 每个电机编码器占 4 字节（32 位有符号整数）
|
||||
MODULE_4ENCODERMOTOR_ENCODER_ADDR = 0x30
|
||||
# 速度寄存器起始地址 = 0x40，用于读取电机每 20ms 的速度值（1 字节）
|
||||
MODULE_4ENCODERMOTOR_SPEED_ADDR = 0x40
|
||||
# 8 位 ADC（模数转换器）寄存器地址 = 0xA0，用于读取模拟输入引脚的值（8 位精度）
|
||||
MODULE_4ENCODERMOTOR_ADC_8BIT_REG = 0xA0
|
||||
# 12 位 ADC 寄存器地址 = 0xB0，用于读取模拟输入引脚的值（12 位精度，更精确）
|
||||
MODULE_4ENCODERMOTOR_ADC_12BIT_REG = 0xB0
|
||||
# 跳转到 Bootloader（引导加载程序）的寄存器地址 = 0xFD，用于进入固件升级模式
|
||||
JUMP_TO_BOOTLOADER_REG = 0xFD
|
||||
# 升级 Bootloader 的寄存器地址 = 0xE0，用于触发 Bootloader 更新流程
|
||||
UPGRADE_BOOTLOADER_REG = 0xE0
|
||||
# 固件版本号寄存器地址 = 0xFE，读取此地址可获得模块当前固件版本
|
||||
MODULE_4ENCODERMOTOR_FIRMWARE_VERSION_ADDR = 0xFE
|
||||
# Bootloader 版本号寄存器地址 = 0xFC，读取此地址可获得 Bootloader 版本
|
||||
MODULE_4ENCODERMOTOR_BOOTLOADER_VERSION_ADDR = 0xFC
|
||||
# I2C 地址配置寄存器地址 = 0xFF，用于读取或修改模块的 I2C 地址
|
||||
MODULE_4ENCODERMOTOR_I2C_ADDRESS_ADDR = 0xFF
|
||||
|
||||
# 配置块寄存器起始地址 = 0x50，用于设置电机的控制模式、PID 参数等
|
||||
MODULE_4ENCODERMOTOR_CONFIG_ADDR = 0x50
|
||||
# 电流寄存器地址 = 0x90，用于读取当前电机总电流（4 字节浮点数）
|
||||
MODULE_4ENCODERMOTOR_CURRENT_ADDR = 0x90
|
||||
# 软启停寄存器地址 = 0xD1，用于启用或禁用电机的软启动/软停止功能
|
||||
MODULE_4ENCODERMOTOR_SOFT_START_STOP_ADDR = 0xD1
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 电机工作模式常量定义
|
||||
# =============================================================================
|
||||
|
||||
# 普通模式 = 0x00，直接通过 PWM 占空比控制电机转速，不做闭环控制
|
||||
NORMAL_MODE = 0x00
|
||||
# 位置模式 = 0x01，使用 PID 闭环控制，使电机旋转到指定位置
|
||||
POSITION_MODE = 0x01
|
||||
# 速度模式 = 0x02，使用 PID 闭环控制，使电机保持指定转速
|
||||
SPEED_MODE = 0x02
|
||||
# IAP（在应用中编程）更新模式 = 0x03，用于在线升级固件
|
||||
IAP_UPDATE_MODE = 0x03
|
||||
# Bootloader 更新模式 = 0x04，用于通过 Bootloader 升级固件
|
||||
BOOTLOADER_UPDATE_MODE = 0x04
|
||||
|
||||
|
||||
def _constrain_index(index: int) -> int:
|
||||
"""
|
||||
将电机索引限制在 0~3 范围内（4 个电机），防止越界访问。
|
||||
|
||||
参数:
|
||||
index: 原始电机索引号（用户传入的，可能是负数或大于 3 的数）
|
||||
返回:
|
||||
限制后的有效索引值，范围 0~3
|
||||
"""
|
||||
# 如果索引小于 0，则返回 0（第 1 个电机），避免负数索引导致错误
|
||||
if index < 0:
|
||||
return 0
|
||||
# 如果索引大于 3，则返回 3（第 4 个电机），避免超出模块支持的电机数量
|
||||
if index > 3:
|
||||
return 3
|
||||
# 索引在有效范围内（0~3），直接返回原值
|
||||
return index
|
||||
|
||||
|
||||
class EncoderMotorError(RuntimeError):
|
||||
"""
|
||||
自定义异常类，继承自 RuntimeError。
|
||||
当 I2C 通信失败或参数超出范围时抛出此异常，方便调用方捕获并处理错误。
|
||||
"""
|
||||
# pass 表示这是一个空类，不添加额外的方法或属性
|
||||
# 只改变异常的名字，便于区分是哪个模块出的错误
|
||||
pass
|
||||
|
||||
|
||||
class EncoderMotorDriver:
|
||||
"""树莓派上控制 M5Stack 4EncoderMotor 模块的最小化驱动程序。
|
||||
|
||||
本类尽量遵循 Arduino 示例代码中的寄存器布局和字节序约定，以保证不同平台
|
||||
（Arduino 和树莓派）之间的兼容性。
|
||||
|
||||
关于字节序（Byte Order）的说明（以匹配 Arduino 实现）：
|
||||
- 编码器寄存器（MODULE_4ENCODERMOTOR_ENCODER_ADDR）：使用大端序（big-endian）
|
||||
有符号 32 位整数。Arduino 代码中将字节按 b0<<24 | b1<<16 | b2<<8 | b3
|
||||
的方式拼接，因此我们从树莓派端读取时需要按大端序解析。
|
||||
- 配置块中的位置点（setPositionPoint）：使用小端序（little-endian）写入，
|
||||
Arduino 代码中先发送最低有效字节（LSB first）。
|
||||
- 电流浮点数：以 4 个原始字节传输，采用模块本身的浮点数表示方式。
|
||||
Arduino 的 memcpy 表明是小端序浮点排列，因此我们用 '<f'
|
||||
（'<' 表示小端序，'f' 表示 float）来解析。
|
||||
|
||||
big-endian（大端序）：高位字节在前（存储在低地址）。例如数值 0x12345678 在
|
||||
内存中顺序为 0x12 0x34 0x56 0x78。
|
||||
little-endian（小端序）：低位字节在前（存储在低地址）。例如数值 0x12345678
|
||||
在内存中顺序为 0x78 0x56 0x34 0x12。
|
||||
"""
|
||||
|
||||
def __init__(self, i2c_addr: int = MODULE_4ENCODERMOTOR_ADDR, bus: int = 1):
|
||||
"""
|
||||
初始化电机驱动对象，打开 I2C 总线。
|
||||
|
||||
参数:
|
||||
i2c_addr: 模块的 I2C 地址，默认为 0x24（MODULE_4ENCODERMOTOR_ADDR）
|
||||
bus: 树莓派上的 I2C 总线编号。树莓派 5 的默认 I2C 总线是 bus 1
|
||||
（对应 GPIO 引脚 2（SDA）和 3（SCL））
|
||||
"""
|
||||
# 保存模块的 I2C 地址到实例变量 self.addr，供后续读写操作使用
|
||||
self.addr = i2c_addr
|
||||
# 保存 I2C 总线编号到实例变量 self.bus_num，以备将来可能重新连接时使用
|
||||
self.bus_num = bus
|
||||
# 创建 SMBus 对象，打开指定编号的 I2C 总线，开始与模块通信
|
||||
self.bus = SMBus(bus)
|
||||
|
||||
def close(self) -> None:
|
||||
"""
|
||||
关闭 I2C 总线连接，释放资源。
|
||||
使用完驱动后应调用此方法，或使用 with 语句让 Python 自动调用。
|
||||
"""
|
||||
try:
|
||||
# 检查总线对象是否不为 None，避免重复关闭导致异常
|
||||
if self.bus is not None:
|
||||
# 调用 SMBus 对象的 close 方法，关闭 I2C 总线
|
||||
self.bus.close()
|
||||
finally:
|
||||
# 无论 close 是否成功，都将总线对象设为 None，标记为已关闭
|
||||
# type: ignore 是告诉类型检查器忽略此处类型不匹配的警告
|
||||
self.bus = None # type: ignore
|
||||
|
||||
def __enter__(self) -> "EncoderMotorDriver":
|
||||
"""
|
||||
Python 上下文管理器协议的方法——进入 with 语句块时调用。
|
||||
配合 __exit__ 使用，使该类支持 with 语句（如 with EncoderMotorDriver() as drv:）。
|
||||
返回 self，这样在 with 块内就可以用 drv 变量来调用其他方法。
|
||||
"""
|
||||
return self
|
||||
|
||||
def __exit__(self, exc_type, exc, tb) -> None:
|
||||
"""
|
||||
Python 上下文管理器协议的方法——退出 with 语句块时调用。
|
||||
无论 with 块中是否发生异常，都会自动关闭 I2C 总线连接。
|
||||
|
||||
参数:
|
||||
exc_type: 异常类型（如果没有异常则为 None）
|
||||
exc: 异常对象（如果没有异常则为 None）
|
||||
tb: 异常回溯信息（如果没有异常则为 None）
|
||||
"""
|
||||
# 调用 close 方法关闭 I2C 总线
|
||||
self.close()
|
||||
|
||||
# =========================================================================
|
||||
# 底层 I2C 读写辅助方法
|
||||
# 这些方法封装了 smbus2 库的基本 I2C 操作，加上错误处理，方便上层方法调用
|
||||
# =========================================================================
|
||||
|
||||
def _write_byte(self, reg: int, value: int) -> None:
|
||||
"""
|
||||
向指定寄存器写入一个字节的数据。
|
||||
|
||||
参数:
|
||||
reg: 目标寄存器地址（如 0x20 表示 PWM 占空比寄存器）
|
||||
value: 要写入的整数值（只取低 8 位，高 24 位被截断）
|
||||
"""
|
||||
# 将 value 与 0xFF 做按位与运算，只保留低 8 位（一个字节）
|
||||
# 例如 value=0x1A3 时，0x1A3 & 0xFF = 0xA3，只取最低一个字节
|
||||
value &= 0xFF
|
||||
try:
|
||||
# 通过 I2C 向模块地址 self.addr 的寄存器 reg 写入一个字节 value
|
||||
# write_byte_data(地址, 寄存器, 数据) 是 smbus2 的标准方法
|
||||
self.bus.write_byte_data(self.addr, reg, value)
|
||||
except Exception as e:
|
||||
# 如果 I2C 写入过程发生任何异常（如设备未连接、总线错误等），
|
||||
# 抛出自定义的 EncoderMotorError 异常，附带错误描述和原始异常信息
|
||||
raise EncoderMotorError(f"I2C write_byte failed: {e}")
|
||||
|
||||
def _write_block(self, reg: int, data: List[int]) -> None:
|
||||
"""
|
||||
向指定寄存器写入多个字节的数据（数据块）。
|
||||
|
||||
参数:
|
||||
reg: 起始寄存器地址
|
||||
data: 要写入的整数列表，每个整数代表一个字节
|
||||
"""
|
||||
# 将 data 列表中的每个元素都与 0xFF 做按位与运算，确保每个值都在 0~255 范围内
|
||||
# 这相当于只取每个元素的低 8 位
|
||||
data = [int(x) & 0xFF for x in data]
|
||||
try:
|
||||
# 通过 I2C 向模块地址 self.addr 的起始寄存器 reg 写入 data 列表
|
||||
# write_i2c_block_data(地址, 起始寄存器, 数据列表) 会依次写入多个字节
|
||||
# 第一个字节写入 reg，第二个字节写入 reg+1，依此类推
|
||||
self.bus.write_i2c_block_data(self.addr, reg, data)
|
||||
except Exception as e:
|
||||
# I2C 写入失败时，抛出自定义异常
|
||||
raise EncoderMotorError(f"I2C write_block failed: {e}")
|
||||
|
||||
def _read_block(self, reg: int, length: int) -> bytes:
|
||||
"""
|
||||
从指定寄存器开始读取多个字节的数据。
|
||||
|
||||
参数:
|
||||
reg: 起始寄存器地址
|
||||
length: 要读取的字节数
|
||||
返回:
|
||||
bytes 类型的字节序列，包含从模块读取的原始数据
|
||||
"""
|
||||
try:
|
||||
# 通过 I2C 从模块地址 self.addr 的起始寄存器 reg 读取 length 个字节
|
||||
# read_i2c_block_data 返回一个整数列表，每个元素代表一个字节的值（0~255）
|
||||
data = self.bus.read_i2c_block_data(self.addr, reg, length)
|
||||
# 将整数列表转换为 bytes 对象（不可变的字节序列），方便后续用 struct 解析
|
||||
return bytes(data)
|
||||
except Exception as e:
|
||||
# I2C 读取失败时，抛出自定义异常
|
||||
raise EncoderMotorError(f"I2C read_block failed: {e}")
|
||||
|
||||
# =========================================================================
|
||||
# 上层 API 方法（命名和行为尽量与 Arduino 示例保持一致）
|
||||
# =========================================================================
|
||||
|
||||
def set_mode(self, index: int, mode: int) -> None:
|
||||
"""
|
||||
设置指定电机的控制模式。
|
||||
|
||||
参数:
|
||||
index: 电机索引（0~3，分别对应模块上的 4 个电机接口）
|
||||
mode: 工作模式，可以是 NORMAL_MODE(0x00)、POSITION_MODE(0x01) 等
|
||||
"""
|
||||
# 将电机索引限制在 0~3 范围内，防止越界
|
||||
index = _constrain_index(index)
|
||||
# 计算配置寄存器的地址：基地址 0x50 + 0x10 * 电机索引
|
||||
# 每个电机在配置块中占用 16 字节（0x10）的空间
|
||||
reg = MODULE_4ENCODERMOTOR_CONFIG_ADDR + (0x10 * index)
|
||||
# 通过 I2C 向计算出的配置寄存器地址写入模式值（1 字节）
|
||||
self._write_byte(reg, mode)
|
||||
|
||||
def get_encoder_value(self, index: int) -> int:
|
||||
"""
|
||||
读取指定电机的编码器数值。
|
||||
|
||||
编码器是安装在电机轴上的传感器，电机每转一圈，编码器会产生固定数量的脉冲。
|
||||
通过统计脉冲数，可以知道电机轴当前的位置（角度）。
|
||||
|
||||
参数:
|
||||
index: 电机索引（0~3）
|
||||
返回:
|
||||
编码器的 32 位有符号整数值，正值表示正转，负值表示反转
|
||||
"""
|
||||
# 将电机索引限制在 0~3 范围内
|
||||
index = _constrain_index(index)
|
||||
# 计算编码器寄存器地址：基地址 0x30 + 4 * 电机索引
|
||||
# 每个电机的编码器数值占用 4 字节（32 位整数），所以索引要乘以 4
|
||||
reg = MODULE_4ENCODERMOTOR_ENCODER_ADDR + 4 * index
|
||||
# 通过 I2C 从 reg 地址开始连续读取 4 个字节的原始数据
|
||||
data = self._read_block(reg, 4)
|
||||
# 使用 struct.unpack 将 4 字节解析为有符号 32 位整数
|
||||
# '>i' 的含义：
|
||||
# '>' 表示大端序（big-endian），即高位字节在前
|
||||
# 'i' 表示有符号 32 位整数（C 语言的 int32_t）
|
||||
# Arduino 代码中将字节按 (b0<<24) | (b1<<16) | (b2<<8) | b3 拼接，
|
||||
# 这正是大端序的组装方式，所以我们用大端序解析
|
||||
return struct.unpack('>i', data)[0]
|
||||
|
||||
def set_encoder_value(self, index: int, encoder: int) -> None:
|
||||
"""
|
||||
设置指定电机的编码器数值（写入/重置编码器计数值）。
|
||||
|
||||
通常用于将编码器归零（写入 0）或设置初始位置。
|
||||
|
||||
参数:
|
||||
index: 电机索引（0~3）
|
||||
encoder: 要写入的 32 位有符号整数值（编码器计数值）
|
||||
"""
|
||||
# 将电机索引限制在 0~3 范围内
|
||||
index = _constrain_index(index)
|
||||
# 计算编码器寄存器地址：基地址 0x30 + 4 * 电机索引
|
||||
reg = MODULE_4ENCODERMOTOR_ENCODER_ADDR + 4 * index
|
||||
# 将 32 位整数拆分为 4 个字节，按大端序排列（与 Arduino 一致）
|
||||
# 拆分方法：
|
||||
# 第 1 字节 = (encoder >> 24) & 0xFF 取最高 8 位
|
||||
# 第 2 字节 = (encoder >> 16) & 0xFF 取次高 8 位
|
||||
# 第 3 字节 = (encoder >> 8) & 0xFF 取次低 8 位
|
||||
# 第 4 字节 = encoder & 0xFF 取最低 8 位
|
||||
data = [
|
||||
(encoder >> 24) & 0xFF,
|
||||
(encoder >> 16) & 0xFF,
|
||||
(encoder >> 8) & 0xFF,
|
||||
encoder & 0xFF
|
||||
]
|
||||
# 通过 I2C 向编码器寄存器写入 4 个字节的数据
|
||||
self._write_block(reg, data)
|
||||
|
||||
def set_motor_speed(self, index: int, duty: int) -> None:
|
||||
"""
|
||||
设置指定电机的 PWM 占空比（即电机转速）。
|
||||
此方法适用于 NORMAL_MODE（普通模式），直接控制 PWM 输出。
|
||||
|
||||
PWM（脉冲宽度调制）：通过快速开关电源来调节电机的平均电压，
|
||||
占空比越高，电机转速越快。正值为正转，负值为反转。
|
||||
|
||||
参数:
|
||||
index: 电机索引（0~3）
|
||||
duty: 占空比值，取值范围 -128~127（有符号 8 位整数）
|
||||
正数表示正转，负数表示反转，数值绝对值越大转速越快
|
||||
"""
|
||||
# 将电机索引限制在 0~3 范围内
|
||||
index = _constrain_index(index)
|
||||
# 计算 PWM 占空比寄存器地址：基地址 0x20 + 电机索引
|
||||
# 每个电机占 1 个字节，所以直接加索引值即可
|
||||
reg = MODULE_4ENCODERMOTOR_PWM_DUTY_ADDR + index
|
||||
# 将 duty 与 0xFF 做按位与运算，只保留低 8 位
|
||||
# 注意：duty 是带符号的整数（如 -50 的补码表示为 0xCE），
|
||||
# 与 0xFF 后可以得到正确的二进制补码表示
|
||||
self._write_byte(reg, duty & 0xFF)
|
||||
|
||||
def get_motor_speed(self, index: int) -> int:
|
||||
"""
|
||||
读取指定电机的当前 PWM 占空比（转速设定值）。
|
||||
|
||||
参数:
|
||||
index: 电机索引（0~3）
|
||||
返回:
|
||||
有符号 8 位整数（-128~127），表示当前的 PWM 占空比设定值
|
||||
"""
|
||||
# 将电机索引限制在 0~3 范围内
|
||||
index = _constrain_index(index)
|
||||
# 计算 PWM 占空比寄存器地址：基地址 0x20 + 电机索引
|
||||
reg = MODULE_4ENCODERMOTOR_PWM_DUTY_ADDR + index
|
||||
# 通过 I2C 从寄存器读取 1 个字节的原始数据
|
||||
data = self._read_block(reg, 1)
|
||||
# 使用 struct.unpack 将单个字节解析为有符号 8 位整数
|
||||
# 'b' 表示有符号字符（signed char），即 -128~127 的范围
|
||||
# 这样 -50 这样的负值就能正确解析，而不是变成 206（无符号值）
|
||||
return struct.unpack('b', data)[0]
|
||||
|
||||
def get_motor_speed_20ms(self, index: int) -> int:
|
||||
"""
|
||||
读取指定电机每 20 毫秒的速度反馈值（编码器测速结果）。
|
||||
这个值代表电机在当前 20ms 时间窗口内的平均速度。
|
||||
|
||||
参数:
|
||||
index: 电机索引（0~3）
|
||||
返回:
|
||||
有符号 8 位整数（-128~127），正值正转，负值反转
|
||||
"""
|
||||
# 将电机索引限制在 0~3 范围内
|
||||
index = _constrain_index(index)
|
||||
# 计算速度反馈寄存器地址：基地址 0x40 + 电机索引
|
||||
# 每个电机占 1 个字节
|
||||
reg = MODULE_4ENCODERMOTOR_SPEED_ADDR + index
|
||||
# 通过 I2C 从寄存器读取 1 个字节的原始数据
|
||||
data = self._read_block(reg, 1)
|
||||
# 使用 struct.unpack 解析为有符号 8 位整数
|
||||
return struct.unpack('b', data)[0]
|
||||
|
||||
def set_position_pid(self, index: int, kp: int, ki: int, kd: int) -> None:
|
||||
"""
|
||||
设置指定电机位置模式下的 PID 参数。
|
||||
|
||||
PID（比例-积分-微分）控制是一种闭环控制算法：
|
||||
- Kp（比例系数）：根据当前误差的大小来调节输出，误差越大调节越强
|
||||
- Ki（积分系数）：根据误差的累积来调节输出，消除稳态误差
|
||||
- Kd（微分系数）：根据误差的变化趋势来调节输出，抑制超调
|
||||
|
||||
参数:
|
||||
index: 电机索引（0~3）
|
||||
kp: 比例系数（0~255）
|
||||
ki: 积分系数（0~255）
|
||||
kd: 微分系数（0~255）
|
||||
"""
|
||||
# 将电机索引限制在 0~3 范围内
|
||||
index = _constrain_index(index)
|
||||
# 计算 PID 参数在配置块中的起始地址：
|
||||
# 基地址 0x50 + 0x10 * 电机索引 + 0x01 偏移
|
||||
# 0x01 偏移表示 Kp 存储在配置块的第 2 个字节（第 1 个字节是模式设置）
|
||||
reg = MODULE_4ENCODERMOTOR_CONFIG_ADDR + index * 0x10 + 0x01
|
||||
# 通过 I2C 向起始寄存器写入 3 个字节：Kp、Ki、Kd 各占 1 字节
|
||||
# 每个参数都与 0xFF 取低 8 位，确保在 0~255 范围内
|
||||
self._write_block(reg, [kp & 0xFF, ki & 0xFF, kd & 0xFF])
|
||||
|
||||
def set_position_point(self, index: int, position_point: int) -> None:
|
||||
"""
|
||||
设置指定电机在位置模式下的目标位置点。
|
||||
电机将自动旋转到指定位置（使用 PID 闭环控制）。
|
||||
|
||||
参数:
|
||||
index: 电机索引（0~3）
|
||||
position_point: 目标位置值（32 位有符号整数），以编码器脉冲数为单位
|
||||
"""
|
||||
# 将电机索引限制在 0~3 范围内
|
||||
index = _constrain_index(index)
|
||||
# 计算位置点在配置块中的起始地址：
|
||||
# 基地址 0x50 + 0x10 * 电机索引 + 0x04 偏移
|
||||
# 0x04 偏移表示位置点存储在配置块的偏移 0x04 处
|
||||
reg = MODULE_4ENCODERMOTOR_CONFIG_ADDR + index * 0x10 + 0x04
|
||||
# 将 32 位整数拆分为 4 个字节，按小端序排列（先写最低有效字节 LSB）
|
||||
# 拆分方法（小端序）：
|
||||
# 第 1 字节 = position_point & 0xFF 取最低 8 位（先发送）
|
||||
# 第 2 字节 = (position_point >> 8) & 0xFF
|
||||
# 第 3 字节 = (position_point >> 16) & 0xFF
|
||||
# 第 4 字节 = (position_point >> 24) & 0xFF 取最高 8 位（最后发送）
|
||||
# Arduino 的 setPositionPoint 函数中先发送 LSB（最低有效字节），即小端序
|
||||
data = [
|
||||
position_point & 0xFF,
|
||||
(position_point >> 8) & 0xFF,
|
||||
(position_point >> 16) & 0xFF,
|
||||
(position_point >> 24) & 0xFF
|
||||
]
|
||||
# 通过 I2C 向位置点寄存器写入 4 个字节（小端序）
|
||||
self._write_block(reg, data)
|
||||
|
||||
def set_position_pid_max_speed(self, index: int, max_pwm: int) -> None:
|
||||
"""
|
||||
设置位置模式下 PID 控制的最大允许 PWM 占空比（即最大转速限制）。
|
||||
这样即使在位置 PID 计算出很大的输出时，也能限制电机不超过这个速度。
|
||||
|
||||
参数:
|
||||
index: 电机索引（0~3）
|
||||
max_pwm: 最大 PWM 占空比（0~255），数值越大允许的最大转速越高
|
||||
"""
|
||||
# 将电机索引限制在 0~3 范围内
|
||||
index = _constrain_index(index)
|
||||
# 计算最大速度寄存器地址：
|
||||
# 基地址 0x50 + 0x10 * 电机索引 + 0x08 偏移
|
||||
# 0x08 偏移对应 Arduino 示例中最大速度参数的存储位置
|
||||
reg = MODULE_4ENCODERMOTOR_CONFIG_ADDR + index * 0x10 + 0x08
|
||||
# 通过 I2C 向寄存器写入最大 PWM 值（1 字节），与 0xFF 确保在范围内
|
||||
self._write_byte(reg, max_pwm)
|
||||
|
||||
def set_speed_pid(self, index: int, kp: int, ki: int, kd: int) -> None:
|
||||
"""
|
||||
设置指定电机在速度模式下的 PID 参数。
|
||||
当使用 SPEED_MODE（速度模式）时，电机通过 PID 闭环控制来维持目标转速。
|
||||
|
||||
参数:
|
||||
index: 电机索引（0~3）
|
||||
kp: 比例系数（0~255）
|
||||
ki: 积分系数（0~255）
|
||||
kd: 微分系数（0~255）
|
||||
"""
|
||||
# 将电机索引限制在 0~3 范围内
|
||||
index = _constrain_index(index)
|
||||
# 计算速度 PID 参数在配置块中的起始地址：
|
||||
# 基地址 0x50 + 0x10 * 电机索引 + 0x09 偏移
|
||||
# 0x09 偏移是速度 PID 参数的存储起始位置（紧跟在位置 PID 最大速度之后）
|
||||
reg = MODULE_4ENCODERMOTOR_CONFIG_ADDR + index * 0x10 + 0x09
|
||||
# 通过 I2C 向起始寄存器写入 3 个字节：Kp、Ki、Kd 各占 1 字节
|
||||
self._write_block(reg, [kp & 0xFF, ki & 0xFF, kd & 0xFF])
|
||||
|
||||
def set_speed_point(self, index: int, speed_point: int) -> None:
|
||||
"""
|
||||
设置指定电机在速度模式下的目标速度值。
|
||||
电机将通过 PID 闭环控制来达到并保持这个速度。
|
||||
|
||||
参数:
|
||||
index: 电机索引（0~3）
|
||||
speed_point: 目标速度值（有符号 8 位整数，-128~127）
|
||||
"""
|
||||
# 将电机索引限制在 0~3 范围内
|
||||
index = _constrain_index(index)
|
||||
# 计算速度点在配置块中的地址：
|
||||
# 基地址 0x50 + 0x10 * 电机索引 + 0x0C 偏移
|
||||
# 0x0C 偏移对应速度目标值的存储位置（紧跟在速度 PID 参数之后）
|
||||
reg = MODULE_4ENCODERMOTOR_CONFIG_ADDR + index * 0x10 + 0x0C
|
||||
# 通过 I2C 向寄存器写入速度目标值（1 字节），与 0xFF 取低 8 位
|
||||
self._write_byte(reg, speed_point & 0xFF)
|
||||
|
||||
def set_linear_speed_m_s(self, index: int, speed_m_s: float) -> None:
|
||||
"""
|
||||
设置指定电机的线速度（米/秒），将线速度换算为电机转速并写入模块。
|
||||
适用于用电机驱动圆柱旋转的场景，已知圆柱的物理参数后可换算。
|
||||
|
||||
速度换算公式推导（核心难点）：
|
||||
speed = int(speed_m_s / (3.1415 * 0.01) * 9 * 16 * 20 / 50)
|
||||
|
||||
公式中各数字的物理含义：
|
||||
3.1415 → 圆周率 π（约等于 3.14159），用于计算圆柱周长
|
||||
0.01 → 圆柱半径 0.01 米（即 1 厘米），这是圆柱的物理半径
|
||||
-----------------------------------------------------------------
|
||||
3.1415 * 0.01 = 圆柱周长（米/圈）
|
||||
圆柱周长 = 2 * π * 半径 = 2 * 3.1415 * 0.01 = 0.06283 米
|
||||
⚠ 注意：代码中实际用的是 π * 半径 而不是 2 * π * 半径,
|
||||
这里需要结合实际的圆柱安装方式理解。如果圆柱是由电机通过摩擦
|
||||
轮或减速机构驱动的，有效半径可能已经包含了减半的因素。
|
||||
-----------------------------------------------------------------
|
||||
speed_m_s / (3.1415 * 0.01) = 每秒旋转的圈数（转/秒）
|
||||
将线速度除以每圈周长，得到目标转速（圈/秒）
|
||||
-----------------------------------------------------------------
|
||||
9 → 电机减速比（减速箱的减速比 9:1）
|
||||
电机内部有减速齿轮，输出轴每转 1 圈，电机转子转 9 圈
|
||||
-----------------------------------------------------------------
|
||||
16 → 编码器每圈的脉冲数（线数）
|
||||
编码器每转一圈产生 16 个脉冲，用于测量电机转子的位置
|
||||
-----------------------------------------------------------------
|
||||
9 * 16 = 144 → 电机输出轴每转 1 圈，编码器产生的脉冲数
|
||||
-----------------------------------------------------------------
|
||||
20 → 每 20 毫秒（即 0.02 秒）采样一次速度
|
||||
模块每 20ms 读取一次编码器脉冲数，计算速度反馈值
|
||||
-----------------------------------------------------------------
|
||||
50 → 速度寄存器每一单位对应的每秒脉冲数
|
||||
模块内部速度值 = 每 20ms 的脉冲数 / 50
|
||||
换句话说，速度寄存器中数值为 50 时，对应的 20ms 脉冲数为
|
||||
50 × 20ms 的脉冲数... 这个 50 是模块固件定义的比例因子
|
||||
-----------------------------------------------------------------
|
||||
完整计算流程：
|
||||
1. 线速度 → 每秒圈数：speed_m_s / (π * 半径)
|
||||
2. 每秒圈数 → 每秒电机转子圈数：× 减速比 9
|
||||
3. 每秒转子圈数 → 每秒编码器脉冲数：× 编码器线数 16
|
||||
4. 每秒脉冲数 → 每 20 毫秒脉冲数：÷ (1000/20) = ÷ 50
|
||||
5. 最终速度值 = 每秒脉冲数 / 50
|
||||
-----------------------------------------------------------------
|
||||
合并公式：
|
||||
speed = speed_m_s / (π * 0.01) × 9 × 16 / 50
|
||||
= speed_m_s / (3.1415 * 0.01) * 9 * 16 / 50
|
||||
|
||||
参数:
|
||||
index: 电机索引（0~3）
|
||||
speed_m_s: 目标线速度，单位米/秒（可以是小数，如 0.5 表示 0.5 米/秒）
|
||||
"""
|
||||
# 将线速度换算为模块速度寄存器所需的数值
|
||||
# speed_m_s / (3.1415 * 0.01) = 每秒圆柱转数（圈/秒）
|
||||
# × 9 × 16 = 每秒编码器脉冲数（考虑减速比和编码器线数）
|
||||
# / 50 = 每 20ms 脉冲数 / 50（模块内部速度单位换算）
|
||||
speed = int(speed_m_s / (3.1415 * 0.01) * 9 * 16 * 20 / 50)
|
||||
# 检查计算出的速度值是否在有符号 8 位整数的有效范围内（-127~127）
|
||||
# 超出范围说明设定的线速度太高或太低，无法用 1 字节表示
|
||||
if speed > 127 or speed < -127:
|
||||
# 如果速度超出范围，抛出异常提示调用方
|
||||
raise EncoderMotorError("Calculated speed out of range for set_linear_speed_m_s")
|
||||
else:
|
||||
# 如果速度在有效范围内，调用 set_speed_point 写入目标速度
|
||||
self.set_speed_point(index, speed)
|
||||
|
||||
def get_motor_current(self) -> float:
|
||||
"""
|
||||
读取模块的电机总电流（所有电机电流之和），单位安培（A）。
|
||||
可用于监控电机是否过载或堵转。
|
||||
|
||||
返回:
|
||||
浮点数，表示当前电机总电流，单位安培（A）
|
||||
"""
|
||||
# 通过 I2C 从电流寄存器 MODULE_4ENCODERMOTOR_CURRENT_ADDR (0x90)
|
||||
# 连续读取 4 个字节的原始数据
|
||||
data = self._read_block(MODULE_4ENCODERMOTOR_CURRENT_ADDR, 4)
|
||||
# 使用 struct.unpack 将 4 字节解析为单精度浮点数
|
||||
# '<f' 的含义：
|
||||
# '<' 表示小端序（little-endian），即低地址存放低字节
|
||||
# 'f' 表示单精度浮点数（C 语言的 float，4 字节）
|
||||
# Arduino 代码通过 memcpy 直接复制字节，在小端序处理器上得到的是小端序浮点数
|
||||
# 所以我们也用小端序解析
|
||||
return struct.unpack('<f', data)[0]
|
||||
|
||||
def get_analog_input(self) -> int:
|
||||
"""
|
||||
读取模块的 8 位模拟输入值（ADC 采样值）。
|
||||
可用于读取外接的模拟传感器（如电位器、光敏电阻等）。
|
||||
|
||||
返回:
|
||||
整数（0~255），代表 8 位 ADC 的采样值
|
||||
"""
|
||||
# 通过 I2C 从 8 位 ADC 寄存器 MODULE_4ENCODERMOTOR_ADC_8BIT_REG (0xA0)
|
||||
# 读取 1 个字节的数据
|
||||
data = self._read_block(MODULE_4ENCODERMOTOR_ADC_8BIT_REG, 1)
|
||||
# 返回读取到的第一个字节（也是唯一一个字节）作为 ADC 采样值
|
||||
return data[0]
|
||||
|
||||
def set_soft_start_and_stop(self, index: int, state: bool) -> None:
|
||||
"""
|
||||
启用或禁用指定电机的软启动/软停止功能。
|
||||
软启动：电机启动时缓慢加速，而不是突然全速，可以减少冲击。
|
||||
软停止：电机停止时缓慢减速，而不是突然刹车。
|
||||
|
||||
实现原理：模块用一个字节（8 位）的位图来记录 4 个电机的软启停状态，
|
||||
每个位（bit）对应一个电机。我们需要先读取当前字节，然后修改对应的位，
|
||||
再写回模块。
|
||||
|
||||
参数:
|
||||
index: 电机索引（0~3）
|
||||
state: True 表示启用软启停，False 表示禁用
|
||||
"""
|
||||
# 将电机索引限制在 0~3 范围内
|
||||
index = _constrain_index(index)
|
||||
# 第 1 步：通过 I2C 读取软启停寄存器的当前值（1 个字节）
|
||||
# 先读取是避免覆盖其他电机已设置的状态
|
||||
data = self._read_block(MODULE_4ENCODERMOTOR_SOFT_START_STOP_ADDR, 1)
|
||||
# 取出读取到的第 1 个字节（也是唯一字节）作为当前状态位图
|
||||
buf = data[0]
|
||||
# 先清除当前电机对应的位：将对应位设为 0，其他位保持不变
|
||||
# 1 << index：将 1 左移 index 位，得到只有第 index 位为 1 的数
|
||||
# 例如 index=2 时，1<<2 = 0b00000100
|
||||
# ~(1 << index)：按位取反，得到只有第 index 位为 0，其他位为 1 的数
|
||||
# buf = buf & ~(1 << index)：按位与运算，将第 index 位强制设为 0
|
||||
buf = buf & ~(1 << index)
|
||||
# 如果 state 为 True（启用软启停），则将对应位设为 1
|
||||
if state:
|
||||
# buf | (1 << index)：按位或运算，将第 index 位设为 1，其他位不变
|
||||
buf = buf | (1 << index)
|
||||
# 第 2 步：通过 I2C 将修改后的字节写回软启停寄存器
|
||||
self._write_byte(MODULE_4ENCODERMOTOR_SOFT_START_STOP_ADDR, buf)
|
||||
|
||||
def get_soft_start_and_stop(self, index: int) -> bool:
|
||||
"""
|
||||
查询指定电机的软启动/软停止功能是否已启用。
|
||||
|
||||
参数:
|
||||
index: 电机索引（0~3）
|
||||
返回:
|
||||
True 表示软启停已启用，False 表示未启用
|
||||
"""
|
||||
# 将电机索引限制在 0~3 范围内
|
||||
index = _constrain_index(index)
|
||||
# 通过 I2C 读取软启停寄存器的当前值（1 个字节）
|
||||
data = self._read_block(MODULE_4ENCODERMOTOR_SOFT_START_STOP_ADDR, 1)
|
||||
# 检查第 index 位是否为 1：
|
||||
# data[0] & (1 << index)：将字节与只有第 index 位为 1 的数做按位与
|
||||
# 如果结果非零，说明该位为 1（已启用），bool() 返回 True
|
||||
# 如果结果为零，说明该位为 0（未启用），bool() 返回 False
|
||||
return bool(data[0] & (1 << index))
|
||||
@@ -0,0 +1,369 @@
|
||||
"""
|
||||
Raspberry Pi 5 + M5Stack StepMotor Driver 模块驱动
|
||||
=====================================================
|
||||
|
||||
本模块用于驱动 M5Stack StepMotor Driver 模块，控制最多三个步进电机，
|
||||
驱动圆柱在水槽中直线移动。
|
||||
|
||||
本模块结合了两种控制方式：
|
||||
1. I2C 通信（地址 0x27）：用于控制模块的使能/复位/配置
|
||||
2. 硬件 PWM（GPIO12）：用于产生步进脉冲，通过调整脉冲频率控制电机速度
|
||||
|
||||
方向控制通过独立的 GPIO 引脚（5/6/16）实现，三个电机共享同一路 PWM
|
||||
但方向可以独立控制。
|
||||
|
||||
PWM 概念简介：
|
||||
PWM（Pulse Width Modulation，脉冲宽度调制）是一种通过快速开关信号来
|
||||
模拟不同电压输出的技术。在步进电机控制中，PWM 的频率决定电机转速——
|
||||
频率越高，脉冲越快，电机转动越快。
|
||||
|
||||
参考文档：
|
||||
https://docs.m5stack.com/en/module/Stepmotor%20Driver%20Module13.2%20v1.1
|
||||
|
||||
依赖库：smbus2（I2C 通信库）
|
||||
|
||||
使用示例：
|
||||
from drv_stepmotor import StepMotorDriver
|
||||
with StepMotorDriver() as driver:
|
||||
driver.set_speeds([0.02, -0.015, 0.01])
|
||||
time.sleep(2)
|
||||
|
||||
"""
|
||||
from __future__ import annotations # 启用 Python 未来的注解特性，允许在类型提示中使用字符串形式的类名（前向引用）
|
||||
|
||||
import math # 导入 math 模块，用于数学计算（如余弦函数 cos() 用于 S 曲线加减速、圆周率 pi）
|
||||
import threading # 导入 threading 模块，用于多线程操作（本模块预留，当前未直接使用）
|
||||
import time # 导入 time 模块，用于延时和计时（如加减速过程中的 sleep 暂停）
|
||||
from typing import List, Optional # 从 typing 模块导入 List（列表类型）和 Optional（可选类型）用于参数类型注解
|
||||
|
||||
from gpiozero import DigitalOutputDevice # 从 gpiozero 库导入 DigitalOutputDevice 类，用于控制树莓派 GPIO 引脚的电平输出（高/低）
|
||||
from smbus2 import SMBus # 从 smbus2 库导入 SMBus 类，用于通过 I2C 总线与 M5Stack 步进电机模块通信
|
||||
from rpi_hardware_pwm import HardwarePWM # 从 rpi_hardware_pwm 库导入 HardwarePWM 类，用于控制树莓派硬件 PWM 发生器
|
||||
|
||||
# ==================== I2C 寄存器地址常量 ====================
|
||||
# 以下常量对应 M5Stack StepMotor Driver 模块内部寄存器的 I2C 地址，
|
||||
# 参考自官方 C++ 实现文件 ref_stepmotor.cpp
|
||||
MODULE_STEPMOTOR_ADDR = 0x27 # 模块的 I2C 从设备地址（7 位地址 0x27），所有 I2C 通信都发往此地址
|
||||
STEPMOTOR_REG_IO_CFG = 0x03 # IO 配置寄存器地址，用于配置模块引脚的输入/输出功能
|
||||
STEPMOTOR_REG_MICROSTEP = 0x01 # 微步进模式配置寄存器地址，设置电机的微步细分数
|
||||
STEPMOTOR_REG_ENABLE = 0x01 # 使能控制寄存器地址（与微步进共用 0x01，通过不同位控制不同功能）
|
||||
STEPMOTOR_REG_EXTIO = 0x00 # 外部 IO 寄存器地址，用于扩展输入输出
|
||||
STEPMOTOR_REG_FAULT = 0x04 # 故障状态寄存器地址，读取过流/过热等错误状态
|
||||
STEPMOTOR_REG_RESET = 0x05 # 复位寄存器地址，用于单独复位各个电机通道
|
||||
|
||||
|
||||
class StepMotorError(RuntimeError):
|
||||
"""自定义异常类，继承自 RuntimeError，用于步进电机驱动相关的错误处理"""
|
||||
pass # 类体为空，仅作为自定义异常类型的标识，不需要额外实现
|
||||
|
||||
|
||||
class StepMotorDriver:
|
||||
"""步进电机驱动器类，最多控制 3 个步进电机。
|
||||
|
||||
本类封装了 I2C 通信和硬件 PWM 的全部控制逻辑，提供启动、停止、调速、
|
||||
方向控制等完整功能。三个步进电机共享同一路硬件 PWM（产生步进脉冲），
|
||||
但每个电机的方向由独立的 GPIO 引脚控制。
|
||||
|
||||
重要设计说明：
|
||||
- 三个电机共享同一路 PWM：因为所有电机同时以相同频率步进，仅方向可以不同
|
||||
- I2C 只用于模块的使能/复位配置，不用于产生步进脉冲
|
||||
- 步进脉冲由树莓派硬件 PWM 直接产生，不受 CPU 负载影响
|
||||
|
||||
构造函数参数说明：
|
||||
- pwm_pins: 用于产生步进脉冲的 GPIO 引脚编号（BCM 编号），默认为 GPIO12
|
||||
- dir_pins: 用于控制方向信号的 GPIO 引脚编号列表，最多 3 个，默认 [5, 6, 16]
|
||||
- steps_per_rev: 电机转一圈所需的步数（微步进），默认 200×32×14 = 89600
|
||||
- mm_per_rev: 电机转一圈对应的直线移动距离（毫米），默认 π×20 ≈ 62.83 mm
|
||||
- ramp_time: 加减速的斜坡时间（秒），默认 20 秒
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
pwm_pins: int = None, # PWM 引脚编号（BCM 编号），默认为 12（即 GPIO12）
|
||||
dir_pins: Optional[List[int]] = None, # 方向控制 GPIO 引脚编号列表，默认为 [5, 6, 16]
|
||||
steps_per_rev: int = 200*32*14, # 电机转一圈的总步数（微步进后）
|
||||
mm_per_rev: float = math.pi*20, # 电机转一圈对应的直线移动距离（毫米）
|
||||
ramp_time: float = 20, # 加减速斜坡时间（秒）
|
||||
) -> None:
|
||||
# 保存所有配置参数为实例变量，供其他方法访问
|
||||
self.pwm_pins = pwm_pins or 12 # 如果未传入 PWM 引脚编号，默认使用 GPIO12
|
||||
self.dir_pins = dir_pins or [5, 6, 16] # 如果未传入方向引脚列表，默认使用 GPIO5、GPIO6、GPIO16
|
||||
self.steps_per_rev = steps_per_rev # 每转总步数 = 200（电机物理步数）× 32（微步数）× 14（减速比）
|
||||
self.mm_per_rev = mm_per_rev # 每转移动距离 = π × 20（驱动轮直径，单位 mm）
|
||||
self.max_speed = 0.1 # 最大速度限制（米/秒），防止电机速度过快导致失步或机械冲击
|
||||
self.ramp_time = float(ramp_time) # 加减速斜坡时间（秒），强制转为 float 类型以确保精度
|
||||
self.i2c_addr = MODULE_STEPMOTOR_ADDR # I2C 设备地址，使用预定义的常量 0x27
|
||||
self.i2c_bus = SMBus(1) # 创建 I2C 总线对象，使用 I2C 总线 1（树莓派 5 上 I2C-1 对应引脚 GPIO2/SDA、GPIO3/SCL）
|
||||
|
||||
self.default_dir = [True, True, False] # 三个电机的默认方向：电机 0 和电机 1 正向，电机 2 反向
|
||||
self._pwm = HardwarePWM(pwm_channel=0, hz=1, chip=0) # 创建硬件 PWM 对象：通道 0，初始 1Hz，PWM 芯片 0
|
||||
self._dir_pins = [DigitalOutputDevice(d, initial_value=False) for d in self.dir_pins] # 为每个方向引脚创建 GPIO 输出对象，初始值为低电平
|
||||
self.current_speed = 0.0 # 当前速度（米/秒），初始化为 0 表示电机处于停止状态
|
||||
|
||||
self.set_dir(True) # 设置三个电机的初始方向为正向
|
||||
|
||||
|
||||
def _write_byte(self, reg: int, value: int) -> None:
|
||||
"""向模块的指定 I2C 寄存器写入一个字节的数据。
|
||||
|
||||
这是一个内部方法（以下划线开头），封装了 I2C 寄存器写入操作。
|
||||
|
||||
参数:
|
||||
reg: 寄存器地址（如 0x01 使能寄存器、0x05 复位寄存器等）
|
||||
value: 要写入的 8 位数据值（0~255），写入前会自动截断为低 8 位
|
||||
|
||||
异常:
|
||||
StepMotorError: 如果 I2C 通信失败则抛出此异常
|
||||
"""
|
||||
value &= 0xFF # 截断高位：只保留低 8 位（0~255），确保写入一个字节
|
||||
try:
|
||||
self.i2c_bus.write_byte_data(self.i2c_addr, reg, value) # 通过 I2C 总线向指定寄存器的地址写入数据
|
||||
except Exception as e:
|
||||
raise StepMotorError(f"I2C write_byte failed: {e}") # 如果 I2C 操作异常，包装为自定义异常向上抛出
|
||||
|
||||
def _read_byte(self, reg: int) -> int:
|
||||
"""读取模块指定 I2C 寄存器的一个字节数据。
|
||||
|
||||
这是一个内部方法（以下划线开头），封装了 I2C 寄存器读取操作。
|
||||
|
||||
参数:
|
||||
reg: 寄存器地址
|
||||
|
||||
返回:
|
||||
int: 读取到的 8 位数据值（0~255）
|
||||
|
||||
异常:
|
||||
StepMotorError: 如果 I2C 通信失败则抛出此异常
|
||||
"""
|
||||
try:
|
||||
self.i2c_bus.write_byte(self.i2c_addr, reg) # 先向 I2C 设备发送要读取的寄存器地址（写入操作选择寄存器）
|
||||
data = self.i2c_bus.read_byte(self.i2c_addr) # 再从设备读取该寄存器的当前值（读取操作获取数据）
|
||||
return data # 返回读取到的数据值
|
||||
except Exception as e:
|
||||
raise StepMotorError(f"I2C read_byte failed: {e}") # 如果 I2C 操作异常，包装为自定义异常向上抛出
|
||||
|
||||
def enable_motor(self):
|
||||
"""使能电机：允许电机驱动芯片接收脉冲信号，电机可以正常转动。
|
||||
|
||||
通过修改使能寄存器（地址 0x01）的第 4 位（bit 4）来实现：
|
||||
bit 4 = 0 表示使能，bit 4 = 1 表示禁止。
|
||||
操作流程：先读取寄存器的当前值 → 修改特定位 → 将新值写回寄存器。
|
||||
这种"读-改-写"模式确保不影响寄存器中其他位的设置。
|
||||
"""
|
||||
reg_data = self._read_byte(STEPMOTOR_REG_ENABLE) # 读取使能寄存器（地址 0x01）的当前值
|
||||
reg_data &= 0xEF # 清除第 4 位（bit 4 置 0）：0xEF = 0b11101111，bit 4 = 0 表示使能电机
|
||||
self._write_byte(STEPMOTOR_REG_ENABLE, reg_data) # 将修改后的值写回使能寄存器（地址 0x01）
|
||||
|
||||
def disable_motor(self):
|
||||
"""禁止电机：阻止电机驱动芯片接收脉冲信号，电机停止转动。
|
||||
|
||||
通过修改使能寄存器（地址 0x01）的第 4 位（bit 4）来实现：
|
||||
将其他位清零、仅保留 bit 4 的值。
|
||||
注意：原代码使用 reg_data &= 0x10 仅保留 bit 4 并清除其他位，
|
||||
这与标准的"将 bit 4 置 1"（reg_data |= 0x10）有所不同，
|
||||
但为保持逻辑不变，此处不修改代码。
|
||||
"""
|
||||
reg_data = self._read_byte(STEPMOTOR_REG_ENABLE) # 读取使能寄存器（地址 0x01）的当前值
|
||||
reg_data &= 0x10 # 将除第 4 位之外的所有位清零：0x10 = 0b00010000，仅保留 bit 4
|
||||
self._write_byte(STEPMOTOR_REG_ENABLE, reg_data) # 将修改后的值写回使能寄存器（地址 0x01）
|
||||
|
||||
def reset_motor(self, resmtr: int, enable: bool):
|
||||
"""复位或释放指定的电机通道。
|
||||
|
||||
通过复位寄存器（地址 0x05）的低 3 位分别控制三个电机：
|
||||
bit 0 控制电机 0，bit 1 控制电机 1，bit 2 控制电机 2。
|
||||
将某位置 1 表示复位该电机，置 0 表示释放。
|
||||
|
||||
参数:
|
||||
resmtr: 电机索引（0、1、2），分别对应三个步进电机通道
|
||||
enable: True 表示复位（将对应位设为 1），False 表示释放（将对应位设为 0）
|
||||
"""
|
||||
reg_data = self._read_byte(STEPMOTOR_REG_RESET) # 读取复位寄存器（地址 0x05）的当前值
|
||||
reg_data &= 0x07 # 仅保留低 3 位（bit 0~2），分别对应三个电机的复位状态，高位清零
|
||||
if not enable:
|
||||
reg_data &= ~(0x01 << resmtr) # 如果 enable=False，将对应电机的位清零：~(1<<n) 将第 n 位取反后做 AND
|
||||
else:
|
||||
reg_data |= (0x01 << resmtr) # 如果 enable=True，将对应电机的位置 1：(1<<n) 左移 n 位得到只有第 n 位为 1 的掩码
|
||||
self._write_byte(STEPMOTOR_REG_RESET, reg_data) # 将修改后的值写回复位寄存器（地址 0x05）
|
||||
|
||||
def close(self) -> None:
|
||||
"""关闭驱动器：停止所有电机、释放硬件资源。
|
||||
|
||||
按顺序执行：紧急停止 → I2C 禁能 → 所有电机通道释放 → 清理 GPIO/PWM/I2C 对象。
|
||||
使用 try/except 确保即使某个清理步骤出错，后续步骤也能继续执行。
|
||||
"""
|
||||
self.emergency_stop() # 紧急停止：立即切断 PWM 输出，电机瞬间失去脉冲
|
||||
self.disable_motor() # 通过 I2C 设置使能寄存器，禁止电机驱动芯片
|
||||
for idx in range(3): # 循环三个电机通道（索引 0、1、2）
|
||||
self.reset_motor(idx, False) # 释放每个电机通道：将复位寄存器中对应位清 0
|
||||
# 关闭 gpiozero 相关的 GPIO 设备
|
||||
try:
|
||||
self._pwm.close() # 关闭硬件 PWM 对象，释放 PWM 通道资源
|
||||
self.i2c_bus.close() # 关闭 I2C 总线连接，释放总线资源
|
||||
for d in self._dir_pins: # 遍历所有方向控制引脚对象
|
||||
d.off() # 将方向 GPIO 引脚置为低电平（关闭输出）
|
||||
d.close() # 关闭方向 GPIO 引脚对象，释放资源
|
||||
except Exception:
|
||||
pass # 忽略清理过程中的任何异常，确保程序能正常退出
|
||||
|
||||
def set_dir(self, dir: bool = True) -> None:
|
||||
"""设置三个电机的整体方向。
|
||||
|
||||
注意：三个电机只能同时设置为相同的方向（全部正向或全部反向）。
|
||||
每个电机的实际方向由 default_dir（电机默认方向）和传入的 dir 参数
|
||||
通过异或（XOR）运算决定，这样可以灵活设置每个电机的正反。
|
||||
|
||||
参数:
|
||||
dir: True 表示正向，False 表示反向
|
||||
电机 n 的实际方向 = default_dir[n] XOR dir
|
||||
|
||||
异常:
|
||||
StepMotorError: 如果电机正在运行（current_speed ≠ 0）则不能改变方向
|
||||
"""
|
||||
if self.current_speed == 0.0: # 检查电机是否处于停止状态，运行时禁止改变方向
|
||||
for idx, dir_dev in enumerate(self._dir_pins): # 遍历三个方向控制引脚（索引 0、1、2）
|
||||
boolean_dir = self.default_dir[idx] ^ dir # 计算实际方向：默认方向 XOR 传入方向（异或运算）
|
||||
if boolean_dir:
|
||||
dir_dev.on() # 如果计算结果为 True，将 GPIO 引脚置为高电平（电机正转）
|
||||
else:
|
||||
dir_dev.off() # 如果计算结果为 False，将 GPIO 引脚置为低电平（电机反转）
|
||||
else:
|
||||
raise StepMotorError("Cannot change direction while motor is running") # 电机运行时改变方向会损坏驱动器
|
||||
|
||||
def start(self, speed_m_s: float) -> None:
|
||||
"""启动电机，并使用 S 曲线加速到目标速度。
|
||||
|
||||
S 曲线加速原理：
|
||||
使用余弦函数产生平滑的"S"形加速曲线，速度变化率（即加速度）
|
||||
在启动和结束时较小，中间较大，从而减少机械冲击和电机失步风险。
|
||||
|
||||
S 曲线加速公式：
|
||||
s = (cos(π × (1 - i/100)) + 1) / 2 × speed_m_s
|
||||
其中 i 从 0 到 99 逐步增加：
|
||||
- i=0 时：cos(π×(1-0)) = cos(π) = -1，s = (-1+1)/2 × speed = 0 × speed = 0（起始速度为 0）
|
||||
- i=50 时：cos(π×(1-0.5)) = cos(π/2) = 0，s = (0+1)/2 × speed = 0.5 × speed（中间速度为目标一半）
|
||||
- i=99 时：cos(π×(1-0.99)) = cos(0.01π) ≈ 1，s = (1+1)/2 × speed ≈ speed（接近目标速度）
|
||||
整个过程中，加速度先增大后减小，形成"S"形曲线，相比线性加速更加平顺。
|
||||
|
||||
频率换算公式：
|
||||
freq = s × steps_per_rev / (mm_per_rev / 1000)
|
||||
推导过程：
|
||||
- freq(Hz) = 每秒步数（脉冲数）
|
||||
- 每秒步数 = 速度(m/s) × 步数/米
|
||||
- 步数/米 = 步数/转 ÷ 米/转 = steps_per_rev ÷ (mm_per_rev / 1000)
|
||||
- steps_per_rev = 200（电机物理步数，1.8°/步）× 32（驱动器微步细分数）× 14（行星减速器减速比）
|
||||
- mm_per_rev = π × 20（驱动轮直径 20mm）= 62.83 mm/转
|
||||
|
||||
参数:
|
||||
speed_m_s: 目标速度（米/秒），必须为正值
|
||||
|
||||
异常:
|
||||
StepMotorError: 如果电机已经在运行中，则不能重复启动
|
||||
"""
|
||||
if self.current_speed == 0.0: # 仅在电机当前处于停止状态时才能启动
|
||||
self._pwm.change_frequency(1) # 先将 PWM 频率设为最低的 1Hz，避免从 0Hz 直接跳变
|
||||
self._pwm.start(100) # 启动硬件 PWM 输出，占空比设为 100%（模块内部处理步进脉冲的生成）
|
||||
for i in range(100): # 将加速过程分为 100 个步进点，从 i=0 到 i=99 逐步加速
|
||||
# S 曲线加速公式：余弦函数值从 cos(π) = -1 逐渐变化到 cos(0.01π) ≈ 1
|
||||
# 经 (cos + 1) / 2 映射后，系数从 0 逐渐变化到接近 1
|
||||
s = (math.cos(math.pi * (1-i/100)) + 1) / 2 * speed_m_s # 计算当前步进点的瞬时目标速度（米/秒）
|
||||
# 将速度（米/秒）换算为 PWM 频率（Hz）：
|
||||
# freq = s × steps_per_rev / (mm_per_rev / 1000)
|
||||
# 除以 1000 因为 mm_per_rev 是毫米，需要转换为米
|
||||
freq = max(s * self.steps_per_rev / (self.mm_per_rev / 1000), 1) # 换算频率，max(...,1) 确保频率不低于 1Hz
|
||||
self._pwm.change_frequency(freq) # 更新硬件 PWM 的频率，使电机以对应速度转动
|
||||
time.sleep(self.ramp_time / 100) # 等待 ramp_time/100 秒，控制每步加速的持续时间
|
||||
self.current_speed = speed_m_s # 加速完成后，将当前速度记录为目标速度
|
||||
else:
|
||||
raise StepMotorError("Motor is already running") # 如果电机已经运行，拒绝重复启动（必须先 stop）
|
||||
|
||||
def stop(self) -> None:
|
||||
"""停止电机，使用 S 曲线减速到零。
|
||||
|
||||
S 曲线减速公式：
|
||||
s = (cos(π × i/100) + 1) / 2 × current_speed
|
||||
其中 i 从 0 到 99 逐步增加：
|
||||
- i=0 时：cos(0) = 1，s = (1+1)/2 × speed = 1 × speed（保持当前速度开始减速）
|
||||
- i=50 时：cos(π/2) = 0，s = (0+1)/2 × speed = 0.5 × speed（速度降到一半）
|
||||
- i=99 时：cos(0.99π) ≈ -1，s = (-1+1)/2 × speed = 0 × speed = 0（减速到零）
|
||||
|
||||
与 start() 中的加速曲线对称，减速曲线也是 S 形，确保停止过程平滑。
|
||||
减速时间比加速时间短（使用 ramp_time/4 而不是 ramp_time/100），
|
||||
因为减速到零后电机即停止，不需要像加速那样缓慢进入。
|
||||
"""
|
||||
if self.current_speed != 0.0: # 只有在电机正在运行时才能执行停止操作
|
||||
for i in range(100): # 将减速过程分为 100 个步进点，从 i=0 到 i=99 逐步减速
|
||||
# S 曲线减速公式：余弦值从 cos(0) = 1 逐渐变化到 cos(0.99π) ≈ -1
|
||||
# 经 (cos + 1) / 2 映射后，系数从 1 逐渐下降到接近 0
|
||||
s = (math.cos(math.pi * i/100) + 1) / 2 * self.current_speed # 计算当前步进点的瞬时速度（米/秒）
|
||||
freq = max(s * self.steps_per_rev / (self.mm_per_rev / 1000), 1) # 将速度换算为 PWM 频率，最小 1Hz
|
||||
self._pwm.change_frequency(freq) # 更新硬件 PWM 频率，降低电机转速
|
||||
time.sleep(self.ramp_time / 4 / 100) # 等待 ramp_time/400 秒，比加速步进快 4 倍
|
||||
self._pwm.stop() # 减速完成后，完全停止硬件 PWM 输出
|
||||
self._pwm.change_frequency(1) # 将 PWM 频率复位到 1Hz 默认值，为下一次启动做准备
|
||||
self.current_speed = 0.0 # 将当前速度设为 0，标记电机已停止
|
||||
else:
|
||||
raise StepMotorError("Motor is already stopped") # 如果电机已经停止，拒绝重复执行停止操作
|
||||
|
||||
def change_speed(self, speed_m_s: float) -> None:
|
||||
"""在电机运行过程中，平滑地将速度切换到新目标值。
|
||||
|
||||
速度切换 S 曲线公式：
|
||||
s = (cos(π × i/100) + 1) / 2 × (current_speed - speed_m_s) + speed_m_s
|
||||
公式推导：
|
||||
将减速公式推广到任意目标速度：
|
||||
- 令 delta = (current_speed - speed_m_s)，即当前速度与目标速度的差值
|
||||
- 减速公式变为：s = 余弦因子 × delta + speed_m_s
|
||||
- i=0 时：因子=1，s = 1×delta + speed_m_s = current_speed（保持当前速度）
|
||||
- i=99 时：因子≈0，s = 0×delta + speed_m_s = speed_m_s（达到目标速度）
|
||||
传入的速度可以是加速（目标 > 当前）或减速（目标 < 当前），
|
||||
S 曲线都能平滑过渡。
|
||||
|
||||
参数:
|
||||
speed_m_s: 新的目标速度（米/秒），可高于或低于当前速度
|
||||
|
||||
异常:
|
||||
StepMotorError: 如果电机未运行，调用此方法会抛出异常，提示使用 start()
|
||||
"""
|
||||
if self.current_speed != 0.0: # 只有在电机正在运行时才能切换速度
|
||||
for i in range(100): # 分 100 步进行速度过渡，从 i=0 到 i=99
|
||||
# 速度切换公式：当前的 S 曲线因子从 1 过渡到 0，
|
||||
# 乘以速度差后加到目标速度上，实现从当前速度到目标速度的平滑过渡
|
||||
s = (math.cos(math.pi * i/100) + 1) / 2 * (self.current_speed - speed_m_s) + speed_m_s
|
||||
freq = max(s * self.steps_per_rev / (self.mm_per_rev / 1000), 1) # 将速度换算为 PWM 频率，最小 1Hz
|
||||
self._pwm.change_frequency(freq) # 更新硬件 PWM 频率，改变电机转速
|
||||
time.sleep(self.ramp_time / 4 / 100) # 等待 ramp_time/400 秒，控制每步过渡的持续时间
|
||||
self.current_speed = speed_m_s # 速度过渡完成后，将当前速度更新为新目标速度
|
||||
else:
|
||||
raise StepMotorError("Motor is not running. Use start() to start the motor.") # 电机停止时不能用 change_speed
|
||||
|
||||
def emergency_stop(self) -> None:
|
||||
"""紧急停止：立即切断 PWM 输出，不经过 S 曲线减速。
|
||||
|
||||
与 stop() 方法的区别：
|
||||
- stop()：使用 S 曲线平滑减速（约需 ramp_time/4 秒）
|
||||
- emergency_stop()：直接切断 PWM 输出，无减速过程（立即停止）
|
||||
|
||||
适用于紧急情况下的快速停机，但会产生较大的机械冲击。
|
||||
"""
|
||||
self._pwm.stop() # 立即停止硬件 PWM 输出，电机瞬间失去步进脉冲
|
||||
self._pwm.change_frequency(1) # 将 PWM 频率复位到 1Hz 默认值，为下一次启动做准备
|
||||
self.current_speed = 0.0 # 将当前速度设为 0，标记电机已停止
|
||||
|
||||
def __enter__(self):
|
||||
"""进入上下文管理器：当使用 'with StepMotorDriver() as driver:' 语法时自动调用
|
||||
|
||||
返回:
|
||||
StepMotorDriver: 返回自身实例，供 with 语句块中的变量引用
|
||||
"""
|
||||
return self # 返回当前 StepMotorDriver 实例
|
||||
|
||||
def __exit__(self, exc_type, exc, tb):
|
||||
"""退出上下文管理器：当 with 语句块执行完毕时自动调用
|
||||
|
||||
参数:
|
||||
exc_type: 异常类型（如果语句块中发生了异常）
|
||||
exc: 异常对象
|
||||
tb: 异常回溯信息
|
||||
"""
|
||||
self.close() # 自动调用 close() 清理所有资源，确保即使发生异常也能释放硬件
|
||||
@@ -0,0 +1,28 @@
|
||||
# ==================================================
|
||||
# 圆柱水槽运动控制系统 — Python 依赖列表
|
||||
# 安装命令：pip install -r requirements.txt
|
||||
# ==================================================
|
||||
|
||||
# SPI 通信库
|
||||
# 用于树莓派通过 SPI 总线与 ADS124S08 ADC 芯片通信
|
||||
# 实现 24 位高精度模数转换数据的读取
|
||||
spidev
|
||||
|
||||
# I2C 通信库
|
||||
# 用于树莓派通过 I2C 总线与以下模块通信：
|
||||
# - M5Stack 4EncoderMotor 模块（地址 0x24）
|
||||
# - M5Stack StepMotor Driver 模块（地址 0x27）
|
||||
smbus2
|
||||
|
||||
# 树莓派硬件 PWM 控制库
|
||||
# 用于通过树莓派硬件 PWM 发生器（GPIO12）产生步进脉冲
|
||||
# 驱动步进电机按指定速度运动
|
||||
rpi_hardware_pwm
|
||||
|
||||
# Jupyter Notebook 环境
|
||||
# 用于交互式运行 test.ipynb 教学演示
|
||||
jupyter
|
||||
|
||||
# 数据可视化库（可选）
|
||||
# test.ipynb 中的绘图功能依赖此库
|
||||
matplotlib
|
||||
Reference in New Issue
Block a user