Modbus 主站通信综合示例

# /**

#  * ex_modbus.py

#  * @brief Modbus 主站通信综合示例（TCP + RTU 双模式）

#  * @attention

#  *   - 确保控制器网络通信正常

#  *   - TCP 模式需在 PC 上启动虚拟 Modbus 从站模拟器（端口 5000）：

#  *     python3 -c "from pymodbus.server import StartTcpServer

#  *     from pymodbus.datastore import ModbusSlaveContext, ModbusSequentialDataBlock

#  *     store = ModbusSlaveContext(di=ModbusSequentialDataBlock(0, [0]*100),

#  *         co=ModbusSequentialDataBlock(0, [0]*100),

#  *         hr=ModbusSequentialDataBlock(0, [0]*100),

#  *         ir=ModbusSequentialDataBlock(0, [0]*100))

#  *     StartTcpServer(context=store, address=('0.0.0.0', 5000))"

#  *   - RTU 模式用于灵巧手（通过 RS485 串口连接控制器）

#  *       从站 ID = 2，波特率 115200，串口号根据实际接线修改

#  *   - 灵巧手寄存器地址（4X 保持寄存器）:

#  *       力阈值: 1115\~1119（5 指）  速度: 1125\~1130（6 关节）

#  *       目标位置: 1135\~1140（6 关节）  实际位置反馈: 1155\~1160

#  * @note 运行步骤

#  *       1. TCP 模式需先启动虚拟从站模拟器

#  *       2. 运行: python3 ex_modbus.py

#  */

import sys

import os

import platform

import time

# 基于脚本所在目录构建 SDK Python 绑定的绝对路径

_script_dir = os.path.dirname(os.path.abspath(__file__))

# 自动检测平台，添加对应 SDK Python 绑定路径

if platform.system() == "Windows":

    sys.path.append(os.path.join(_script_dir, "../../lib/windows/python"))

elif platform.machine() in ("aarch64", "arm64"):

    sys.path.append(os.path.join(_script_dir, "../../lib/linux/arm64/python"))

else:

    sys.path.append(os.path.join(_script_dir, "../../lib/linux/x86/python"))

# Windows: 将 tl_host.dll 所在目录加入 DLL 搜索路径

if platform.system() == "Windows":

    _dll_dir = os.path.join(_script_dir, "../../lib/windows")

    os.add_dll_directory(_dll_dir)

from tl_interface import *

# 灵巧手寄存器地址（Modbus 4X 保持寄存器）

REG_FORCE_LIMIT     = 1115   # 手指力阈值（5 个）

REG_SPEED           = 1125   # 手指速度（6 个）

REG_POS_TARGET      = 1135   # 目标位置（6 个）

REG_POS_FEEDBACK    = 1155   # 实际位置反馈（6 个）

# ============================================================

# 工具函数

# ============================================================

def print_result(msg, ret):

    """@brief 打印操作结果"""

    if ret == SUCCESS:

        print("  [成功] %s" % msg)

    else:

        print("  [失败] %s，错误码: %d" % (msg, ret))

def print_separator(title):

    """@brief 打印分隔标题"""

    print("\n========== %s ==========" % title)

def read_with_retry(sock, id, addr, qty, desc):

    """@brief 带重试的读取：最多尝试 3 次，每次间隔 500ms"""

    for attempt in range(1, 4):

        data = VectorInt()

        ret = modbus_read_holding_registers(sock, id, addr, qty, data)

        if ret == SUCCESS:

            print("  [成功] %s（第%d次）" % (desc, attempt))

            return ret, data

        print("  [重试] %s 第%d次失败，500ms后重试..." % (desc, attempt))

        time.sleep(0.5)

    print("  [失败] %s，错误码: %d（重试3次均失败）" % (desc, ret))

    return ret, VectorInt()

# ============================================================

# 主函数

# ============================================================

def main():

    # ---- 获取 SDK 版本 ----

    version = get_library_version()

    print("[信息] SDK 版本: %s" % version)

    # ---- 连接控制器 ----

    print_separator("连接控制器")

    sock = connect_robot("192.168.1.13", "6001")

    if sock < 0:

        print("[ERROR] 连接控制器失败")

        return -1

    print("[信息] 连接成功 (socket = %d)" % sock)

    # ===========================================================

    # 第一部分：Modbus TCP 模式（master ID = 0）

    #   覆盖功能码 01H\~06H + 10H

    # ===========================================================

    print_separator("第一部分：Modbus TCP 模式")

    # 1.1 设置 TCP 主站参数

    param = ModbusMasterParameter()

    param.type = "TCP"

    param.startAddress = True               # 起始地址 0

    param.TCP.IP = "192.168.1.100"          # 从站 IP（PC 端 Modbus 模拟器）

    param.TCP.port = 5000                   # 从站端口

    ret = modbus_set_master_parameter(sock, 0, param)

    print_result("modbus_set_master_parameter (id=0, TCP)", ret)

    if ret != SUCCESS:

        disconnect_robot(sock)

        return -1

    print("  类型: %s, IP: %s, 端口: %d" % (param.type, param.TCP.IP, param.TCP.port))

    # 1.2 打开 Modbus 主站

    ret = modbus_open_master(sock, 0)

    print_result("modbus_open_master (id=0)", ret)

    if ret != SUCCESS:

        disconnect_robot(sock)

        return -1

    time.sleep(0.5)

    # 1.3 功能码 05H: 写单线圈

    print_separator("TCP --- 05H 写单线圈")

    ret = modbus_write_signal_coil_status(sock, 0, 0, 0xFF00)

    print_result("modbus_write_signal_coil_status (addr=0, ON)", ret)

    time.sleep(0.2)

    # 1.4 功能码 01H: 读线圈状态（验证 05H 写入）

    print_separator("TCP --- 01H 读线圈状态")

    read_data = VectorInt()

    ret = modbus_read_coil_status(sock, 0, 0, 1, read_data)

    print_result("modbus_read_coil_status (addr=0, qty=1)", ret)

    if ret == SUCCESS and len(read_data) > 0:

        print("  线圈[0] = %d" % read_data[0])

    # 1.5 功能码 02H: 读输入状态

    print_separator("TCP --- 02H 读输入状态")

    read_data = VectorInt()

    ret = modbus_read_input_status(sock, 0, 0, 4, read_data)

    print_result("modbus_read_input_status (addr=0, qty=4)", ret)

    if ret == SUCCESS:

        print("  输入状态: ", end="")

        for i in range(len(read_data)):

            print("%d" % read_data[i], end=" ")

        print()

    # 1.6 功能码 06H: 写单寄存器

    print_separator("TCP --- 06H 写单寄存器")

    ret = modbus_write_signal_holding_registers(sock, 0, 0, 5678)

    print_result("modbus_write_signal_holding_registers (addr=0, val=5678)", ret)

    # 1.7 功能码 03H: 读保持寄存器（验证 06H 写入）

    print_separator("TCP --- 03H 读保持寄存器")

    read_data = VectorInt()

    ret = modbus_read_holding_registers(sock, 0, 0, 3, read_data)

    print_result("modbus_read_holding_registers (addr=0, qty=3)", ret)

    if ret == SUCCESS:

        print("  保持寄存器[0\~2]: ", end="")

        for i in range(len(read_data)):

            print("%d" % read_data[i], end=" ")

        print()

        if len(read_data) > 0 and read_data[0] == 5678:

            print("  [验证] 06H 写入与 03H 读取一致")

    # 1.8 功能码 04H: 读输入寄存器

    print_separator("TCP --- 04H 读输入寄存器")

    read_data = VectorInt()

    ret = modbus_read_input_registers(sock, 0, 0, 3, read_data)

    print_result("modbus_read_input_registers (addr=0, qty=3)", ret)

    if ret == SUCCESS:

        print("  输入寄存器: ", end="")

        for i in range(len(read_data)):

            print("%d" % read_data[i], end=" ")

        print()

    # 1.9 功能码 10H: 写多个保持寄存器

    print_separator("TCP --- 10H 写多个保持寄存器")

    write_data = [100, 200, 300]

    ret = modbus_write_multiple_holding_registers(sock, 0, 10, write_data)

    print_result("modbus_write_multiple_holding_registers (addr=10)", ret)

    if ret == SUCCESS:

        print("  写入数据: %d, %d, %d" % (write_data[0], write_data[1], write_data[2]))

    # 1.10 03H 回读验证批量写入

    read_data = VectorInt()

    ret = modbus_read_holding_registers(sock, 0, 10, 3, read_data)

    print_result("modbus_read_holding_registers (addr=10, qty=3) 回读验证", ret)

    if ret == SUCCESS and len(read_data) >= 3:

        match = (read_data[0] == 100 and read_data[1] == 200 and read_data[2] == 300)

        print("  批量读写一致性验证: %s" % ("通过" if match else "失败"))

    # ===========================================================

    # 第二部分：Modbus RTU 模式（master ID = 1）

    #   灵巧手控制示例

    # ===========================================================

    print_separator("第二部分：Modbus RTU 模式（灵巧手控制）")

    # 2.1 设置 RTU 主站参数

    param_rtu = ModbusMasterParameter()

    param_rtu.type = "RTU"

    param_rtu.startAddress = True

    param_rtu.RTU.slaveId = 2             # 灵巧手从站 ID

    param_rtu.RTU.port = 1                # 控制器串口号（根据接线修改）

    param_rtu.RTU.baudrate = 115200       # 波特率

    param_rtu.RTU.checkBit = "None"

    param_rtu.RTU.dataBit = 8

    param_rtu.RTU.stopBit = 1

    ret = modbus_set_master_parameter(sock, 1, param_rtu)

    print_result("modbus_set_master_parameter (id=1, RTU slaveId=2)", ret)

    if ret != SUCCESS:

        disconnect_robot(sock)

        return -1

    print("  从站ID: %d（灵巧手）" % param_rtu.RTU.slaveId)

    # 2.2 打开 Modbus 主站

    ret = modbus_open_master(sock, 1)

    print_result("modbus_open_master (id=1)", ret)

    if ret != SUCCESS:

        disconnect_robot(sock)

        return -1

    time.sleep(0.5)

    # 2.3 03H 读灵巧手寄存器（目标位置 + 实际位置）

    print_separator("RTU --- 03H 读保持寄存器")

    ret_tgt, tgt_data = read_with_retry(sock, 1, REG_POS_TARGET, 6, "读目标位置 1135\~1140")

    if ret_tgt == SUCCESS:

        print("  目标位置: ", end="")

        for i in range(len(tgt_data)):

            print("%d" % tgt_data[i], end=" ")

        print()

    ret_fb, fb_data = read_with_retry(sock, 1, REG_POS_FEEDBACK, 6, "读实际位置 1155\~1160")

    if ret_fb == SUCCESS:

        print("  实际位置: ", end="")

        for i in range(len(fb_data)):

            print("%d" % fb_data[i], end=" ")

        print()

    # 2.4 06H 设置手指力阈值

    print_separator("RTU --- 06H 写单寄存器（力阈值）")

    ret = modbus_write_signal_holding_registers(sock, 1, REG_FORCE_LIMIT, 500)

    print_result("写力阈值 (addr=%d, val=500)" % REG_FORCE_LIMIT, ret)

    time.sleep(0.2)

    # 2.5 06H 设置手指速度

    print_separator("RTU --- 06H 写单寄存器（速度）")

    ret = modbus_write_signal_holding_registers(sock, 1, REG_SPEED, 200)

    print_result("写速度 (addr=%d, val=200)" % REG_SPEED, ret)

    time.sleep(0.2)

    # 2.6 05H 写单线圈（测试）

    print_separator("RTU --- 05H 写单线圈")

    ret = modbus_write_signal_coil_status(sock, 1, 0, 0xFF00)

    print_result("modbus_write_signal_coil_status (addr=0, ON)", ret)

    time.sleep(0.2)

    # 2.7 01H 读线圈状态（验证）

    print_separator("RTU --- 01H 读线圈状态")

    coil_data = VectorInt()

    for at in range(3):

        ret = modbus_read_coil_status(sock, 1, 0, 1, coil_data)

        if ret == SUCCESS:

            break

        time.sleep(0.5)

    print_result("modbus_read_coil_status (addr=0, qty=1)", ret)

    if ret == SUCCESS and len(coil_data) > 0:

        print("  线圈[0] = %d" % coil_data[0])

    # 2.8 02H 读输入状态

    print_separator("RTU --- 02H 读输入状态")

    in_status = VectorInt()

    for at in range(3):

        ret = modbus_read_input_status(sock, 1, 0, 4, in_status)

        if ret == SUCCESS:

            break

        time.sleep(0.5)

    print_result("modbus_read_input_status (addr=0, qty=4)", ret)

    if ret == SUCCESS:

        print("  输入状态: ", end="")

        for i in range(len(in_status)):

            print("%d" % in_status[i], end=" ")

        print()

    # 2.9 04H 读输入寄存器

    print_separator("RTU --- 04H 读输入寄存器")

    in_reg = VectorInt()

    for at in range(3):

        ret = modbus_read_input_registers(sock, 1, 0, 3, in_reg)

        if ret == SUCCESS:

            break

        time.sleep(0.5)

    print_result("modbus_read_input_registers (addr=0, qty=3)", ret)

    if ret == SUCCESS:

        print("  输入寄存器: ", end="")

        for i in range(len(in_reg)):

            print("%d" % in_reg[i], end=" ")

        print()

    # 2.10 06H 设置手指目标位置（张开：所有手指 0）

    print_separator("RTU --- 06H 设置目标位置（张开）")

    # 手势"张开"位置 [0, 0, 0, 0, 0, 728]

    pos = [0, 0, 0, 0, 0, 728]

    for i in range(6):

        ret = modbus_write_signal_holding_registers(sock, 1, REG_POS_TARGET + i, pos[i])

        if ret != SUCCESS:

            print("  [失败] 写位置[%d] 错误码: %d" % (i, ret))

        time.sleep(0.05)

    print("  [成功] 手指已张开（位置全0，拇指728）")

    # ===========================================================

    # 第三部分：六维力传感器通讯参数

    # ===========================================================

    print_separator("第三部分：六维力传感器通讯参数")

    force_param = SixDimensionalForceCommunicationParams()

    force_param.sensorCommunicationType = 0       # 0: EtherCAT

    force_param.startupAutoConnectSensor = True   # 启动自动连接传感器

    force_param.sensorDragEnable = True           # 传感器拖拽使能

    force_param.etherCat_mapNum = 1

    force_param.YDirection = -1

    force_param.ZDirection = -1

    ret = set_six_dimensional_force_communication_params(sock, force_param)

    print_result("set_six_dimensional_force_communication_params", ret)

    if ret == SUCCESS:

        print("  传感器通信类型: EtherCAT")

        print("  拖拽使能: %s" % ("开启" if force_param.sensorDragEnable else "关闭"))

    # ===========================================================

    # 断开连接

    # ===========================================================

    print_separator("断开连接")

    disconnect_robot(sock)

    print("[信息] 已断开连接")

    print("\n[信息] Modbus 综合示例程序运行完毕")

    return 0

if __name__ == "__main__":

    main()