
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)

"""--------------------Ext 18.闭环--------------------"""
def Ext_CloseLoopCom(devHandle, axisHandle):
    print("闭环 测试开始")

    dacPara = pymtn.TDacMotor()
    dacPara.inverse = 0
    dacPara.bias = 0
    dacPara.dacLmt = 500
    ret = pymtn.NMC_MtSetCloseLoopDac(axisHandle, dacPara)
    if ret != 0:
        print(f"设置DAC参数失败，错误码: {ret}")
    else:
        print("设置DAC参数成功")


    # 设置编码器为外部编码器
    ret = pymtn.NMC_SetEncMode(devHandle, 0, 0)
    if ret != 0:
        print(f"设置编码器为外部编码器失败，错误码: {ret}")
    else:
        print("设置编码器为外部编码器成功")

    # 设置为闭环控制模式
    ret = pymtn.NMC_MtSetCtrlMode(axisHandle, 1)
    if ret != 0:
        print(f"设置为闭环控制模式失败，错误码: {ret}")
    else:
        print("设置为闭环控制模式成功")


    # 设置闭环最大位置误差必需设置
    ret = pymtn.NMC_MtSetPosErrLmt(axisHandle, 32767)
    if ret != 0:
        print(f"设设置闭环最大位置误差必需设置失败，错误码: {ret}")
    else:
        print("设置闭环最大位置误差必需设置成功")

    pidPara = pymtn.TPidPara()
    pidPara.kp = 5
    pidPara.ki = 5
    # 设置PID参数
    ret = pymtn.NMC_MtSetPIDPara(axisHandle, 0, pidPara)
    if ret != 0:
        print(f"设置PID参数失败，错误码: {ret}")
    else:
        print("设置PID参数成功")

    # 设置PID组号示例
    ret = pymtn.NMC_MtSetPIDIndex(axisHandle, 0)
    if ret != 0:
        print(f"设置PID组号失败，错误码: {ret}")
    else:
        print("设置PID组号成功")

    """ 设置伺服ON """
    ret = pymtn.NMC_MtSetSvOn(axisHandle)
    if ret != 0:
        print(f"设置伺服ON失败，错误码: {ret}")
    else:
        print("设置伺服ON成功")

    print("闭环 测试结束")

if __name__ == "__main__":

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