
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 _AxisConfig(devHandle, axisHandle):
    print("配置开始")

    # 设置编码器模式
    # 设置轴0的编码器为AB相90度差模式（外部输入）
    encId = 0  # 轴编码器通道
    encMode = 0  # AB相90度差（外部）
    ret = pymtn.NMC_SetEncMode(devHandle, encId, encMode)
    if ret != 0:
        print(f"设置编码器模式失败，错误码: {ret}")
    else:
        print("设置编码器模式成功")

    # 启用软件限位停止功能
    swt = 1  # 1为允许软件限位触发停止
    ret = pymtn.NMC_MtSwLmtOnOff(axisHandle, swt)
    if ret != 0:
        print(f"设置软件限位使能失败，错误码: {ret}")
    else:
        print("设置软件限位使能成功")

    # 设置软件限位数值
    # 设置正向限位为10000脉冲，负向限位为-10000脉冲
    posLmt = 10000
    negLmt = -10000
    ret = pymtn.NMC_MtSwLmtValue(axisHandle, posLmt, negLmt)
    if ret != 0:
        print(f"设置软件限位值失败，错误码: {ret}")
    else:
        print("设置软件限位值成功")

    # 设置轴安全参数
    safePrm = pymtn.TSafePara()
    safePrm.estpDec = 500.0  # 急停减速度
    safePrm.maxVel = 50.0  # 最大速度
    safePrm.maxAcc = 100.0  # 最大加速度

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

    # 设置脉冲输出模式
    # 设置脉冲输出为脉冲+方向模式，不取反
    inv = 0  # 不取反
    mode = 0  # 脉冲+方向模式
    ret = pymtn.NMC_MtSetStepMode(axisHandle, inv, mode)
    if ret != 0:
        print(f"设置脉冲输出模式失败，错误码: {ret}")
    else:
        print("设置脉冲输出模式成功")

    # 设置轴速度滤波参数
    # 设置速度滤波系数为10
    filterCoef = 10
    ret = pymtn.NMC_MtSetAxisVelFilter(axisHandle, filterCoef)
    if ret != 0:
        print(f"设置速度滤波参数失败，错误码: {ret}")
    else:
        print("设置速度滤波参数成功")


    # 硬件限位配置
    # 启用正负限位，高电平触发
    posLmtEnable = 1  # 允许正向限位
    negLmtEnable = 1  # 允许负向限位
    posLmtSns = 0  # 低电平触发
    negLmtSns = 0  # 低电平触发

    ret = pymtn.NMC_MtSetLmtCfg(axisHandle, posLmtEnable, negLmtEnable, posLmtSns, negLmtSns)
    if ret != 0:
        print(f"设置硬件限位配置失败，错误码: {ret}")
    else:
        print("设置硬件限位配置成功")

    # 伺服报警配置
    # 启用伺服报警，低电平触发
    alarmEnable = 1  # 允许伺服报警
    alarmSns = 0  # 低电平触发

    ret = pymtn.NMC_MtSetAlarmCfg(axisHandle, alarmEnable, alarmSns)
    if ret != 0:
        print(f"设置伺服报警配置失败，错误码: {ret}")
    else:
        print("设置伺服报警配置成功")

    # 设置脉冲输出滤波
    # 设置脉冲输出滤波系数为100
    coe = 100
    ret = pymtn.NMC_MtSetStepFilter(axisHandle, coe)
    if ret != 0:
        print(f"设置脉冲输出滤波失败，错误码: {ret}")
    else:
        print("设置脉冲输出滤波成功")

    """ 清除轴错误状态 """
    ret = pymtn.NMC_MtClrError(axisHandle)
    if ret != 0:
        print(f"清除错误状态失败，错误码: {ret}")
    else:
        print("清除错误状态成功")

    print("配置结束")
if __name__ == "__main__":
    devHandle, axisHandle = DevInit()
    _AxisConfig(devHandle, axisHandle[0])