674 lines
34 KiB
Python
674 lines
34 KiB
Python
"""
|
||
树莓派 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))
|