feat(pybot): bot4cj 的 Python 迁移版（.mt 解析 + 硬件驱动 + 插件系统）
- parser: FSM 解析器与仓颉版逐语义对齐，修 S01/S02/S04/Q05 - robot/hardvars: 六类硬件变量工厂，修 r.cj:911 重名 FIXME、Q07 倒序释放 - plugin: Python 插件替代 Lua 桥接，修 S07 三级路径查找 - hd: i2c(每fd锁 Q03/S06)/spi/servo/kalman/mpu6050(S05/Q06)/speaker(S03 实声) - modules: PCA9685 与 Lua 版逐字节对齐；ADS1115 重写(experimental)；ADS1256 空桩 - tests: 42 项 pytest 全绿（Mock 总线 journal 断言） - CLI: --dry-run / --test roundtrip / --verbose；运行时零第三方依赖
This commit is contained in:
qing committed
1 parent
85eaa2114e
commit
bba7ee3eb4
31 files changed
+2155
No files matched your search
@@ -0,0 +1,27 @@
|
||||
"""pybot —— 机器人 .mt 动作脚本解析器（bot4cj 的 Python 迁移版）
|
||||
|
||||
架构与 bot4cj（仓颉版）逐语义对齐：
|
||||
.mt 脚本 → parser.MatrixParser → MatrixObject 列表
|
||||
→ robot.RobotStruct（按 @块名分发硬件变量工厂）
|
||||
→ active() / run() / deactive()
|
||||
|
||||
硬件驱动插件（modules/<Name>.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
|
||||
@@ -0,0 +1,7 @@
|
||||
"""python3 -m pybot 入口"""
|
||||
|
||||
import sys
|
||||
|
||||
from .main import main
|
||||
|
||||
sys.exit(main())
|
||||
@@ -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)
|
||||
@@ -0,0 +1 @@
|
||||
"""硬件层（Linux I2C / SPI / 舵机 / 传感器 / 扬声器）"""
|
||||
@@ -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)
|
||||
@@ -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
|
||||
@@ -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")
|
||||
@@ -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)
|
||||
@@ -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("<h", v) * self.channels
|
||||
return bytes(frames)
|
||||
|
||||
def _play_aplay(self, frequency, delay_ms):
|
||||
fd, path = tempfile.mkstemp(suffix=".wav", prefix="pybot_beep_")
|
||||
os.close(fd)
|
||||
try:
|
||||
with wave.open(path, "wb") as f:
|
||||
f.setnchannels(self.channels)
|
||||
f.setsampwidth(2) # 16bit
|
||||
f.setframerate(self.sample_rate)
|
||||
f.writeframes(self._synth(frequency, delay_ms))
|
||||
subprocess.run([self._aplay, "-q", path], check=True,
|
||||
stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
|
||||
finally:
|
||||
os.unlink(path)
|
||||
@@ -0,0 +1,120 @@
|
||||
"""Linux spidev 封装 —— 对应 bot4cj/modules/SPI.lua 所需的底层能力
|
||||
|
||||
纯标准库实现（fcntl.ioctl + ctypes 缓冲区），零第三方依赖。
|
||||
与 hd.i2c 同构：后端可切换（use_mock()/use_real()），
|
||||
Mock journal 供 --dry-run 与单测断言。
|
||||
|
||||
spi_ioc_transfer 结构（32 字节，struct.pack 'QQIIHBBBBH'）：
|
||||
__u64 tx_buf; __u64 rx_buf; __u32 len; __u32 speed_hz;
|
||||
__u16 delay_usecs; __u8 bits_per_word; __u8 cs_change;
|
||||
__u8 tx_nbits; __u8 rx_nbits; __u16 pad;
|
||||
"""
|
||||
|
||||
import ctypes
|
||||
import fcntl
|
||||
import logging
|
||||
import os
|
||||
import struct
|
||||
|
||||
log = logging.getLogger("pybot.spi")
|
||||
|
||||
# spidev ioctl 常量（linux/spi/spidev.h，小端 arm64/x86_64 通用）
|
||||
SPI_IOC_WR_MODE = 0x40016B01
|
||||
SPI_IOC_WR_BITS_PER_WORD = 0x40016B03
|
||||
SPI_IOC_WR_MAX_SPEED_HZ = 0x40046B04
|
||||
SPI_IOC_MESSAGE_1 = 0x40206B00 # SPI_IOC_MESSAGE(1)
|
||||
|
||||
|
||||
class SPIError(OSError):
|
||||
"""SPI 设备异常"""
|
||||
|
||||
|
||||
# ---------------- 后端（真实总线 / Mock） ----------------
|
||||
|
||||
class _RealBackend:
|
||||
|
||||
def open(self, path):
|
||||
return os.open(path, os.O_RDWR)
|
||||
|
||||
def close(self, fd):
|
||||
os.close(fd)
|
||||
|
||||
def setup(self, fd, mode, bits, speed):
|
||||
fcntl.ioctl(fd, SPI_IOC_WR_MODE, struct.pack("B", mode))
|
||||
fcntl.ioctl(fd, SPI_IOC_WR_BITS_PER_WORD, struct.pack("B", bits))
|
||||
fcntl.ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, struct.pack("I", speed))
|
||||
|
||||
def transfer(self, fd, tx, speed, bits):
|
||||
"""全双工传输：发送 tx 字节串，返回等长 rx 字节串"""
|
||||
n = len(tx)
|
||||
tx_buf = ctypes.create_string_buffer(bytes(tx), n)
|
||||
rx_buf = ctypes.create_string_buffer(n)
|
||||
msg = struct.pack("QQIIHBBBBH",
|
||||
ctypes.addressof(tx_buf), ctypes.addressof(rx_buf),
|
||||
n, speed, 0, bits, 0, 0, 0, 0)
|
||||
if fcntl.ioctl(fd, SPI_IOC_MESSAGE_1, msg) < 0:
|
||||
raise SPIError(f"spi transfer failed (fd={fd}, {n} bytes)")
|
||||
return rx_buf.raw
|
||||
|
||||
|
||||
class MockBackend:
|
||||
"""记录型 Mock：不碰硬件，记录全部操作序列供断言/diff"""
|
||||
|
||||
def __init__(self):
|
||||
self.journal = []
|
||||
self.rx_values = {} # {fd: bytes} transfer 返回值注入
|
||||
self._next_fd = 2000
|
||||
|
||||
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 setup(self, fd, mode, bits, speed):
|
||||
self.journal.append(("setup", fd, mode, bits, speed))
|
||||
|
||||
def transfer(self, fd, tx, speed, bits):
|
||||
self.journal.append(("transfer", fd, bytes(tx), speed, bits))
|
||||
rx = self.rx_values.get(fd, b"")
|
||||
return rx.ljust(len(tx), b"\x00")[:len(tx)]
|
||||
|
||||
|
||||
_backend = _RealBackend()
|
||||
|
||||
|
||||
def use_mock():
|
||||
"""切换到 Mock 总线（--dry-run / 单测），返回 MockBackend 以便读取 journal"""
|
||||
global _backend
|
||||
_backend = MockBackend()
|
||||
return _backend
|
||||
|
||||
|
||||
def use_real():
|
||||
"""切换回真实总线"""
|
||||
global _backend
|
||||
_backend = _RealBackend()
|
||||
|
||||
|
||||
# ---------------- 模块级函数（供插件 modules/*.py 直接调用） ----------------
|
||||
|
||||
def open_dev(path):
|
||||
fd = _backend.open(path)
|
||||
log.debug("spi open %s -> 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)
|
||||
@@ -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 <in.mt> <out.mt> 解析↔序列化往返")
|
||||
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())
|
||||
@@ -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)
|
||||
@@ -0,0 +1,102 @@
|
||||
"""Python 插件加载器 —— 替代 bot4cj/src/r.cj 的 Lua 状态机桥接（MtVars_Lua）
|
||||
|
||||
插件协议（modules/<Name>.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):
|
||||
"""按三级路径查找 <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)
|
||||
@@ -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)
|
||||
Reference in new issue
Block a user