diff --git a/pybot/.gitignore b/pybot/.gitignore new file mode 100644 index 0000000..f487a21 --- /dev/null +++ b/pybot/.gitignore @@ -0,0 +1,5 @@ +__pycache__/ +*.pyc +venv/ +.pytest_cache/ +/tmp*.mt diff --git a/pybot/README.md b/pybot/README.md new file mode 100644 index 0000000..985b8f2 --- /dev/null +++ b/pybot/README.md @@ -0,0 +1,57 @@ +# pybot + +机器人 `.mt` 动作脚本解析器 —— [bot4cj](../bot4cj/)（仓颉版）的 Python 迁移版。 + +驱动大顺（Dashun）蜘蛛机器人（树莓派 aarch64，10 舵机 + 2 云台， +PCA9685 PWM 控制器 + MPU6050 姿态传感器）。 + +迁移方案与审计修复映射见 +[docs/reports/bot4cj_python_migration_dev_doc.md](../docs/reports/bot4cj_python_migration_dev_doc.md)。 + +## 特性 + +- **零第三方依赖**：纯标准库（fcntl / struct / threading / wave / argparse）， + 树莓派上 `git pull` 即部署，无构建步骤。 +- **与仓颉版逐语义对齐**：FSM 解析器、占位符递归、Servo 校准算法、 + PCA9685 寄存器序列均与 bot4cj 一致（Mock 总线 journal 可逐字节 diff）。 +- **Python 插件替代 Lua**：`modules/<块名>.py` 定义 + `build(ctx) / call(ctx) / destroy(ctx)`，消除 JSON 深拷贝 + 五层 FFI 桥接。 +- **审计修复**：bot4cj 审计 15 项发现（S01~S08 / Q01~Q07）全部修复， + 详见迁移文档 §5。 + +## 用法 + +```bash +cat scripts/std.mt | ./_run.sh # 与 bot4cj 相同的管道用法 +./_run.sh scripts/std.mt # 位置参数（增强） +./_run.sh --dry-run scripts/std.mt # Mock 总线，动作序列到 stdout +./_run.sh --test roundtrip in.mt out.mt # 解析↔序列化往返自检 +./_run.sh --verbose scripts/std.mt # DEBUG 日志（stderr） +``` + +`-i` / `-o` 参数位保留（reserved），未来对接 [actpred](../actpred/) 动作预测。 + +## 目录 + +``` +pybot/ # 包：parser / robot / hardvars / plugin / main +pybot/hd/ # 硬件层：i2c / spi / servo / kalman / mpu6050 / speaker +modules/ # 硬件驱动插件：PCA9685 / SPI / ADS1115 / ADS1256 +scripts -> ../bot4cj/scripts # .mt 脚本与仓颉版共享 +tests/ # pytest 单元测试（Mock 总线，无需硬件） +``` + +插件查找顺序：`$PYBOT_MODULES_PATH`（冒号分隔）→ 包内置 `modules/` → `./modules/`。 + +## 开发 + +```bash +./_set.sh # venv + pytest 全量测试 +venv/bin/python -m pytest tests/ -v +``` + +## .mt 脚本格式 + +见 [bot4cj/README.md](../bot4cj/README.md) 与 `scripts/std.mt`。 +格式要点：`@块名` 开块、行 0 为表头、`;` 结行、`#` 注释、 +`-` 占位符（非首行取上一行同列值；首行数值列解析为 0）、`@` 收块。 diff --git a/pybot/_run.sh b/pybot/_run.sh new file mode 100755 index 0000000..39aa920 --- /dev/null +++ b/pybot/_run.sh @@ -0,0 +1,5 @@ +#!/bin/bash +# 与 bot4cj 用法一致：cat scripts/std.mt | ./_run.sh +# 零第三方依赖（纯标准库），直接用系统 python3，无需 venv +cd "$(dirname "$0")" || exit +exec python3 -m pybot "$@" diff --git a/pybot/_set.sh b/pybot/_set.sh new file mode 100755 index 0000000..ca40a60 --- /dev/null +++ b/pybot/_set.sh @@ -0,0 +1,10 @@ +#!/bin/bash +cd "$(dirname "$0")" || exit + +# 运行时零第三方依赖（纯标准库）；venv 仅为开发测试（pytest） +python3 -m venv venv +source venv/bin/activate +pip install -r requirements.txt -i https://pypi.tuna.tsinghua.edu.cn/simple + +# 运行测试 +exec python3 -m pytest tests/ -v diff --git a/pybot/modules/ADS1115.py b/pybot/modules/ADS1115.py new file mode 100644 index 0000000..0db2d07 --- /dev/null +++ b/pybot/modules/ADS1115.py @@ -0,0 +1,56 @@ +"""ADS1115 16 位 I2C ADC 插件（experimental） + +⚠️ bot4cj/modules/ADS1115.lua 是 PCA9685 的复制残片（引用未定义的 +prescale、写的是 PCA9685 寄存器，运行必错），无忠实移植价值。 +本文件按 ADS1115 数据手册写干净的最小单次转换实现，未经实机验证。 + +ctx 元数据：name / fd（I2C 引用展开）/ address（如 "0x48"） +call 实时参数：channel（0~3，单端输入 AINx） +call 返回：{"raw": 有符号 16 位原始值, "voltage": 电压（±4.096V 量程）} +""" + +import logging +import struct +import time + +from pybot.hd import i2c + +log = logging.getLogger("pybot.modules.ADS1115") + +REG_CONVERSION = 0x00 +REG_CONFIG = 0x01 + +# 单次转换配置：OS=1(开始) | MUX=100+ch(单端) | PGA=001(±4.096V) | MODE=1(单次) +# 低字节：DR=100(128SPS) | 比较器禁用(11) +_CONFIG_HI_BASE = 0b1_100_001_1 # channel 0，其余通道 MUX +ch +_CONFIG_LO = 0b100_0_0_0_11 +FSR = 4.096 # 满量程 ±4.096V + + +def build(ctx): + """无需初始化（单次转换模式按需触发），仅校验设备可达""" + fd = int(ctx["fd"]) + addr = int(str(ctx["address"]), 0) + log.info("ADS1115 build (experimental): fd=%d addr=0x%02X", fd, addr) + return {"result": "OK"} + + +def call(ctx): + """触发一次单次转换并读回结果""" + fd = int(ctx["fd"]) + addr = int(str(ctx["address"]), 0) + channel = int(ctx.get("channel", 0)) & 0x03 + config_hi = _CONFIG_HI_BASE + (channel << 4) + with i2c.transaction(fd): + i2c.set_addr(fd, addr) + i2c.write_block(fd, REG_CONFIG, bytes([config_hi, _CONFIG_LO])) + time.sleep(0.009) # 128SPS 一次转换约 8ms + data = i2c.read_block(fd, REG_CONVERSION, 2) + raw = struct.unpack(">h", data)[0] # 大端有符号 16 位 + return {"result": "OK", "raw": raw, "voltage": raw * FSR / 32768.0} + + +def destroy(ctx): + """单次转换模式空闲即低功耗，无需复位""" + log.info("ADS1115 destroy") + return {"result": "OK"} diff --git a/pybot/modules/ADS1256.py b/pybot/modules/ADS1256.py new file mode 100644 index 0000000..50b200e --- /dev/null +++ b/pybot/modules/ADS1256.py @@ -0,0 +1,24 @@ +"""ADS1256 24 位 SPI ADC 插件 —— 对应 bot4cj/modules/ADS1256.lua（空桩） + +Lua 版即为空桩（无任何实现），保持同等状态：三个入口仅记日志。 +实机接入时参考 modules/SPI.py 取 fd 后经 pybot.hd.spi.transfer 实现。 +""" + +import logging + +log = logging.getLogger("pybot.modules.ADS1256") + + +def build(ctx): + log.warning("ADS1256 is a stub (not implemented), build ctx: %s", ctx) + return {"result": "OK", "status": "stub"} + + +def call(ctx): + log.warning("ADS1256 is a stub (not implemented)") + return {"result": "OK", "status": "stub"} + + +def destroy(ctx): + log.warning("ADS1256 is a stub (not implemented)") + return {"result": "OK", "status": "stub"} diff --git a/pybot/modules/PCA9685.py b/pybot/modules/PCA9685.py new file mode 100644 index 0000000..4f97f6a --- /dev/null +++ b/pybot/modules/PCA9685.py @@ -0,0 +1,72 @@ +"""PCA9685 16 路 PWM 舵机控制器插件 —— 与 bot4cj/modules/PCA9685.lua 逐语义对齐 + +pybot 插件协议：build(ctx) / call(ctx) / destroy(ctx) +ctx 元数据来自 @PCA9685 块： + name / fd（I2C 引用展开）/ address（原始字符串如 "0x40"） +call 实时参数：channel（int）/ angle（int，0~180） +""" + +import logging +import time + +from pybot.hd import i2c + +log = logging.getLogger("pybot.modules.PCA9685") + +# MODE1 寄存器（0x00）控制位 +MODE1 = 0x00 +MODE1_RESTART = 0x80 +MODE1_SLEEP = 0x10 +MODE1_AUTOINC = 0x20 +PRE_SCALE = 0xFE +LED0_ON_L = 0x06 # 通道 0 PWM 寄存器组起点，每通道 4 个寄存器 + +# 50Hz 预分频：25MHz / (4096 * 50Hz) - 1，四舍五入 = 121（与 Lua 版一致） +PRESCALE_50HZ = int(25000000.0 / (4096.0 * 50.0) - 1.0 + 0.5) + + +def _fd_addr(ctx): + return int(ctx["fd"]), int(str(ctx["address"]), 0) + + +def build(ctx): + """初始化：重启 → SLEEP → 设 prescale(50Hz) → 唤醒（寄存器序列与 Lua 版一致）""" + fd, addr = _fd_addr(ctx) + log.info("PCA9685 build: fd=%d addr=0x%02X prescale=%d", + fd, addr, PRESCALE_50HZ) + with i2c.transaction(fd): + i2c.set_addr(fd, addr) + i2c.write_reg(fd, MODE1, MODE1_RESTART) # 复位 + time.sleep(0.005) + i2c.write_reg(fd, MODE1, MODE1_SLEEP) # 休眠（改 prescale 前提） + i2c.write_reg(fd, PRE_SCALE, PRESCALE_50HZ) # 50Hz + i2c.write_reg(fd, MODE1, MODE1_AUTOINC) # 唤醒 + 自动递增 + time.sleep(0.005) + return {"result": "OK"} + + +def call(ctx): + """写通道 PWM：0.5ms(0°) ~ 2.5ms(180°) 脉宽 @ 50Hz(20ms 周期)""" + fd, addr = _fd_addr(ctx) + channel = int(ctx["channel"]) + angle = float(ctx["angle"]) + width = 0.5 + 2.0 * angle / 180.0 # 脉宽 ms + offval = int(width * 4096.0 / 20.0) # 20ms 周期内的计数（floor） + reg = LED0_ON_L + 4 * channel + with i2c.transaction(fd): + i2c.set_addr(fd, addr) + i2c.write_reg(fd, reg + 0, 0x00) # ON_L + i2c.write_reg(fd, reg + 1, 0x00) # ON_H + i2c.write_reg(fd, reg + 2, offval & 0xFF) # OFF_L + i2c.write_reg(fd, reg + 3, (offval >> 8) & 0xFF) # OFF_H + return {"result": "OK"} + + +def destroy(ctx): + """复位 PCA9685""" + fd, addr = _fd_addr(ctx) + log.info("PCA9685 destroy: fd=%d addr=0x%02X", fd, addr) + with i2c.transaction(fd): + i2c.set_addr(fd, addr) + i2c.write_reg(fd, MODE1, MODE1_RESTART) + return {"result": "OK"} diff --git a/pybot/modules/SPI.py b/pybot/modules/SPI.py new file mode 100644 index 0000000..73b31c3 --- /dev/null +++ b/pybot/modules/SPI.py @@ -0,0 +1,42 @@ +"""SPI 总线插件 —— 与 bot4cj/modules/SPI.lua 逐语义对齐 + +@SPI 块元数据：name / path / spi_mode / bits / speed / delay +build 打开并配置 spidev；call 返回 {"fd": fd}（供其他块引用展开）； +destroy 关闭。模块级全局 _fd 即设备私有状态 +（pybot 每设备加载独立模块实例，与 Lua 每设备独立状态机等价）。 +""" + +import logging + +from pybot.hd import spi + +log = logging.getLogger("pybot.modules.SPI") + +_fd = -1 # 全局文件句柄（对应 SPI.lua 的 fd） + + +def build(ctx): + global _fd + for key in ("path", "spi_mode", "bits", "speed", "delay"): + if key not in ctx: + return {"result": "ERR", "message": f"Invalid Arguments: {key}"} + log.info("SPI build parameters: %s", ctx) + _fd = spi.open_dev(ctx["path"]) + if _fd <= 0: + return {"result": "ERR", "message": "Failed to open SPI file"} + spi.setup(_fd, int(str(ctx["spi_mode"]), 0), + int(ctx["bits"]), int(ctx["speed"])) + return {"result": "OK", "fd": _fd} + + +def call(ctx): + return {"fd": _fd} + + +def destroy(ctx): + global _fd + log.info("SPI destroy, fd=%d", _fd) + if _fd > 0: + spi.close_dev(_fd) + _fd = -1 + return {"result": "OK"} diff --git a/pybot/pybot/__init__.py b/pybot/pybot/__init__.py new file mode 100644 index 0000000..d732b84 --- /dev/null +++ b/pybot/pybot/__init__.py @@ -0,0 +1,27 @@ +"""pybot —— 机器人 .mt 动作脚本解析器（bot4cj 的 Python 迁移版） + +架构与 bot4cj（仓颉版）逐语义对齐： + .mt 脚本 → parser.MatrixParser → MatrixObject 列表 + → robot.RobotStruct（按 @块名分发硬件变量工厂） + → active() / run() / deactive() + +硬件驱动插件（modules/.py）替代仓颉版的 Lua 扩展层， +协议为 build(ctx) / call(ctx) / destroy(ctx) 三段生命周期。 + +迁移方案见 docs/reports/bot4cj_python_migration_dev_doc.md。 +""" + +__version__ = "0.1.0" + +from .parser import ( # noqa: F401 + MatrixObject, + MatrixParser, + MatrixSaver, + ParseError, + DoesNotSync, + MatrixIndexError, + get_mt, + parse_mt, + to_int, +) +from .robot import RobotStruct # noqa: F401 diff --git a/pybot/pybot/__main__.py b/pybot/pybot/__main__.py new file mode 100644 index 0000000..8f35da2 --- /dev/null +++ b/pybot/pybot/__main__.py @@ -0,0 +1,7 @@ +"""python3 -m pybot 入口""" + +import sys + +from .main import main + +sys.exit(main()) diff --git a/pybot/pybot/hardvars.py b/pybot/pybot/hardvars.py new file mode 100644 index 0000000..b25a31f --- /dev/null +++ b/pybot/pybot/hardvars.py @@ -0,0 +1,183 @@ +"""硬件变量 —— 移植自 bot4cj/src/r.cj 的 MatrixVars_Hardware 及六个工厂 + +仓颉版三闭包 rise/things/free 改为基类三方法 build()/play(params)/destroy()， +JsonValue 通信协议改为原生 dict/str（无跨语言边界后无需序列化）。 + +每个子类提供 from_matrix(matrix, i, M, **kwargs) 工厂： + matrix: MatrixObject（行 0 是表头） + i: 数据行号（从 1 开始） + M: 已创建的硬件变量字典（供引用其他硬件，如 Servo→controller） +""" + +import logging + +from .hd import i2c as hd_i2c +from .hd.mpu6050 import MPU6050 +from .hd.servo import Servo +from .hd.speaker import Speaker +from .parser import get_mt, parse_mt, to_int + +log = logging.getLogger("pybot.hardvars") + + +class HardwareVar: + """硬件变量基类（对应仓颉版 MatrixVars_Hardware） + + build() —— 激活（对应闭包 rise） + play(params) —— 调用（对应闭包 things），入参/返回 dict（Alias 返回 str） + destroy() —— 释放（对应闭包 free），要求幂等 + """ + + def __init__(self, name): + self.name = name + + def build(self): + pass + + def play(self, params): + return {} + + def destroy(self): + pass + + +# ---------------- Alias 别名变量 ---------------- + +class AliasVar(HardwareVar): + """参数别名：@Action 列名 `s1.A` 中的 A → angle；play() 返回真实参数名 str""" + + def __init__(self, name, alias): + super().__init__(name) + self.alias = alias + + def play(self, params): + return self.alias + + @classmethod + def from_matrix(cls, matrix, i, M, **kwargs): + name = matrix[i, matrix.index_of_col(0, "name")] + alias = get_mt(matrix, i, matrix.index_of_col(0, "alias")) + log.info("Create a Alias hard-Var: %s -> %s", name, alias) + return cls(name, alias) + + +# ---------------- I2C 总线变量 ---------------- + +class I2CVar(HardwareVar): + """I2C 总线：工厂阶段即打开设备（与仓颉版一致）；play() 返回 {"fd": fd}""" + + def __init__(self, name, path): + super().__init__(name) + self.bus = hd_i2c.I2CBus(path) + + def play(self, params): + return {"fd": self.bus.fd} + + def destroy(self): + log.info("I2C HVar: %s is closing now", self.name) + self.bus.close() # close 幂等 + + @classmethod + def from_matrix(cls, matrix, i, M, **kwargs): + name = matrix[i, matrix.index_of_col(0, "name")] + path = get_mt(matrix, i, matrix.index_of_col(0, "path")) + log.info("Create a I2C hard-Var: %s, path: %s", name, path) + return cls(name, path) + + +# ---------------- MPU6050_BIND 传感器变量 ---------------- + +class MPU6050Var(HardwareVar): + """MPU6050 绑定：build 启动采样线程，destroy 停止；play() 返回最近姿态""" + + def __init__(self, name, sensor): + super().__init__(name) + self.sensor = sensor + + def build(self): + log.info("MPU6050_BIND HVar: %s is starting now", self.name) + self.sensor.setup() + self.sensor.start_daemon() + + def play(self, params): + return self.sensor.latest() + + def destroy(self): + log.info("MPU6050_BIND HVar: %s is closing now", self.name) + self.sensor.stop_daemon() + self.sensor.wait_daemon() + + @classmethod + def from_matrix(cls, matrix, i, M, **kwargs): + name = matrix[i, matrix.index_of_col(0, "name")] + i2c_name = get_mt(matrix, i, matrix.index_of_col(0, "I2C")) + addr = get_mt(matrix, i, matrix.index_of_col(0, "addr")) + log.info("Create a MPU6050_BIND HVar: %s, use i2c interf: %s, " + "i2c addr: %s", name, i2c_name, addr) + fd = M[i2c_name].play({})["fd"] # 引用 I2C 变量取 fd + return cls(name, MPU6050(fd, to_int(addr))) + + +# ---------------- Servo 舵机变量 ---------------- + +class ServoVar(HardwareVar): + """舵机：play({"angle": x}) 经校准算法后回调 controller（如 PCA9685 插件）""" + + def __init__(self, name, servo): + super().__init__(name) + self.servo = servo + + def play(self, params): + self.servo.set_angle(float(params["angle"])) + return {"status": "OK"} + + @classmethod + def from_matrix(cls, matrix, i, M, **kwargs): + name = matrix[i, matrix.index_of_col(0, "name")] + max_a = parse_mt(matrix, i, matrix.index_of_col(0, "max")) + min_a = parse_mt(matrix, i, matrix.index_of_col(0, "min")) + offset = parse_mt(matrix, i, matrix.index_of_col(0, "offset")) + ctrl = get_mt(matrix, i, matrix.index_of_col(0, "controller")) + channel = parse_mt(matrix, i, matrix.index_of_col(0, "channel"), int) + log.info("Create a Servo hard-Var: %s, max: %s, min: %s, offset: %s, " + "controller: %s, channel: %s", + name, max_a, min_a, offset, ctrl, channel) + controller = M[ctrl] # like pca9685 + # int(angle) 截断对齐仓颉版 JsonInt(Int64(angle)) + servo = Servo(max_a, min_a, offset, + lambda angle: controller.play( + {"channel": channel, "angle": int(angle)})) + return cls(name, servo) + + +# ---------------- Speaker 扬声器变量 ---------------- + +class SpeakerVar(HardwareVar): + """扬声器：play({"frequency": f, "delay": ms}) 发声并阻塞 delay 毫秒""" + + def __init__(self, name, speaker): + super().__init__(name) + self.speaker = speaker + + def play(self, params): + freq = float(params["frequency"]) + delay = int(float(params["delay"])) # 对齐仓颉版 Int32(asFloat) + self.speaker.play(freq, delay) + return {"status": "OK"} + + @classmethod + def from_matrix(cls, matrix, i, M, dry_run=False, **kwargs): + name = matrix[i, matrix.index_of_col(0, "name")] + channels = parse_mt(matrix, i, matrix.index_of_col(0, "channels"), int) + sample_rate = parse_mt(matrix, i, + matrix.index_of_col(0, "sample_rate"), int) + volumn = parse_mt(matrix, i, matrix.index_of_col(0, "volumn")) + max_f = parse_mt(matrix, i, matrix.index_of_col(0, "max_f")) + min_f = parse_mt(matrix, i, matrix.index_of_col(0, "min_f")) + log.info("Create a Speaker: %s, channels: %s, sample_rate: %s, " + "volume: %s, max_f: %s, min_f: %s", + name, channels, sample_rate, volumn, max_f, min_f) + speaker = Speaker(channels=channels, sample_rate=sample_rate, + volumn=volumn, max_f=max_f, min_f=min_f, + dry_run=dry_run) + return cls(name, speaker) diff --git a/pybot/pybot/hd/__init__.py b/pybot/pybot/hd/__init__.py new file mode 100644 index 0000000..6472e7e --- /dev/null +++ b/pybot/pybot/hd/__init__.py @@ -0,0 +1 @@ +"""硬件层（Linux I2C / SPI / 舵机 / 传感器 / 扬声器）""" diff --git a/pybot/pybot/hd/i2c.py b/pybot/pybot/hd/i2c.py new file mode 100644 index 0000000..7db5b8a --- /dev/null +++ b/pybot/pybot/hd/i2c.py @@ -0,0 +1,219 @@ +"""Linux I2C 封装 —— 移植自 bot4cj/src/hd/I2C.cj + src/cpp/I2C.c + +纯标准库实现（fcntl.ioctl + os.read/write），零第三方依赖。 + +与仓颉版的差异（审计修复，语义不变）： + S06: read_reg 返回 0..255 无符号 int（仓颉版返回有符号 Int8， + 16 位拼接时符号扩展破坏低字节）； + Q03: 每 fd 锁用 dict[fd → threading.Lock]（仓颉版 20 槽数组 O(n) 扫描且容量受限）。 + +后端可切换：默认真实总线；use_mock() 切换为记录型 Mock（--dry-run 与单测共用）， +Mock 的写序列 journal 是与仓颉版做逐字节 diff 的等价性验证工件（方案 §6.3）。 +""" + +import fcntl +import logging +import os +import threading +from contextlib import contextmanager + +log = logging.getLogger("pybot.i2c") + +I2C_SLAVE = 0x0703 # at least on arm64（与仓颉版一致） + + +class I2CError(OSError): + """I2C 设备异常（对应仓颉版 I2cExcept 系列）""" + + +# ---------------- 后端（真实总线 / Mock） ---------------- + +class _RealBackend: + """真实 I2C 总线：glibc open/ioctl/read/write 一层到底""" + + def open(self, path): + return os.open(path, os.O_RDWR) + + def close(self, fd): + os.close(fd) + + def set_addr(self, fd, addr): + fcntl.ioctl(fd, I2C_SLAVE, addr) + + def write_reg(self, fd, reg, val): + if os.write(fd, bytes([reg & 0xFF, val & 0xFF])) != 2: + raise I2CError(f"failed to write i2c reg 0x{reg:02X} (fd={fd})") + + def read_reg(self, fd, reg): + # 读取寄存器之前要先写寄存器地址 + if os.write(fd, bytes([reg & 0xFF])) != 1: + raise I2CError(f"failed to select i2c reg 0x{reg:02X} (fd={fd})") + data = os.read(fd, 1) + if len(data) != 1: + raise I2CError(f"failed to read i2c reg 0x{reg:02X} (fd={fd})") + return data[0] # 0..255 无符号（修复 S06） + + def write_block(self, fd, reg, data): + """向 reg 起连写多字节（ADS1115 等 16 位寄存器设备）""" + buf = bytes([reg & 0xFF]) + bytes(data) + if os.write(fd, buf) != len(buf): + raise I2CError(f"failed to write i2c block at reg 0x{reg:02X} (fd={fd})") + + def read_block(self, fd, reg, n): + """从 reg 起连读 n 字节（自动递增寄存器，MPU6050 一次读 14 字节）""" + if os.write(fd, bytes([reg & 0xFF])) != 1: + raise I2CError(f"failed to select i2c reg 0x{reg:02X} (fd={fd})") + data = os.read(fd, n) + if len(data) != n: + raise I2CError(f"short read at i2c reg 0x{reg:02X}: " + f"{len(data)}/{n} bytes (fd={fd})") + return data + + +class MockBackend: + """记录型 Mock 总线：不碰硬件，记录全部操作序列供断言/diff""" + + def __init__(self): + self.journal = [] # [(op, ...), ...] 全部操作按序记录 + self.reg_values = {} # {(fd, reg): int} 单寄存器读注入 + self.block_values = {} # {(fd, reg): bytes} 块读注入 + self._next_fd = 1000 + + def open(self, path): + fd = self._next_fd + self._next_fd += 1 + self.journal.append(("open", path, fd)) + return fd + + def close(self, fd): + self.journal.append(("close", fd)) + + def set_addr(self, fd, addr): + self.journal.append(("set_addr", fd, addr)) + + def write_reg(self, fd, reg, val): + self.journal.append(("write_reg", fd, reg & 0xFF, val & 0xFF)) + + def read_reg(self, fd, reg): + self.journal.append(("read_reg", fd, reg)) + return self.reg_values.get((fd, reg), 0) & 0xFF + + def write_block(self, fd, reg, data): + self.journal.append(("write_block", fd, reg & 0xFF, bytes(data))) + + def read_block(self, fd, reg, n): + self.journal.append(("read_block", fd, reg, n)) + return self.block_values.get((fd, reg), bytes(n)) + + +_backend = _RealBackend() + + +def use_mock(): + """切换到 Mock 总线（--dry-run / 单测），返回 MockBackend 以便读取 journal""" + global _backend + _backend = MockBackend() + return _backend + + +def use_real(): + """切换回真实总线""" + global _backend + _backend = _RealBackend() + + +# ---------------- I2C 锁具（每 fd 一把，修复 Q03） ---------------- +# 同一 fd 上"设地址 + 读/写寄存器"必须是原子序列（多设备共享总线） + +_MAIN_LOCK = threading.Lock() # 主锁，操作锁具注册表时使用 +_LOCKS = {} # {fd: threading.Lock} + + +def _register_lock(fd): + with _MAIN_LOCK: + _LOCKS[fd] = threading.Lock() + + +def _unregister_lock(fd): + with _MAIN_LOCK: + _LOCKS.pop(fd, None) + + +@contextmanager +def transaction(fd): + """with transaction(fd): 持有该 fd 的总线锁，保证 I2C 操作序列原子""" + with _MAIN_LOCK: + lock = _LOCKS.get(fd) + if lock is None: + raise I2CError(f"no i2c lock for fd {fd} (device not opened by pybot?)") + with lock: + yield + + +# ---------------- 模块级函数（供插件 modules/*.py 直接调用） ---------------- +# 对应仓颉版注册给 Lua 的 i2c_open/i2c_close/i2c_set_addr/i2c_write_reg/i2c_read_reg + +def open_dev(path): + fd = _backend.open(path) + _register_lock(fd) + log.debug("i2c open %s -> fd=%d", path, fd) + return fd + + +def close_dev(fd): + _unregister_lock(fd) + _backend.close(fd) + log.debug("i2c close fd=%d", fd) + + +def set_addr(fd, addr): + _backend.set_addr(fd, addr) + + +def write_reg(fd, reg, val): + _backend.write_reg(fd, reg, val) + + +def read_reg(fd, reg): + return _backend.read_reg(fd, reg) + + +def write_block(fd, reg, data): + _backend.write_block(fd, reg, data) + + +def read_block(fd, reg, n): + return _backend.read_block(fd, reg, n) + + +# ---------------- I2C 总线对象（对应仓颉版 I2C_Orig） ---------------- + +class I2CBus: + """持有一个 I2C 设备文件句柄；close() 幂等""" + + def __init__(self, path): + self.path = path + self.fd = open_dev(path) + + def close(self): + if self.fd >= 0: + close_dev(self.fd) + self.fd = -1 + + def set_addr(self, addr): + set_addr(self.fd, addr) + + def write_reg(self, reg, val): + write_reg(self.fd, reg, val) + + def read_reg(self, reg): + return read_reg(self.fd, reg) + + def write_block(self, reg, data): + write_block(self.fd, reg, data) + + def read_block(self, reg, n): + return read_block(self.fd, reg, n) + + def transaction(self): + return transaction(self.fd) diff --git a/pybot/pybot/hd/kalman.py b/pybot/pybot/hd/kalman.py new file mode 100644 index 0000000..b3001a4 --- /dev/null +++ b/pybot/pybot/hd/kalman.py @@ -0,0 +1,29 @@ +"""一维卡尔曼滤波器 —— 移植自 bot4cj/src/math/kalman.cj + +用法： + k = Kalman(20.0, 200.0) # Q=20 R=200（与仓颉版 MPU6050 参数一致） + filtered = k.filter(sensor_value) + +R 固定时，Q 越大代表越信任测量值；Q 越小代表越信任模型预测值。 +""" + + +class Kalman: + """A=1 B=0 H=1 的一维卡尔曼滤波（五个核心公式）""" + + def __init__(self, q, r): + self.x_last = 0.0 # 上一时刻的最优结果 X(k-1|k-1) + self.p_last = 0.0 # 上一时刻最优结果的协方差 P(k-1|k-1) + self.a = 1.0 + self.q = q # 系统噪声协方差 + self.r = r # 测量噪声协方差 + + def filter(self, dat): + x_mid = self.a * self.x_last # (1) x(k|k-1) = A*X(k-1|k-1) + p_mid = self.a * self.p_last + self.q # (2) p(k|k-1) = A*p(k-1|k-1)*A'+Q + kg = p_mid / (p_mid + self.r) # (4) kg(k) = p(k|k-1)/(p(k|k-1)+R) + x_now = x_mid + kg * (dat - x_mid) # (3) x(k|k) = X(k|k-1)+kg*(Z(k)-X(k|k-1)) + p_now = (1.0 - kg) * p_mid # (5) p(k|k) = (1-kg)*P(k|k-1) + self.p_last = p_now + self.x_last = x_now + return x_now diff --git a/pybot/pybot/hd/mpu6050.py b/pybot/pybot/hd/mpu6050.py new file mode 100644 index 0000000..a5b40a2 --- /dev/null +++ b/pybot/pybot/hd/mpu6050.py @@ -0,0 +1,134 @@ +"""MPU6050 六轴姿态传感器 —— 移植自 bot4cj/src/hd/MPU6050.cj + +10ms 周期采样线程 + 6 路一维卡尔曼滤波，输出 roll/pitch 姿态角。 + +与仓颉版的差异（审计修复，语义不变）： + S05: ADDR_1 = 0x69（AD0 拉高时的地址；仓颉版误写 0x68 与 ADDR_0 重复）； + Q06: 停止采样用 threading.Event（仓颉版裸 bool 无内存屏障）； + pitch 用 atan2(-ax, sqrt(ay²+az²)) 代替 atan(-ax/sqrt(...))，防除零且值域一致。 +""" + +import logging +import math +import struct +import threading +import time + +from . import i2c +from .kalman import Kalman + +log = logging.getLogger("pybot.mpu6050") + +ADDR_0 = 0x68 # AD0 引脚接地 +ADDR_1 = 0x69 # AD0 引脚拉高（修复 S05） + +# 寄存器地址 +PWR_MGMT_1 = 0x6B +GYRO_CONFIG = 0x1B +ACCEL_CONFIG = 0x1C +ACCEL_XOUT_H = 0x3B # 14 字节连读起点：加速度(6) + 温度(2) + 陀螺仪(6) + +RAD_TO_DEG = 57.2956 # 弧度转角度（与仓颉版一致） + +SAMPLE_INTERVAL = 0.01 # 10ms 采样周期 + + +class MPU6050: + """MPU6050 传感器：绑定到某条 I2C 总线的 fd 上 + + 用法： + m = MPU6050(fd, ADDR_0) + m.setup() + m.start_daemon() + ... m.latest() ... + m.stop_daemon(); m.wait_daemon() + """ + + def __init__(self, fd, addr=ADDR_0): + self.fd = fd + self.addr = addr + # 6 路卡尔曼滤波（Q=20 R=200，与仓颉版参数一致） + self._k_ax = Kalman(20.0, 200.0) + self._k_ay = Kalman(20.0, 200.0) + self._k_az = Kalman(20.0, 200.0) + self._k_gx = Kalman(20.0, 200.0) + self._k_gy = Kalman(20.0, 200.0) + self._k_gz = Kalman(20.0, 200.0) + self._stop = threading.Event() # 修复 Q06 + self._thread = None + self._lock = threading.Lock() + self._latest = {"roll": 0.0, "pitch": 0.0, + "ax": 0.0, "ay": 0.0, "az": 0.0, + "gx": 0.0, "gy": 0.0, "gz": 0.0} + + # ---------------- 初始化 ---------------- + + def setup(self): + """唤醒 + 配置量程：陀螺仪 ±500°/s、加速度 ±8g（与仓颉版一致）""" + with i2c.transaction(self.fd): + i2c.set_addr(self.fd, self.addr) + time.sleep(0.001) + i2c.write_reg(self.fd, PWR_MGMT_1, 0x00) # 解除休眠 + i2c.write_reg(self.fd, GYRO_CONFIG, 0x08) # ±500°/s + i2c.write_reg(self.fd, ACCEL_CONFIG, 0x10) # ±8g + time.sleep(0.1) # 等待传感器稳定（锁外，不占总线） + log.debug("mpu6050 setup done (fd=%d addr=0x%02X)", self.fd, self.addr) + + # ---------------- 读取 ---------------- + + def read_sensor(self): + """一次事务连读 14 字节，返回 (ax, ay, az, gx, gy, gz) 原始值""" + with i2c.transaction(self.fd): + i2c.set_addr(self.fd, self.addr) + data = i2c.read_block(self.fd, ACCEL_XOUT_H, 14) + vals = struct.unpack(">7h", data) # 大端 7 个有符号 16 位 + # 索引 3 是温度，跳过 + return vals[0], vals[1], vals[2], vals[4], vals[5], vals[6] + + def _sample_once(self): + ax, ay, az, gx, gy, gz = self.read_sensor() + fax = self._k_ax.filter(float(ax)) + fay = self._k_ay.filter(float(ay)) + faz = self._k_az.filter(float(az)) + fgx = self._k_gx.filter(float(gx)) + fgy = self._k_gy.filter(float(gy)) + fgz = self._k_gz.filter(float(gz)) + # 姿态角（加速度计解算，与仓颉版公式一致；pitch 用 atan2 防除零） + roll = math.atan2(fay, faz) * RAD_TO_DEG + pitch = math.atan2(-fax, math.sqrt(fay * fay + faz * faz)) * RAD_TO_DEG + with self._lock: + self._latest.update(roll=roll, pitch=pitch, + ax=fax, ay=fay, az=faz, + gx=fgx, gy=fgy, gz=fgz) + + def latest(self): + """最近一次滤波后的姿态数据（线程安全，返回副本）""" + with self._lock: + return dict(self._latest) + + # ---------------- 采样线程 ---------------- + + def start_daemon(self): + if self._thread is not None: + return + self._stop.clear() + self._thread = threading.Thread(target=self._loop, + name="mpu6050-sampler", daemon=True) + self._thread.start() + log.debug("mpu6050 sampler started") + + def _loop(self): + while not self._stop.wait(SAMPLE_INTERVAL): # 10ms 周期 + try: + self._sample_once() + except i2c.I2CError as e: + log.warning("mpu6050 sample failed: %s", e) + + def stop_daemon(self): + self._stop.set() + + def wait_daemon(self): + if self._thread is not None: + self._thread.join() + self._thread = None + log.debug("mpu6050 sampler stopped") diff --git a/pybot/pybot/hd/servo.py b/pybot/pybot/hd/servo.py new file mode 100644 index 0000000..52c6c69 --- /dev/null +++ b/pybot/pybot/hd/servo.py @@ -0,0 +1,29 @@ +"""舵机类 —— 移植自 bot4cj/src/hd/Servo.cj + +含最大值、最小值、漂移值三个调整项，以及一个设置角度回调函数。 +角度算法与仓颉版逐行对齐： + a = angle + offset + a = clamp(a, |min|, |max|) + if max < 0: a = 180 - a # 负 max 表示舵机反装，角度翻转 +""" + +MAX_ANGLE = 180.0 + + +class Servo: + + def __init__(self, max_angle, min_angle, offset, set_angle_callback): + self.max = max_angle + self.min = min_angle + self.offset = offset + self._set_angle = set_angle_callback + + def set_angle(self, angle): + a = angle + self.offset # 加上漂移值 + if a > abs(self.max): + a = abs(self.max) + if a < abs(self.min): + a = abs(self.min) + if self.max < 0.0: + a = MAX_ANGLE - a # 反装翻转 + self._set_angle(a) diff --git a/pybot/pybot/hd/speaker.py b/pybot/pybot/hd/speaker.py new file mode 100644 index 0000000..fb9879d --- /dev/null +++ b/pybot/pybot/hd/speaker.py @@ -0,0 +1,98 @@ +"""扬声器 —— 移植自 bot4cj/src/hd/snd.cj + +与仓颉版的差异（审计修复 S03）： + 仓颉版 play 只是 sleep 占位、不出声；本版用标准库 wave 生成正弦波 + PCM 文件，经 `aplay -q` 播放（树莓派自带 alsa-utils）。 + aplay 不存在时降级为 sleep 占位（保持时序节拍不变）； + dry_run 模式下仅记日志立即返回（不 sleep、不出声）。 + +时序约定：play() 阻塞总耗时对齐 delay_ms（与仓颉版 sleep(delay) 节拍一致）， +播放提前结束则补足剩余时间。 +""" + +import logging +import math +import os +import shutil +import struct +import subprocess +import tempfile +import time +import wave + +log = logging.getLogger("pybot.speaker") + +FADE_MS = 5 # 首尾淡入淡出时长，消除爆音 + + +class Speaker: + """正弦波蜂鸣扬声器 + + volumn: 0.0~1.0 音量；max_f/min_f: 频率钳制范围（Hz） + """ + + def __init__(self, channels=1, sample_rate=44100, volumn=0.5, + max_f=20000.0, min_f=20.0, dry_run=False): + self.channels = channels + self.sample_rate = sample_rate + self.volumn = volumn + self.max_f = max_f + self.min_f = min_f + self.dry_run = dry_run + self._aplay = shutil.which("aplay") + if self._aplay is None and not dry_run: + log.warning("aplay not found, speaker degrades to silent sleep") + + def play(self, frequency, delay_ms): + """播放 frequency Hz 正弦波 delay_ms 毫秒（阻塞）""" + # 频率钳制（与仓颉版一致） + if frequency > self.max_f: + frequency = self.max_f + if frequency < self.min_f: + frequency = self.min_f + if self.dry_run: + log.info("speaker(dry-run): %.1fHz %dms", frequency, delay_ms) + return + start = time.monotonic() + if self._aplay is not None: + try: + self._play_aplay(frequency, delay_ms) + except (OSError, subprocess.SubprocessError) as e: + log.warning("aplay failed: %s", e) + # 补足剩余时间，保证总耗时对齐 delay_ms + remain = delay_ms / 1000.0 - (time.monotonic() - start) + if remain > 0: + time.sleep(remain) + + # ---------------- 内部 ---------------- + + def _synth(self, frequency, delay_ms): + """生成带 5ms 淡入淡出的 16bit PCM 正弦波""" + n = int(self.sample_rate * delay_ms / 1000.0) + fade = min(int(self.sample_rate * FADE_MS / 1000.0), n // 2) + amp = self.volumn * 32767.0 + w = 2.0 * math.pi * frequency / self.sample_rate + frames = bytearray() + for i in range(n): + gain = 1.0 + if i < fade: + gain = i / fade # 淡入 + elif i >= n - fade: + gain = (n - i) / fade # 淡出 + v = int(amp * gain * math.sin(w * i)) + frames += struct.pack(" fd=%d", path, fd) + return fd + + +def close_dev(fd): + _backend.close(fd) + log.debug("spi close fd=%d", fd) + + +def setup(fd, mode=0, bits=8, speed=500000): + _backend.setup(fd, mode, bits, speed) + + +def transfer(fd, tx, speed=500000, bits=8): + return _backend.transfer(fd, tx, speed, bits) diff --git a/pybot/pybot/main.py b/pybot/pybot/main.py new file mode 100644 index 0000000..5cd324a --- /dev/null +++ b/pybot/pybot/main.py @@ -0,0 +1,104 @@ +"""CLI 入口 —— 移植自 bot4cj/src/main.cj + +用法（与仓颉版管道方式兼容）： + cat scripts/std.mt | python3 -m pybot # stdin 管道 + python3 -m pybot scripts/std.mt # 位置参数（增强） + python3 -m pybot --dry-run scripts/std.mt # Mock 总线，动作序列到 stdout + python3 -m pybot --test roundtrip in.mt out.mt # 解析↔序列化往返 + +与仓颉版的差异（见方案 §4.7）： + --test nn/cnn/cnn1 移除（NN 由 actpred 承担）； + --test matrix_o/matrix_2 合并为 --test roundtrip（路径改为参数）； + 新增 --dry-run / --verbose；-i / -o 保留参数位（reserved，对接 actpred）。 +日志规范（S02 根治）：全部诊断走 logging(stderr)，stdout 保持纯净。 +""" + +import argparse +import logging +import sys + +from .hd import i2c, spi +from .parser import MatrixParser, MatrixSaver +from .robot import RobotStruct + + +def _build_argparser(): + p = argparse.ArgumentParser( + prog="pybot", + description="机器人 .mt 动作脚本解析器（bot4cj 的 Python 迁移版）") + p.add_argument("script", nargs="?", default=None, + help=".mt 脚本路径（缺省从 stdin 读取）") + p.add_argument("--test", nargs=3, metavar=("MODE", "IN", "OUT"), + help="自检：roundtrip 解析↔序列化往返") + p.add_argument("--dry-run", action="store_true", + help="Mock 总线运行（不碰硬件），动作序列输出到 stdout") + p.add_argument("--verbose", action="store_true", + help="DEBUG 级日志（stderr）") + p.add_argument("-i", metavar="FIFO", default=None, + help="(reserved) 传感器数据输出管道，对接 actpred") + p.add_argument("-o", metavar="FIFO", default=None, + help="(reserved) 动作指令输入管道，对接 actpred") + return p + + +def _roundtrip(in_path, out_path): + """解析 in.mt 再序列化到 out.mt（对应仓颉版 --test matrix_o/matrix_2）""" + objects = [] + with open(in_path, encoding="utf-8") as f: + parser = MatrixParser(f) + while True: + obj = parser.parse() + if obj is None: + break + objects.append(obj) + with open(out_path, "w", encoding="utf-8") as f: + MatrixSaver(f).save(objects) + logging.info("roundtrip: %d blocks, %s -> %s", + len(objects), in_path, out_path) + return 0 + + +def _dump_journal(*mocks): + """--dry-run 结束后把 Mock 总线动作序列输出到 stdout（等价性验证工件）""" + for mock in mocks: + for entry in mock.journal: + print(" ".join(str(x) for x in entry)) + + +def main(argv=None): + args = _build_argparser().parse_args(argv) + logging.basicConfig( + stream=sys.stderr, + level=logging.DEBUG if args.verbose else logging.INFO, + format="%(levelname)s %(name)s: %(message)s") + + if args.i or args.o: + logging.warning("-i/-o are reserved for actpred integration, ignored") + + if args.test: + mode, in_path, out_path = args.test + if mode != "roundtrip": + logging.error("unknown --test mode: %s (supported: roundtrip)", mode) + return 2 + return _roundtrip(in_path, out_path) + + mocks = () + if args.dry_run: + mocks = (i2c.use_mock(), spi.use_mock()) + + source = open(args.script, encoding="utf-8") if args.script else sys.stdin + try: + robot = RobotStruct(source, dry_run=args.dry_run) + with robot: + robot.run() + finally: + if args.script: + source.close() + + if args.dry_run: + _dump_journal(*mocks) + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/pybot/pybot/parser.py b/pybot/pybot/parser.py new file mode 100644 index 0000000..39bc066 --- /dev/null +++ b/pybot/pybot/parser.py @@ -0,0 +1,243 @@ +"""矩阵脚本（.mt）解析器 —— 移植自 bot4cj/src/P.cj + +与仓颉版的差异（均为审计修复，语义不变）： + S01: submit_line 的列数校验修复为真实生效（仓颉版因 `!` 优先级恒假）； + S02: 移除逐字符 debug 打印，stdout 保持纯净，诊断走 logging(stderr)； + S04: 换行统一用 '\\n' 字符比较（Python 无 Rune/String 混淆问题）； + Q05: 越界访问抛 MatrixIndexError（携带块名/行/列），报错可读。 +""" + +import logging + +log = logging.getLogger("pybot.parser") + + +# ---------------- 解析异常 ---------------- + +class ParseError(Exception): + """解析异常基类（对应仓颉版 ParseExcept）""" + + def __init__(self, msg, line): + super().__init__(f"{msg}. we got line: {line}") + self.line = line + + +class DoesNotSync(ParseError): + """行没对齐：某一行的列数与上一行不一致（修复 S01，携带期望/实际列数）""" + + def __init__(self, line, expected, actual): + super().__init__( + f"Matrix line does not sync (expected {expected} columns, got {actual})", + line) + self.expected = expected + self.actual = actual + + +class MatrixIndexError(Exception): + """矩阵越界访问（修复 Q05，携带块名/行/列）""" + + def __init__(self, name, row, col): + super().__init__(f"matrix @{name}: cell ({row}, {col}) out of range") + self.name = name + self.row = row + self.col = col + + +# ---------------- 矩阵编程语言 - 基本对象 ---------------- + +class MatrixObject: + """一个 @块（名字 + 原始字符串矩阵）""" + + def __init__(self): + self.name = "" # 块名（@ 后面的标识符） + self.values = [] # 原始矩阵 list[list[str]]（含标题行） + self._tmp = [] # rune buf 字符缓冲区 + self._grp = [] # one line 整行缓冲区 + + # -------- 是否已命名 -------- + def has_name(self): + return self.name != "" + + # -------- 行数（含标题行）/ 列数 -------- + @property + def size(self): + return len(self.values) + + @property + def col(self): + return len(self.values[0]) if self.values else 0 + + # -------- 获取矩阵元素，只能取，不能修改 -------- + def __getitem__(self, rc): + row, col = rc + try: + return self.values[row][col] + except IndexError: + raise MatrixIndexError(self.name, row, col) from None + + # -------- 在某一行中寻找某一个字符串的列号 -------- + def index_of_col(self, row, key): + for c, item in enumerate(self.values[row]): + if item == key: + return c + raise KeyError(f"column '{key}' not found in matrix @{self.name}") + + # -------- 储存对象（与仓颉版 MatrixObject.save 输出格式一致） -------- + def save(self, out): + out.write(f"@{self.name}\n") + for vec in self.values: + for item in vec: + out.write(item) + out.write(" ") + out.write(";\n") + out.write("@\n") + + # -------- FSM 提交接口（由 MatrixParser 驱动） -------- + def add_rune(self, c): + self._tmp.append(c) + + def submit_name(self): + if not self._tmp: + return + self.name = "".join(self._tmp) + self._tmp.clear() + + def submit_item(self): + if not self._tmp: + return + self._grp.append("".join(self._tmp)) + self._tmp = [] + + def submit_line(self, count): + if not self._grp: + return + # 修复 S01：仓颉版 `!values.size == 0 && !grp.size == ...` 因优先级恒假 + if self.values and len(self._grp) != len(self.values[-1]): + raise DoesNotSync(count, len(self.values[-1]), len(self._grp)) + self.values.append(self._grp) + self._grp = [] + + +# ---------------- 矩阵式解析器（FSM 逐字符，状态与仓颉版一致） ---------------- + +class MatrixParser: + """输入文本流，逐块产出 MatrixObject（一次 parse() 解析一个 @块）""" + + def __init__(self, source): + self.reset(source) + + def reset(self, source): + """切换目标输入；source 为 str 或带 read() 的文本流""" + self._text = source if isinstance(source, str) else source.read() + self._pos = 0 + self._flag = False # 结束标志 + self._nl = True # new line + self._cm = False # comment + self._nm = False # name-line + self.obj = MatrixObject() + + def parse(self): + """解析下一个 @块；输入耗尽且无新块时返回 None""" + count = 1 # 行计数器始于 1 + self._flag = False # 重置结束标志 + self.obj = MatrixObject() # 一次只解析一个对象 + while not self._flag and self._pos < len(self._text): + c = self._text[self._pos] + self._pos += 1 + count = self._next(c, count) + + if not self.obj.has_name(): # 意外终止（无新对象） + return None + log.debug("parsed block @%s: %d rows x %d cols", + self.obj.name, self.obj.size, self.obj.col) + return self.obj + + def _next(self, c, ct): + """FSM 状态转移，与仓颉版 P.cj _next() 逐分支对齐（S02：无逐字符打印）""" + count = ct + # ------------ COMMENTS ------------ + if not self._cm and c == '#': # 注释开始时 + self._cm = True + elif self._cm and c == '\n': # 注释结束时 + self._nl = True + self._cm = False + count += 1 + elif self._cm: # 注释进行时 + pass + # ------------ new line ------------ + elif self._nl and c in ' \t\r': # 新行空字符，维持现状 + pass + elif self._nl and c == '\n': # 新行空行 + count += 1 + elif self._nl and c == '@' and not self.obj.has_name(): + self._nl = False + self._nm = True + elif self._nl and c == '@' and self.obj.has_name(): + self._flag = True # 块结束 + self._nl = False + self._nm = True + elif self._nl: # 新行内容 + self._nl = False + self.obj.add_rune(c) + # ------------- not nl ------------- + elif self._nm and c in ' \t\n': # 添加名字 + self.obj.submit_name() + self._nl = True + self._nm = False + elif not self._nm and c in ' \t\n': # 添加命名项（S04：'\n' 统一处理） + self.obj.submit_item() + if c == '\n': + count += 1 + elif c == ';': # 普通行结束 + self.obj.submit_item() + self.obj.submit_line(count) + self._nl = True + else: # 非空格、非制表符、非分号 + self.obj.add_rune(c) + return count + + +# ---------------- 矩阵式储存器 ---------------- + +class MatrixSaver: + """接收 MatrixObject 容器与输出流，序列化回 .mt 格式""" + + def __init__(self, out): + self._out = out + + def save(self, objects): + for item in objects: + item.save(self._out) + self._out.write("\n") # 对象之间空一行 + + +# ---------------- 占位符语义（与仓颉版 MatrixVars_Hardware.getMt/parseMt 对齐） ---------------- +# +# ⚠️ 与 actpred/mt_parser.py 的语义差异（不得误统一）： +# actpred 数据管线首行 '-' 用 @Servo 初始角度填充； +# pybot 运行时首行 '-' 数值列解析为 0（Servo 有 max/min 钳位保护）。 + +def get_mt(obj, row, col): + """获得项目值：原始值是占位符且非首行（row>1），递归取上一行同列值""" + raw = obj[row, col] + if raw == "-" and row > 1: + return get_mt(obj, row - 1, col) + return raw + + +def parse_mt(obj, row, col, typ=float): + """解析项目值：占位符递归向上；首行（row==1）占位符解析为 typ("0")""" + raw = obj[row, col] + if raw == "-" and row > 1: + return parse_mt(obj, row - 1, col, typ) + if raw == "-": + return typ("0") + return typ(raw) + + +def to_int(s): + """整数解析，支持 0x 十六进制前缀（@PCA9685 address、@MPU6050_BIND addr 列）""" + s = s.strip() + if s.lower().startswith("0x") or s.lower().startswith("-0x"): + return int(s, 16) + return int(s) diff --git a/pybot/pybot/plugin.py b/pybot/pybot/plugin.py new file mode 100644 index 0000000..9f5b00e --- /dev/null +++ b/pybot/pybot/plugin.py @@ -0,0 +1,102 @@ +"""Python 插件加载器 —— 替代 bot4cj/src/r.cj 的 Lua 状态机桥接（MtVars_Lua） + +插件协议（modules/.py，替代 Lua build/call/destroy，见方案 §3.4）： + def build(ctx: dict) -> dict # 初始化硬件 + def call(ctx: dict) -> dict # ctx = 元数据 + 实时参数 + def destroy(ctx: dict) -> dict # 复位/关闭硬件 + +与仓颉版的差异（审计修复 S07）： + Lua 路径硬编码 ./modules/ → 三级查找： + $PYBOT_MODULES_PATH（冒号分隔多路径）→ 包内置 modules/ → cwd/modules/。 + +每次加载都创建独立模块实例（spec_from_file_location 唯一模块名）， +对齐仓颉版"每个设备一个独立 Lua 状态机"的隔离语义 +（插件内模块级全局变量即设备私有状态）。 +""" + +import importlib.util +import itertools +import logging +import os +from pathlib import Path + +from .hardvars import HardwareVar + +log = logging.getLogger("pybot.plugin") + +_instance_counter = itertools.count() + + +class PluginNotFound(Exception): + """插件未找到（携带插件名与全部搜索路径）""" + + def __init__(self, name, searched): + paths = ", ".join(str(p) for p in searched) + super().__init__(f"plugin '{name}' not found (searched: {paths})") + self.name = name + self.searched = searched + + +def search_dirs(): + """插件搜索目录（按优先级）：$PYBOT_MODULES_PATH → 包内置 → cwd""" + dirs = [] + env = os.environ.get("PYBOT_MODULES_PATH", "") + for p in env.split(":"): + if p: + dirs.append(Path(p)) + dirs.append(Path(__file__).resolve().parent.parent / "modules") # 包内置 + dirs.append(Path.cwd() / "modules") + return dirs + + +def find_module(name): + """按三级路径查找 .py，找不到抛 PluginNotFound""" + searched = search_dirs() + for d in searched: + path = d / f"{name}.py" + if path.is_file(): + return path + raise PluginNotFound(name, searched) + + +def load_module(name): + """加载插件为独立模块实例（每次调用产生新实例，状态互不共享）""" + path = find_module(name) + mod_name = f"pybot_plugin_{name}_{next(_instance_counter)}" + spec = importlib.util.spec_from_file_location(mod_name, path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + for fn in ("build", "call", "destroy"): + if not callable(getattr(module, fn, None)): + raise PluginNotFound( + name, [path]) from ValueError( + f"plugin '{name}' missing function {fn}(ctx)") + log.debug("loaded plugin %s from %s", name, path) + return module + + +class PluginVar(HardwareVar): + """插件硬件变量（对应仓颉版 MtVars_Lua） + + metadata: @块行元数据 + 引用硬件的 play({}) 展开结果（如 I2C → fd） + """ + + def __init__(self, name, block_name, metadata): + super().__init__(name) + self.metadata = metadata + self.module = load_module(block_name) + + def build(self): + ctx = dict(self.metadata) + log.info("plugin build parameters: %s", ctx) + return self.module.build(ctx) + + def play(self, params): + ctx = dict(self.metadata) # 元数据拷贝，合并实时参数 + ctx.update(params) + return self.module.call(ctx) + + def destroy(self): + ctx = dict(self.metadata) + log.info("plugin destroy parameters: %s", ctx) + return self.module.destroy(ctx) diff --git a/pybot/pybot/robot.py b/pybot/pybot/robot.py new file mode 100644 index 0000000..fb0c588 --- /dev/null +++ b/pybot/pybot/robot.py @@ -0,0 +1,177 @@ +"""机器人结构 —— 移植自 bot4cj/src/r.cj 的 RobotStruct + +流程：解析 .mt 脚本 → 创建硬件变量字典 → active（按序 build） + → run（逐行执行 @Action）→ deactive（倒序 destroy）。 + +与仓颉版的差异（审计修复，语义不变）： + r.cj:911 FIXME —— 重名硬件变量先 destroy 旧值再覆盖，并输出警告； + Q05: @Action 列名合法性（必须含一个 '.'）在执行前整表校验一次； + Q07: deactive 幂等 + atexit 兜底 + with 上下文管理器，确定性释放 fd。 +""" + +import atexit +import logging + +from . import plugin +from .hardvars import (AliasVar, I2CVar, MPU6050Var, ServoVar, SpeakerVar) +from .parser import MatrixParser, MatrixSaver, get_mt, parse_mt + +log = logging.getLogger("pybot.robot") + +# 已知块名 → 硬件变量工厂（Action 特殊处理；未知块 → 插件） +_FACTORIES = { + "I2C": I2CVar, + "Alias": AliasVar, + "Servo": ServoVar, + "Speaker": SpeakerVar, + "MPU6050_BIND": MPU6050Var, +} + + +class RobotStruct: + """机器人结构：从矩阵脚本动态构建 + + 用法： + with RobotStruct(sys.stdin) as robot: # __enter__ → active() + robot.run() # __exit__ → deactive() + """ + + def __init__(self, source, dry_run=False): + self.C = [] # 解析出的 MatrixObject 容器（保持脚本顺序） + self.M = {} # {name: HardwareVar}（dict 有序，支撑倒序 destroy） + self.dry_run = dry_run + self._deactivated = False + self._read_matrix(source) + self._create_hardvars() + atexit.register(self.deactive) # Q07 兜底：异常退出也释放 fd + + # ---------------- 生命周期 ---------------- + + def active(self): + """激活所有硬件变量（按创建顺序 build）""" + for item in self.M.values(): + item.build() + + def deactive(self): + """倒序释放所有硬件变量；幂等（with / atexit 双路径安全）""" + if self._deactivated: + return + self._deactivated = True + for item in reversed(list(self.M.values())): + try: + item.destroy() + except Exception: + log.exception("destroy %s failed", item.name) + + def __enter__(self): + self.active() + return self + + def __exit__(self, exc_type, exc, tb): + self.deactive() + return False + + # ---------------- 解析与构建 ---------------- + + def _read_matrix(self, source): + parser = MatrixParser(source) + while True: + obj = parser.parse() + if obj is None: + break + log.debug("read matrix block @%s", obj.name) + self.C.append(obj) + + def _put_hardvar(self, name, var): + """插入硬件变量；重名先释放旧值（修复 r.cj:911 FIXME）""" + old = self.M.get(name) + if old is not None: + log.warning("hard-Var '%s' redefined, destroying the old one", name) + old.destroy() + self.M[name] = var + + def _create_hardvars(self): + """按块名分发工厂；Action 跳过；未知块 → Python 插件""" + for matrix in self.C: + mt_name = matrix.name + if mt_name == "Action": + continue + factory = _FACTORIES.get(mt_name) + if factory is not None: + idx_name = matrix.index_of_col(0, "name") + for i in range(1, matrix.size): + name = matrix[i, idx_name] + self._put_hardvar(name, factory.from_matrix( + matrix, i, self.M, dry_run=self.dry_run)) + elif matrix.size > 0: + self._create_plugin_vars(matrix) + + def _create_plugin_vars(self, matrix): + """未知块：逐行构建元数据并加载 modules/<块名>.py 插件""" + idx_name = matrix.index_of_col(0, "name") + for i in range(1, matrix.size): + name = matrix[i, idx_name] + metadata = {} + for j in range(matrix.col): + col_name = matrix[0, j] + value = get_mt(matrix, i, j) + ref = self.M.get(value) + if ref is not None: + # 值是已存在的硬件变量名 → play({}) 展开（如 I2C → fd） + metadata.update(ref.play({})) + else: + metadata[col_name] = value + self._put_hardvar(name, plugin.PluginVar(name, matrix.name, + metadata)) + + # ---------------- 执行 ---------------- + + def find_mt(self, name): + """在矩阵对象容器中寻找目标块""" + for item in self.C: + if item.name.strip() == name: + return item + return None + + def run(self): + """逐行执行 @Action 矩阵""" + action = self.find_mt("Action") + if action is None: + raise ValueError("no @Action matrix in script") + columns = self._validate_action(action) # Q05: 执行前整表校验 + for i in range(1, action.size): + row_sets = {} # {设备名: {参数名: float}} + for j, (dev, param) in enumerate(columns): + # 可能的参数别名转换（A → angle） + alias = self.M.get(param) + if isinstance(alias, AliasVar): + param = alias.play({}) + val = parse_mt(action, i, j) + row_sets.setdefault(dev, {})[param] = val + # 执行一行：逐设备调用 play + for dev, params in row_sets.items(): + self.M[dev].play(params) + + def _validate_action(self, action): + """校验 @Action 全部列名为 '设备.参数' 且设备已定义，返回 [(dev, param)]""" + columns = [] + for j in range(action.col): + col_name = action[0, j] + parts = col_name.split(".") + if len(parts) != 2: + raise ValueError( + f"@Action column {j} '{col_name}' is not valid " + f"(expected 'device.param')") + dev, param = parts + if dev not in self.M: + raise ValueError( + f"@Action column {j} '{col_name}': " + f"device '{dev}' is not defined") + columns.append((dev, param)) + return columns + + # ---------------- 序列化 ---------------- + + def save(self, out): + """储存整个矩阵变量表（--test roundtrip 用）""" + MatrixSaver(out).save(self.C) diff --git a/pybot/requirements.txt b/pybot/requirements.txt new file mode 100644 index 0000000..ac94880 --- /dev/null +++ b/pybot/requirements.txt @@ -0,0 +1,3 @@ +# 运行时零第三方依赖（纯标准库：fcntl/struct/threading/wave/argparse） +# 以下仅开发测试用 +pytest>=7.0 diff --git a/pybot/scripts b/pybot/scripts new file mode 120000 index 0000000..e4bef60 --- /dev/null +++ b/pybot/scripts @@ -0,0 +1 @@ +../bot4cj/scripts \ No newline at end of file diff --git a/pybot/tests/__init__.py b/pybot/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pybot/tests/conftest.py b/pybot/tests/conftest.py new file mode 100644 index 0000000..c73806f --- /dev/null +++ b/pybot/tests/conftest.py @@ -0,0 +1,30 @@ +"""pytest 公共配置：把项目根加入 sys.path（无需安装即可 import pybot）""" + +import sys +from pathlib import Path + +ROOT = Path(__file__).resolve().parent.parent +sys.path.insert(0, str(ROOT)) + +import pytest # noqa: E402 + +from pybot.hd import i2c, spi # noqa: E402 + + +SCRIPTS_DIR = ROOT / "scripts" + + +@pytest.fixture() +def mock_i2c(): + """切换到 Mock I2C 总线，测试结束恢复真实后端""" + mock = i2c.use_mock() + yield mock + i2c.use_real() + + +@pytest.fixture() +def mock_spi(): + """切换到 Mock SPI 总线，测试结束恢复真实后端""" + mock = spi.use_mock() + yield mock + spi.use_real() diff --git a/pybot/tests/test_hardvars.py b/pybot/tests/test_hardvars.py new file mode 100644 index 0000000..480503c --- /dev/null +++ b/pybot/tests/test_hardvars.py @@ -0,0 +1,121 @@ +"""硬件变量 + RobotStruct 全流程测试（Mock I2C 总线，journal 断言） + +关键期望值（与 bot4cj + PCA9685.lua 逐字节对齐）： + 45° → 脉宽 0.5+2*45/180 = 1.0ms → offval = int(1.0*4096/20) = 204 + 135° → 脉宽 2.0ms → offval = int(2.0*4096/20) = 409（s3 反装 45°→135°） + 0° → 脉宽 0.5ms → offval = 102（首行 '-' 占位解析为 0） +""" + +from pybot.robot import RobotStruct +from tests.conftest import SCRIPTS_DIR + +PRESCALE = 121 # int(25e6/(4096*50) - 1 + 0.5) + + +def std_robot(mock_i2c): + with open(SCRIPTS_DIR / "std.mt", encoding="utf-8") as f: + return RobotStruct(f, dry_run=True) + + +def _writes(mock, fd): + return [(e[2], e[3]) for e in mock.journal + if e[0] == "write_reg" and e[1] == fd] + + +def test_create_hardvars(mock_i2c): + robot = std_robot(mock_i2c) + assert set(robot.M) == {"A", "F", "i2c-0", "i2c-1", "pca9685-0", "spk", + "s1", "s2", "s3", "s4", "s5", + "s6", "s7", "s8", "s9", "s10"} + # 两条 I2C 总线在工厂阶段即打开 + opens = [e for e in mock_i2c.journal if e[0] == "open"] + assert [e[1] for e in opens] == ["/dev/i2c-0", "/dev/i2c-1"] + # 别名变量 + assert robot.M["A"].play({}) == "angle" + assert robot.M["F"].play({}) == "frequency" + robot.deactive() + + +def test_pca9685_build_sequence(mock_i2c): + robot = std_robot(mock_i2c) + fd = robot.M["i2c-1"].play({})["fd"] + robot.active() + # 初始化序列：复位 → SLEEP → prescale=121 → 唤醒（与 Lua 版一致） + assert _writes(mock_i2c, fd) == [ + (0x00, 0x80), (0x00, 0x10), (0xFE, PRESCALE), (0x00, 0x20)] + assert ("set_addr", fd, 0x40) in mock_i2c.journal + robot.deactive() + + +def test_run_stand_action(mock_i2c): + robot = std_robot(mock_i2c) + fd = robot.M["i2c-1"].play({})["fd"] + robot.active() + mock_i2c.journal.clear() # 只看 @Action 阶段 + robot.run() + writes = _writes(mock_i2c, fd) + reg = lambda ch: 0x06 + 4 * ch # noqa: E731 + + def servo_writes(ch, offval): + return [(reg(ch) + 0, 0x00), (reg(ch) + 1, 0x00), + (reg(ch) + 2, offval & 0xFF), (reg(ch) + 3, offval >> 8)] + + # s1: 45° 正装 → offval 204；s2: '-' 首行 → 0° → 102 + assert writes[0:4] == servo_writes(0, 204) + assert writes[4:8] == servo_writes(1, 102) + # s3: max=-90 反装，45° → 135° → offval 409（写 [153, 1]） + assert writes[8:12] == servo_writes(2, 409) + # s9/s10: 90° → offval = int(1.5*4096/20) = 307 + assert writes[32:36] == servo_writes(8, 307) + assert writes[36:40] == servo_writes(9, 307) + # 10 舵机 × 4 寄存器 + assert len(writes) == 40 + robot.deactive() + + +def test_deactive_reverse_and_idempotent(mock_i2c): + robot = std_robot(mock_i2c) + fd0 = robot.M["i2c-0"].play({})["fd"] + fd1 = robot.M["i2c-1"].play({})["fd"] + robot.active() + robot.deactive() + # 倒序：先 PCA9685 复位（fd1 写 0x80），再关 i2c-1，最后关 i2c-0 + closes = [e[1] for e in mock_i2c.journal if e[0] == "close"] + assert closes == [fd1, fd0] + reset_idx = mock_i2c.journal.index(("write_reg", fd1, 0x00, 0x80), + mock_i2c.journal.index(("close", fd1)) + - 5) + assert reset_idx < mock_i2c.journal.index(("close", fd1)) + # 幂等：重复调用不再产生操作 + n = len(mock_i2c.journal) + robot.deactive() + assert len(mock_i2c.journal) == n + + +def test_redefined_hardvar_destroys_old(mock_i2c, caplog): + """重名硬件变量：destroy 旧值 + 告警（修复 r.cj:911 FIXME）""" + text = ("@I2C\nname path;\nx /dev/i2c-0;\nx /dev/i2c-1;\n@\n" + "@Action\nx.angle;\n0;\n@\n") + robot = RobotStruct(text, dry_run=True) + closes = [e for e in mock_i2c.journal if e[0] == "close"] + assert len(closes) == 1 # 旧的 /dev/i2c-0 已被关闭 + assert any("redefined" in r.message for r in caplog.records) + robot.deactive() + + +def test_action_validation(mock_i2c): + """Q05：执行前整表校验列名格式与设备存在性""" + import pytest + text = ("@I2C\nname path;\ni /dev/i2c-0;\n@\n" + "@Action\nbadcolumn;\n0;\n@\n") + robot = RobotStruct(text, dry_run=True) + with pytest.raises(ValueError, match="not valid"): + robot.run() + robot.deactive() + + text2 = ("@I2C\nname path;\ni /dev/i2c-0;\n@\n" + "@Action\nghost.angle;\n0;\n@\n") + robot2 = RobotStruct(text2, dry_run=True) + with pytest.raises(ValueError, match="not defined"): + robot2.run() + robot2.deactive() diff --git a/pybot/tests/test_mpu6050.py b/pybot/tests/test_mpu6050.py new file mode 100644 index 0000000..773eb2c --- /dev/null +++ b/pybot/tests/test_mpu6050.py @@ -0,0 +1,48 @@ +"""MPU6050 测试：14 字节连读解包（含负值符号，回归 S06）与采样滤波""" + +import struct + +from pybot.hd import i2c +from pybot.hd.mpu6050 import (ACCEL_CONFIG, ACCEL_XOUT_H, ADDR_0, ADDR_1, + GYRO_CONFIG, MPU6050, PWR_MGMT_1) + + +def test_addr_constants(): + """修复 S05：ADDR_1 必须是 0x69（仓颉版误写 0x68）""" + assert ADDR_0 == 0x68 + assert ADDR_1 == 0x69 + + +def test_setup_sequence(mock_i2c): + fd = i2c.open_dev("/dev/i2c-1") + m = MPU6050(fd, ADDR_0) + m.setup() + writes = [(e[2], e[3]) for e in mock_i2c.journal if e[0] == "write_reg"] + assert writes == [(PWR_MGMT_1, 0x00), # 解除休眠 + (GYRO_CONFIG, 0x08), # ±500°/s + (ACCEL_CONFIG, 0x10)] # ±8g + assert ("set_addr", fd, ADDR_0) in mock_i2c.journal + + +def test_read_sensor_signed(mock_i2c): + """负值（高字节 ≥ 0x80）必须正确解为负数——回归 S06 符号扩展错误""" + fd = i2c.open_dev("/dev/i2c-1") + m = MPU6050(fd) + # ax=-2 ay=3 az=4096 temp=999(跳过) gx=100 gy=-200 gz=300 + mock_i2c.block_values[(fd, ACCEL_XOUT_H)] = struct.pack( + ">7h", -2, 3, 4096, 999, 100, -200, 300) + assert m.read_sensor() == (-2, 3, 4096, 100, -200, 300) + + +def test_sample_and_latest(mock_i2c): + fd = i2c.open_dev("/dev/i2c-1") + m = MPU6050(fd) + # 静止平放：az=4096(+1g@±8g)，其余 0 → roll/pitch 趋近 0 + mock_i2c.block_values[(fd, ACCEL_XOUT_H)] = struct.pack( + ">7h", 0, 0, 4096, 0, 0, 0, 0) + for _ in range(50): # 多次采样让卡尔曼收敛 + m._sample_once() + latest = m.latest() + assert abs(latest["roll"]) < 1.0 + assert abs(latest["pitch"]) < 1.0 + assert latest["az"] > 3000.0 # 滤波值向测量值收敛 diff --git a/pybot/tests/test_parser.py b/pybot/tests/test_parser.py new file mode 100644 index 0000000..d7b6eeb --- /dev/null +++ b/pybot/tests/test_parser.py @@ -0,0 +1,112 @@ +"""解析器测试：FSM / 列数校验(S01) / 占位符语义 / 序列化往返 / 全量脚本""" + +import io + +import pytest + +from pybot.parser import (DoesNotSync, MatrixParser, MatrixSaver, + get_mt, parse_mt) +from tests.conftest import SCRIPTS_DIR + + +def parse_all(text): + parser = MatrixParser(text) + objects = [] + while True: + obj = parser.parse() + if obj is None: + break + objects.append(obj) + return objects + + +# ---------------- 基本解析 ---------------- + +def test_parse_simple_block(): + objs = parse_all("@Foo\nname val;\na 1;\nb 2;\n@\n") + assert len(objs) == 1 + m = objs[0] + assert m.name == "Foo" + assert m.size == 3 and m.col == 2 + assert m[0, 0] == "name" and m[2, 1] == "2" + + +def test_parse_multiple_blocks_and_comments(): + text = ("# 头部注释\n" + "@A\nx y;\n1 2;\n@\n" + "@B\nn;\nv; # 行内注释\n@\n") + objs = parse_all(text) + assert [o.name for o in objs] == ["A", "B"] + assert objs[1][1, 0] == "v" + + +def test_parse_std_mt(): + objs = parse_all((SCRIPTS_DIR / "std.mt").read_text(encoding="utf-8")) + names = [o.name for o in objs] + assert names == ["Alias", "I2C", "PCA9685", "MPU6050_BIND", + "Speaker", "Servo", "Action"] + servo = objs[names.index("Servo")] + assert servo.size == 11 and servo.col == 6 # 表头 + 10 舵机 + # 注释掉的 MPU6050_BIND 数据行不产生内容 + assert objs[names.index("MPU6050_BIND")].size == 1 + + +@pytest.mark.parametrize("script", sorted(SCRIPTS_DIR.glob("*.mt")), + ids=lambda p: p.name) +def test_parse_all_scripts(script): + """全量 scripts/*.mt 解析：块内每行列数一致且非空""" + objs = parse_all(script.read_text(encoding="utf-8")) + assert objs, f"{script.name}: no blocks parsed" + for o in objs: + assert o.name + for row in o.values: + assert len(row) == o.col + + +# ---------------- S01：列数校验 ---------------- + +def test_does_not_sync(): + with pytest.raises(DoesNotSync) as ei: + parse_all("@Foo\na b c;\n1 2;\n@\n") + assert ei.value.expected == 3 and ei.value.actual == 2 + + +def test_missing_semicolon_merges_lines(): + """漏写分号导致跨行合并成一行（S04 回归场景），列数不齐即报错""" + with pytest.raises(DoesNotSync): + parse_all("@Foo\na b;\n1 2\n3 4;\n@\n") + + +# ---------------- 占位符语义 ---------------- + +def _matrix(text): + return parse_all(text)[0] + + +def test_placeholder_recursion(): + m = _matrix("@T\nname v;\na 5;\nb -;\nc -;\n@\n") + assert get_mt(m, 3, 1) == "5" # 递归两级到首行 + assert parse_mt(m, 3, 1) == 5.0 + + +def test_placeholder_first_row(): + """首行 '-'：get_mt 返回字面 '-'；parse_mt 解析为 typ("0") + ⚠️ 与 actpred/mt_parser.py 不同（后者用 @Servo 初始角度填充），不得误统一""" + m = _matrix("@T\nname v;\na -;\n@\n") + assert get_mt(m, 1, 1) == "-" + assert parse_mt(m, 1, 1) == 0.0 + assert parse_mt(m, 1, 1, int) == 0 + + +# ---------------- 序列化往返 ---------------- + +def test_roundtrip(): + text = (SCRIPTS_DIR / "std.mt").read_text(encoding="utf-8") + objs = parse_all(text) + out = io.StringIO() + MatrixSaver(out).save(objs) + objs2 = parse_all(out.getvalue()) + assert len(objs2) == len(objs) + for a, b in zip(objs, objs2): + assert a.name == b.name + assert a.values == b.values diff --git a/pybot/tests/test_plugin.py b/pybot/tests/test_plugin.py new file mode 100644 index 0000000..211967c --- /dev/null +++ b/pybot/tests/test_plugin.py @@ -0,0 +1,49 @@ +"""插件加载器测试：三级路径查找(S07) / 缺失报错 / 实例隔离""" + +import pytest + +from pybot import plugin + + +def test_find_builtin_module(): + """包内置 modules/ 能找到 PCA9685""" + path = plugin.find_module("PCA9685") + assert path.name == "PCA9685.py" + assert path.parent.name == "modules" + + +def test_plugin_not_found(): + with pytest.raises(plugin.PluginNotFound) as ei: + plugin.find_module("NoSuchDevice") + assert "NoSuchDevice" in str(ei.value) + assert "modules" in str(ei.value) # 报错含搜索路径 + + +def test_env_path_priority(tmp_path, monkeypatch): + """$PYBOT_MODULES_PATH 优先于包内置目录""" + (tmp_path / "PCA9685.py").write_text( + "def build(ctx): return {'from': 'env'}\n" + "def call(ctx): return {}\n" + "def destroy(ctx): return {}\n", encoding="utf-8") + monkeypatch.setenv("PYBOT_MODULES_PATH", str(tmp_path)) + assert plugin.find_module("PCA9685") == tmp_path / "PCA9685.py" + mod = plugin.load_module("PCA9685") + assert mod.build({}) == {"from": "env"} + + +def test_missing_protocol_function(tmp_path, monkeypatch): + """插件缺 build/call/destroy 任一函数即拒绝加载""" + (tmp_path / "Broken.py").write_text( + "def build(ctx): return {}\n", encoding="utf-8") + monkeypatch.setenv("PYBOT_MODULES_PATH", str(tmp_path)) + with pytest.raises(plugin.PluginNotFound): + plugin.load_module("Broken") + + +def test_independent_instances(): + """每次加载新模块实例：模块级全局状态互不共享（对齐 Lua 每设备状态机）""" + a = plugin.load_module("SPI") + b = plugin.load_module("SPI") + assert a is not b + a._fd = 42 + assert b._fd == -1 diff --git a/pybot/tests/test_servo.py b/pybot/tests/test_servo.py new file mode 100644 index 0000000..e56629e --- /dev/null +++ b/pybot/tests/test_servo.py @@ -0,0 +1,47 @@ +"""Servo 校准算法测试：正装/反装/漂移/钳位（与 Servo.cj 语义逐项对齐）""" + +from pybot.hd.servo import Servo + + +def make(max_a, min_a, offset): + sent = [] + servo = Servo(max_a, min_a, offset, sent.append) + return servo, sent + + +def test_normal_install(): + servo, sent = make(90.0, 0.0, 0.0) + servo.set_angle(45.0) + assert sent == [45.0] + + +def test_clamp_max(): + servo, sent = make(90.0, 0.0, 0.0) + servo.set_angle(120.0) + assert sent == [90.0] + + +def test_clamp_min(): + servo, sent = make(180.0, 30.0, 0.0) + servo.set_angle(10.0) + assert sent == [30.0] + + +def test_offset(): + servo, sent = make(180.0, 0.0, 5.0) + servo.set_angle(40.0) + assert sent == [45.0] + + +def test_inverted_install(): + """负 max 表示反装：a = 180 - a（std.mt 的 s3 max=-90，45° → 135°）""" + servo, sent = make(-90.0, 0.0, 0.0) + servo.set_angle(45.0) + assert sent == [180.0 - 45.0] + + +def test_inverted_clamp_uses_abs(): + """反装钳位用 |max|：-90 装机 100° → 钳到 90 → 翻转 90""" + servo, sent = make(-90.0, 0.0, 0.0) + servo.set_angle(100.0) + assert sent == [90.0]