Frank14f/pinball_exp_rp5
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

上传文件至 /

Browse Source
This commit is contained in:
Frank14f 2026-06-11 19:37:42 +08:00
commit 4de19b217e
5 changed files with 2747 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

671
README.md Normal file
View File

@ -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
drv_adc.py Normal file
View File

File diff suppressed because it is too large Load Diff

673
drv_encodermotor.py Normal file
View File

@ -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))

369
drv_stepmotor.py Normal file
View File

@ -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() 清理所有资源，确保即使发生异常也能释放硬件

28
requirements.txt Normal file
View File

@ -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