"""--------------------控制器初始化--------------------"""
from pymtn_lib20 import *
import time

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)

"""--------------------JOG-------------------"""
def _JOG(devHandle, axisHandle):
    print("JOG测试开始")

    # # 一条指令完成jog运动
    # """ 单轴连续速度运动 """
    # acc = 200.0  # 加速度
    # dec = 200.0  # 减速度
    # maxVel = 15.0  # 最大速度
    # smoothCoef = 30  # 平滑系数
    # clrStsFlag = 1  # 运动前清除轴状态
    #
    # ret = pymtn.NMC_MtMoveJog(axisHandle, acc, dec, maxVel, smoothCoef, clrStsFlag)
    # if ret != 0:
    #     print(f"JOG运动失败，错误码: {ret}")
    # else:
    #     print("JOG运动启动成功")
    ######################################################################
    # # 一条指令完成jog运动
    # """ 单轴绝对位置点位运动 """
    # acc = 100.0  # 加速度
    # dec = 100.0  # 减速度
    # startVel = 0.1  # 起跳速度
    # endVel = 0.1  # 终止速度
    # maxVel = 10.0  # 最大速度
    # smoothCoef = 50  # 平滑系数
    # tgtPos = 50000  # 目标位置（脉冲）
    #
    # ret = pymtn.NMC_MtMovePtpAbs(axisHandle, acc, dec, startVel, endVel, maxVel, smoothCoef, tgtPos)
    # if ret != 0:
    #     print(f"绝对位置运动失败，错误码: {ret}")
    # else:
    #     print("绝对位置运动启动成功")
    #####################################################################
    """ 设置单轴规划模式 """
    # 设置为梯形规划模式（PTP模式）
    mode = pymtn.MT_JOG_PRF_MODE  # 梯形规划模式
    ret = pymtn.NMC_MtSetPrfMode(axisHandle, mode)
    if ret != 0:
        print(f"设置规划模式失败，错误码: {ret}")
    else:
        print("设置规划模式成功")

    """ 设置JOG运动参数 """
    jog_para = pymtn.TJogPara()
    jog_para.acc = 200.0  # 加速度
    jog_para.dec = 200.0  # 减速度
    jog_para.smoothCoef = 30.0  # 平滑系数

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

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


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


    while 1:
        ''' 读取电机当前的理论位置'''
        (axisPos, ret) = pymtn.NMC_MtGetAxisPos(axisHandle)
        if ret != 0:
            print("读取机械坐标的位置出错,返回值为: ", ret)
            exit()
        ''' 位置大于20000时，停止运动'''
        if axisPos.value > 20000:
            ret = pymtn.NMC_MtStop(axisHandle)
            print("axisPos位置%d大于20000，停止运动" % axisPos.value)
            if ret != 0:
                print("  停止运动出错,返回值为: ", ret)
                exit()
            break
        else:
            print("axisPos值:%d ,等待" % axisPos.value)
            time.sleep(0.5)

    print("JOG测试结束")

if __name__ == "__main__":
    devHandle, axisHandle = DevInit()
    _JOG(devHandle, axisHandle[0])