""" CTAG2F90D（Servo）控制器SDK (基于Modbus RTU的RS-485通信) --------------------------------------------- 一个轻量级的Python SDK，用于通过minimalmodbus库控制电机， 具有线程安全、清晰的寄存器访问方式和实时数据监控功能。 依赖安装: pip install minimalmodbus pyserial 作者: 知行机器人 """ import time import threading import minimalmodbus import serial # ----------------------------- # 寄存器映射 (保持寄存器，功能码0x03/0x06) # ----------------------------- # 写入寄存器 (目标值/控制) REG_TARGET_POS_HIGH = 0x0102 # 目标位置（高16位） REG_TARGET_POS_LOW = 0x0103 # 目标位置（低16位） REG_TARGET_SPEED = 0x0104 # 目标速度 REG_TARGET_FORCE = 0x0105 # 目标力/力矩 REG_TARGET_ACCELERATION = 0x0106 # 目标加速度 REG_TARGET_DECELERATION = 0x0107 # 目标减速度 REG_MOTION_TRIGGER = 0x0108 # 运动触发（0: 空闲, 1: 触发） # 读取寄存器 (反馈/状态) REG_REAL_POS_HIGH = 0x0418 # 实时位置（高16位） REG_REAL_POS_LOW = 0x0419 # 实时位置（低16位） REG_REAL_SPEED = 0x041A # 实时速度反馈 REG_REAL_CURRENT = 0x041B # 实时电流反馈 class MotorController: def __init__(self, port: str, slave_id: int = 1, baudrate: int = 115200, timeout: float = 1.0): """初始化与电机控制器的Modbus RTU通信 参数: port: 串口端口（例如，Windows上为'COM3'，Linux上为'/dev/ttyUSB0'） slave_id: Modbus从机地址（默认: 1） baudrate: 串口波特率（默认: 115200） timeout: 读写超时时间（秒，默认: 1.0） """ self.instrument = minimalmodbus.Instrument(port, slave_id) self.instrument.serial.baudrate = baudrate self.instrument.serial.bytesize = 8 self.instrument.serial.parity = serial.PARITY_NONE self.instrument.serial.stopbits = 1 self.instrument.serial.timeout = timeout self.instrument.mode = minimalmodbus.MODE_RTU self._lock = threading.Lock() # 线程锁，确保串口操作线程安全 self._monitor_running = False # 监控线程运行标志 self._monitor_thread = None # 监控线程对象 # ----------------------------- # 低级寄存器操作工具方法 # ----------------------------- def _write_register(self, addr: int, value: int) -> None: """写入单个16位保持寄存器（Modbus功能码0x06） 参数: addr: 寄存器地址 value: 要写入的值 """ with self._lock: self.instrument.write_register(addr, value, functioncode=6) def _read_register(self, addr: int) -> int: """读取单个16位保持寄存器（Modbus功能码0x03） 参数: addr: 寄存器地址 返回: 寄存器的值 """ with self._lock: return self.instrument.read_register(addr, functioncode=3) def _write_registers(self, addr: int, values: list) -> None: """写入多个16位保持寄存器（Modbus功能码0x10） 参数: addr: 起始寄存器地址 values: 要写入的值列表 """ with self._lock: self.instrument.write_registers(addr, values) def _read_registers(self, addr: int, count: int) -> list: """读取多个16位保持寄存器（Modbus功能码0x03） 参数: addr: 起始寄存器地址 count: 要读取的寄存器数量 返回: 寄存器值的列表 """ with self._lock: return self.instrument.read_registers(addr, count, functioncode=3) # ----------------------------- # 目标值/控制写入操作 # ----------------------------- def set_target_position(self, position: int) -> None: """设置目标位置（组合高低16位寄存器） 参数: position: 目标位置值 """ hi = (position >> 16) & 0xFFFF # 提取高16位 lo = position & 0xFFFF # 提取低16位 self._write_registers(REG_TARGET_POS_HIGH, [hi, lo]) def set_target_speed(self, speed: int) -> None: """设置目标速度 参数: speed: 目标速度值 """ self._write_register(REG_TARGET_SPEED, speed) def set_target_force(self, force: int) -> None: """设置目标力/力矩 参数: force: 目标力/力矩值 """ self._write_register(REG_TARGET_FORCE, force) def set_target_acceleration(self, acceleration: int) -> None: """设置目标加速度 参数: acceleration: 目标加速度值 """ self._write_register(REG_TARGET_ACCELERATION, acceleration) def set_target_deceleration(self, deceleration: int) -> None: """设置目标减速度 参数: deceleration: 目标减速度值 """ self._write_register(REG_TARGET_DECELERATION, deceleration) def trigger_motion(self) -> None: """使用已配置的目标参数触发运动""" self._write_register(REG_MOTION_TRIGGER, 1) def temp_move(self, position_mm: int, speed_pct: int, force_pct: int, accel: int, decel: int, trigger: bool = True) -> None: """临时运动控制方法，一次性设置所有运动参数并可选触发运动 参数: position_mm: 目标位置(毫米) speed_pct: 速度百分比 force_pct: 力/力矩百分比 accel: 加速度值 decel: 减速度值 trigger: 是否立即触发运动，默认为True """ self.set_target_position(position_mm) self.set_target_speed(speed_pct) self.set_target_force(force_pct) self.set_target_acceleration(accel) self.set_target_deceleration(decel) if trigger: self.trigger_motion() print(f"已触发运动到位置 {position_mm}，速度 {speed_pct}%，力 {force_pct}%") # ----------------------------- # 反馈/状态读取操作 # ----------------------------- def read_real_position(self) -> int: """读取实时位置（组合高低16位寄存器） 返回: 组合后的32位实时位置（处理有符号值） """ regs = self._read_registers(REG_REAL_POS_HIGH, 2) hi, lo = regs[0], regs[1] combined = (hi << 16) | lo # 处理32位有符号整数 if combined & 0x80000000: # 最高位为1表示负数 combined = combined - 0x100000000 return combined def read_real_speed(self) -> int: """读取实时速度反馈 返回: 实时速度值 """ return self._read_register(REG_REAL_SPEED) def read_real_current(self) -> int: """读取实时电流反馈 返回: 实时电流值 """ return self._read_register(REG_REAL_CURRENT) # ----------------------------- # 实时监控线程 # ----------------------------- def _monitor_loop(self, interval: float = 0.5) -> None: """实时数据监控的后台循环""" while self._monitor_running: try: pos = self.read_real_position() speed = self.read_real_speed() current = self.read_real_current() print(f"[实时数据] 位置: {pos:6d}, 速度: {speed:3d}, 电流: {current:3d}") except Exception as e: print(f"监控错误: {e}") time.sleep(interval) def start_monitoring(self, interval: float = 0.01) -> None: """启动实时数据监控线程 参数: interval: 监控间隔（秒，默认: 0.01） """ if not self._monitor_running: self._monitor_running = True self._monitor_thread = threading.Thread( target=self._monitor_loop, args=(interval,) ) self._monitor_thread.daemon = True # 守护线程，主程序退出时自动结束 self._monitor_thread.start() print("实时监控已启动。") def stop_monitoring(self) -> None: """停止实时数据监控线程""" self._monitor_running = False if self._monitor_thread: self._monitor_thread.join(timeout=1.0) # 等待线程结束 print("实时监控已停止。") if __name__ == "__main__": # 示例用法（根据实际设备调整端口/从机ID） PORT = "COM5" SLAVE_ID = 1 sdk = MotorController(PORT, SLAVE_ID, baudrate=115200, timeout=1.0) try: # 启动实时监控 sdk.start_monitoring(interval=0.5) # 循环控制：5次往复运动 # 假设 9000 为闭合位置，0 为张开位置 # 最后一次循环结束后，确保处于张开位置 (0) for i in range(5): print(f"\n--- 第 {i+1}/5 次循环 ---") # 阶段1: 运动到位置 9000 (闭合?) sdk.temp_move(position_mm=9000, speed_pct=5, force_pct=15, accel=60, decel=60, trigger=True) time.sleep(5) # 等待运动完成 # 阶段2: 运动到位置 0 (张开?) sdk.temp_move(position_mm=0, speed_pct=5, force_pct=15, accel=60, decel=60, trigger=True) time.sleep(5) # 等待运动完成 print("\n循环结束，确认最终状态为张开 (位置 0)...") # 再次发送指令确保最终状态为 0 (虽然循环最后一步已经是 0，但这保证了"最后要张开"的要求) sdk.temp_move(position_mm=0, speed_pct=5, force_pct=15, accel=60, decel=60, trigger=True) time.sleep(1) except KeyboardInterrupt: print("\n程序被用户中断。") except Exception as e: print(f"执行过程中出错: {e}") finally: sdk.stop_monitoring() print("程序已完成。")