
from pymtn_lib20 import *

def DevInit():
    print("--控制器初始化开始")
    ''' 搜索控制器'''
    (devNum, devinfolist, ret) = pymtn.NMC_DevSearch(pymtn.TSearchMode.Internet)
    if ret != 0:
        print("控制器搜索出错,返回值为:", ret)
        exit()
    if ret == 0:
        print("the count of devices: (%s)" % (devNum))
        for i in range(0, devNum):
            print(devinfolist[i].address)
            print(devinfolist[i].idStr)
            print(devinfolist[i].description)
            print(devinfolist[i].ID)
    if devNum <= 0:
        print("没有找到控制器")
        exit()

    ''' 打开第一个控制器'''
    (devHandle, ret) = pymtn.NMC_DevOpenByID(devinfolist[0].idStr)
    if ret != pymtn.RTN_CMD_SUCCESS:
        print("控制器打开出错", ret)
        exit()

    # ''' 打开第一个控制器--只有一张卡的时候，直接打开，不用搜索'''
    # (devHandle, ret) = pymtn.NMC_DevOpen(0)
    # if ret != pymtn.RTN_CMD_SUCCESS:
    #     print("控制器打开出错", ret)
    #     exit()

    (Info, ret) = pymtn.NMC_GetCardInfo(devHandle)
    num = Info.axisNum
    axisHandle = [c_ushort(0)] * num
    for i in range(0, num):
        (axisHandle[i], ret) = pymtn.NMC_MtOpen(devHandle, i)
        if ret != pymtn.RTN_CMD_SUCCESS:
            print("控制器打开出错", ret)
            exit()

    print("控制器初始化结束")
    return (devHandle, axisHandle)
"""--------------------状态读取--------------------"""
def _AxisSts(devHandle, axisHandle):
    print("状态读取 测试开始")

    # 获取轴状态
    sts, ret = pymtn.NMC_MtGetSts(axisHandle)
    if ret != 0:
        print(f"获取轴状态失败，错误码: {ret}")
    else:
        status = sts.value
        print(f"轴状态字: 0x{status:04X} (十进制: {status})")

        # 解析各个状态位
        status_bits = {
            "运动中": (status & pymtn.BIT_AXIS_BUSY) != 0,
            "位置到达": (status & pymtn.BIT_AXIS_POSREC) != 0,
            "运动错误": (status & pymtn.BIT_AXIS_MVERR) != 0,
            "伺服允许": (status & pymtn.BIT_AXIS_SVON) != 0,
            "坐标系模式": (status & pymtn.BIT_AXIS_CRD) != 0,
            "步进模式": (status & pymtn.BIT_AXIS_STEP) != 0,
            "正向限位": (status & pymtn.BIT_AXIS_LMTP) != 0,
            "负向限位": (status & pymtn.BIT_AXIS_LMTN) != 0,
            "正向软限位": (status & pymtn.BIT_AXIS_SOFTPOSLMT) != 0,
            "负向软限位": (status & pymtn.BIT_AXIS_SOFTNEGLMT) != 0,
            "伺服报警": (status & pymtn.BIT_AXIS_ALM) != 0,
            "位置超限": (status & pymtn.BIT_AXIS_POSERR) != 0,
            "急停触发": (status & pymtn.BIT_AXIS_ESTP) != 0,
            "硬件错误": (status & pymtn.BIT_AXIS_HWERR) != 0,
            "捕获触发": (status & pymtn.BIT_AXIS_CAPTSET) != 0,
        }

        # 打印状态信息
        print("轴状态详情:")
        for desc, state in status_bits.items():
            print(f"  {desc}: {'是' if state else '否'}")

    # 获取轴规划位置
    pos, ret = pymtn.NMC_MtGetPrfPos(axisHandle)
    if ret != 0:
        print(f"获取规划位置失败，错误码: {ret}")
    else:
        print(f"规划位置: {pos.value}")

    # 获取轴规划速度
    vel, ret = pymtn.NMC_MtGetPrfVel(axisHandle)
    if ret != 0:
        print(f"获取规划速度失败，错误码: {ret}")
    else:
        print(f"规划速度: {vel.value}")


    # 获取编码器位置
    enc_pos, ret = pymtn.NMC_MtGetEncPos(axisHandle)
    if ret != 0:
        print(f"获取编码器位置失败，错误码: {ret}")
    else:
        print(f"编码器位置: {enc_pos.value}")

    # 获取编码器速度
    enc_vel, ret = pymtn.NMC_GetEncVel(devHandle, 0)
    if ret != 0:
        print(f"获取编码器速度失败，错误码: {ret}")
    else:
        print(f"编码器速度: {enc_vel.value}")

    # 定义信号名称映射
    IO_SIGNALS = {
        pymtn.BIT_AXMTIO_LMTN: "负向限位",
        pymtn.BIT_AXMTIO_LMTP: "正向限位",
        pymtn.BIT_AXMTIO_HOME: "原点信号",
        pymtn.BIT_AXMTIO_ALARM: "驱动报警",
        pymtn.BIT_AXMTIO_ARRIVE: "电机到位",
        pymtn.BIT_AXMTIO_CAPT: "捕获信号",
        pymtn.BIT_AXMTIO_SVRCLR: "伺服清除"
    }

    # 获取运动控制专用IO
    motion_io, ret = pymtn.NMC_MtGetMotionIO(axisHandle)
    if ret != 0:
        print(f"获取运动IO失败，错误码: {ret}")
    else:
        io_value = motion_io.value
        print(f"运动IO状态 (二进制): {io_value:032b}")
        for bit_mask, signal_name in IO_SIGNALS.items():
            state = 1 if io_value & bit_mask else 0
            print(f"  {signal_name}: {state}")

    # 获取运动控制专用IO逻辑电平
    motion_io_logical, ret = pymtn.NMC_MtGetMotionIOLogical(axisHandle)
    if ret != 0:
        print(f"获取运动IO逻辑电平失败，错误码: {ret}")
    else:
        io_logical_value = motion_io_logical.value
        print(f"运动IO逻辑电平 (二进制): {io_logical_value:032b}")
        for bit_mask, signal_name in IO_SIGNALS.items():
            state = 1 if io_logical_value & bit_mask else 0
            print(f"  {signal_name}: {state}")

    #打包状态##########################################################
    # 读取4轴打包状态
    firstAxisHandle = axisHandle
    status_list = pymtn.TAxisStsPack()
    status_list, ret = pymtn.NMC_MtGetStsPack(firstAxisHandle, 4)
    if ret != 0:
        print(f"获取打包状态失败，错误码: {ret}")
    else:
        for i in range(4):
            print(f"轴{i}状态: 规划位置={status_list.prfPos[i]}, 实际位置={status_list.encPos[i]}, 规划速度={status_list.prfVel[i]}")

    # 读取12轴打包状态
    packSts, ret = pymtn.NMC_MtGetStsPack12Ex(firstAxisHandle)
    if ret != 0:
        print(f"获取12轴打包状态失败，错误码: {ret}")
    else:
        for i in range(12):
            print(f"轴{i}规划位置: {packSts.mtPrfPos[i]}, 规划速度: {packSts.mtPrfVel[i]}")

    print("状态读取 测试结束")
if __name__ == "__main__":

    devHandle, axisHandle = DevInit()
    _AxisSts(devHandle, axisHandle[0])