""" 树莓派 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 表明是小端序浮点排列，因此我们用 ' 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 字节解析为单精度浮点数 # ' 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))