
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 Crd():
    crdHandle, ret = pymtn.NMC_CrdOpenEx(devHandle, 0)
    if ret != 0:
        print(f"打开坐标系组失败，错误码: {ret}")
    else:
        print("打开坐标系组成功")

    """ Crd系统配置"""
    crdCfg = pymtn.TCrdConfig()
    crdCfg.axCnts = 4  # 三轴
    crdCfg.pAxArray[0] = 0  # 坐标系运动X轴为第一轴
    crdCfg.pAxArray[1] = 1  # 坐标系运动Y轴为第二轴
    crdCfg.pAxArray[2] = 2  # 坐标系运动Z轴为第三轴
    crdCfg.pAxArray[3] = 3
    crdCfg.port[0] = 0  # 端口为0
    crdCfg.port[1] = 0  # 端口为0
    crdCfg.port[2] = 0  # 端口为0
    crdCfg.port[3] = 0  # 端口为0

    ret = pymtn.NMC_CrdConfig(crdHandle, crdCfg)
    if ret != 0:
        print(f"坐标系系统配置失败，错误码: {ret}")
    else:
        print("坐标系系统配置成功")

    """ Crd系统参数配置"""
    crdPrm = pymtn.TCrdPara()
    crdPrm.orgFlag = 1  # 设置坐标系原点为机械原点
    crdPrm.synAccMax = 10  # 插补最大合成加速度
    crdPrm.synVelMax = 50  # 插补最大合成速度
    crdPrm.offset[0] = 0  # 第一轴原点为0
    crdPrm.offset[1] = 0  # 第二轴原点为0
    crdPrm.offset[2] = 0  # 第三轴原点为0
    crdPrm.offset[3] = 0  # 第四轴原点为0

    ret = pymtn.NMC_CrdSetPara(crdHandle, crdPrm)
    if ret != 0:
        print(f"坐标系系统参数配置失败，错误码: {ret}")
    else:
        print("坐标系系统参数配置成功")

    """ 清空缓冲区数据"""
    ret = pymtn.NMC_CrdBufClr(crdHandle)
    if ret != 0:
        print(f"清空缓冲区数据失败，错误码: {ret}")
    else:
        print("清空缓冲区数据成功")

    # 插补运动指令#################################
    tg_pos_array = [1000, 2000, 3000, 4000]  # XYZ A 目标位置
    crd_ax_mask = 0x0F  # 参与运动的轴掩码 (bit0:X, bit1:Y, bit2:Z, bit3:A)
    ret = pymtn.NMC_CrdLineXYZA(crdHandle, 1, crd_ax_mask, tg_pos_array, 0, 500.0, 2000.0, 0)
    if ret != 0:
        print(f"四轴直线插补失败，错误码: {ret}")
    else:
        print("四轴直线插补指令添加成功")

    # 结束缓冲区运动（等待运动完成）
    ret = pymtn.NMC_CrdEndMtn(crdHandle)
    if ret != 0:
        print(f"结束缓冲区运动失败，错误码: {ret}")
    else:
        print("结束缓冲区运动成功")

    # 启动坐标系运动
    ret = pymtn.NMC_CrdStartMtn(crdHandle)
    if ret != 0:
        print(f"启动坐标系运动失败，错误码: {ret}")
    else:
        print("启动坐标系运动成功")

    # 状态获取#################################
    while 1:

        # 获取坐标系合成速度
        vel, ret = pymtn.NMC_CrdGetVel(crdHandle)
        if ret != 0:
            print(f"读取速度失败，错误码: {ret}")
        else:
            print(f"合成速度: {vel.value}")

        # 读取当前段号
        seg_no, ret = pymtn.NMC_CrdGetUserSegNo(crdHandle)
        if ret != 0:
            print(f"读取段号失败，错误码: {ret}")
        else:
            print(f"当前段号: {seg_no.value}")

        # 读取指令缓冲区空闲长度
        free_space, ret = pymtn.NMC_CrdBufGetFree(crdHandle)
        if ret != 0:
            print(f"读取缓冲区空闲长度失败，错误码: {ret}")
        else:
            print(f"缓冲区空闲长度: {free_space.value}")

        # 读取坐标系状态
        crd_sts, ret = pymtn.NMC_CrdGetSts(crdHandle)
        if ret != 0:
            print(f"读取坐标系状态失败，错误码: {ret}")
        else:
            print(f"坐标系状态字: {crd_sts.value:#06x}")
            if crd_sts.value & pymtn.BIT_CORD_BUSY == 0:
                print("坐标系运动结束")
                break
            if crd_sts.value & pymtn.BIT_CORD_MVERR:
                print("坐标系运动出错，需要复位")
                break
            if crd_sts.value & pymtn.BIT_CORD_EMPTY:
                print("缓冲区为空")
            if crd_sts.value & pymtn.BIT_CORD_FULL:
                print("缓冲区已满")
            if crd_sts.value & pymtn.BIT_CORD_NODATASTOP:
                print("缓冲区空异常停止或急停")
            if crd_sts.value & pymtn.BIT_CORD_SDRAM_HWERR:
                print("插补缓冲区硬件错误")


    return  crdHandle
if __name__ == "__main__":

    devHandle, axisHandle = DevInit()
    crdHandle = Crd()