
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()
        ret = pymtn.NMC_MtSetSvOn(axisHandle[i])

    print("控制器初始化结束")
    return (devHandle, axisHandle)

"""--------------------回零--------------------"""
def _Home(devHandle, axisHandle):
    print("回零 测试开始")
    # 设置回零参数
    home_setting = pymtn.THomeSetting()
    home_setting.mode = pymtn.THomeMode.HM_MODE1  # 单原点模式
    home_setting.dir = 1  # 正向搜寻
    home_setting.offset = 0  # 原点偏移
    home_setting.scan1stVel = 100.0  # 基本搜寻速度
    home_setting.scan2ndVel = 10.0  # 低速
    home_setting.acc = 1000.0  # 加速度
    home_setting.reScanEn = 0  # 不进行两次搜寻
    home_setting.homeEdge = 0  # 下降沿触发
    home_setting.lmtEdge = 0  # 下降沿触发
    home_setting.zEdge = 0  # 下降沿触发
    home_setting.iniRetPos = 0  # 起始反向运动距离
    home_setting.retSwOffset = 0  # 反向运动时离开开关距离
    home_setting.safeLen = 0  # 安全距离
    home_setting.usePreSetPtpPara = 0  # 不使用预设点动参数

    # 设置回零参数
    ret = pymtn.NMC_MtSetHomePara(axisHandle, home_setting)
    if ret != 0:
        print(f"设置回零参数失败，错误码: {ret}")
    else:
        print("设置回零参数成功")

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

    # 监控回零状态
    while True:
        stsWord, ret = pymtn.NMC_MtGetHomeSts(axisHandle)
        if ret != 0:
            print(f"读取回零状态失败，错误码: {ret}")
            break

        if stsWord.value & pymtn.BIT_AXHOME_OK:
            print("回零成功")
            break
        elif stsWord.value & pymtn.BIT_AXHOME_FAIL:
            print("回零失败")
            break
        elif stsWord.value & pymtn.BIT_AXHOME_ERR_MV:
            print("回零错误：运动参数出错")
            break
        elif stsWord.value & pymtn.BIT_AXHOME_ERR_SWT:
            print("回零错误：开关没触发")
            break

    # 读取回零误差（可选）
    if stsWord.value & pymtn.BIT_AXHOME_OK:
        errPos = c_long()
        ret = pymtn.NMC_MtGetHomeError(axisHandle, errPos)
        if ret != 0:
            print(f"读取回零误差失败，错误码: {ret}")
        else:
            print(f"回零位置偏差: {errPos.value}")


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