"""--------------------控制器初始化--------------------"""
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)

"""--------------------PTP--------------------"""
def _Ptp(devHandle, axisHandle):
    print("PTP测试开始")

    """ 设置单轴规划模式 """
    # 设置为梯形规划模式（PTP模式）
    mode = pymtn.MT_PTP_PRF_MODE  # 梯形规划模式
    ret = pymtn.NMC_MtSetPrfMode(axisHandle, mode)
    if ret != 0:
        print(f"设置规划模式失败，错误码: {ret}")
    else:
        print("设置规划模式成功")

    """ 设置PTP运动参数 """
    ptp_para = pymtn.TPtpPara()
    ptp_para.acc = 100.0  # 加速度
    ptp_para.dec = 100.0  # 减速度
    ptp_para.startVel = 0.1  # 起跳速度
    ptp_para.endVel = 0.1  # 终止速度
    ptp_para.smoothCoef = 50  # 平滑系数

    ret = pymtn.NMC_MtSetPtpPara(axisHandle, ptp_para)
    if ret != 0:
        print(f"设置PTP参数失败，错误码: {ret}")
    else:
        print("设置PTP参数成功")


    """ 设置目标运动速度 """
    # 设置PTP模式下的最大速度
    vel = 10.0  # 脉冲/ms
    ret = pymtn.NMC_MtSetVel(axisHandle, vel)
    if ret != 0:
        print(f"设置目标速度失败，错误码: {ret}")
    else:
        print("设置目标速度成功")

    """ 设置目标运动位置 """
    # 设置PTP模式下的目标位置
    pos = 10000  # 脉冲
    ret = pymtn.NMC_MtSetPtpTgtPos(axisHandle, pos)
    if ret != 0:
        print(f"设置目标位置失败，错误码: {ret}")
    else:
        print("设置目标位置成功")

    """ 启动运动 """
    ret = pymtn.NMC_MtUpdate(axisHandle)
    if ret != 0:
        print(f"启动运动失败，错误码: {ret}")
    else:
        print("启动运动成功")

    while 1:
        (axisSts, ret) = pymtn.NMC_MtGetSts(axisHandle)
        if ret != 0:
            print("读当前轴状态出错,返回值为: ", ret)
            exit()
        if (axisSts.value & pymtn.BIT_AXIS_BUSY) == 0:
            print("运动完成")
            break
        ''' 读取电机当前的理论位置'''
        (axisPos , ret)= pymtn.NMC_MtGetAxisPos(axisHandle)
        if ret != 0:
            print("读取机械坐标的位置出错,返回值为: ",ret)
            exit()


    print("PTP测试结束")
if __name__ == "__main__":
    devHandle, axisHandle = DevInit()
    _Ptp(devHandle, axisHandle[0])