指令流中轴和振镜组合运动例程

指令流中轴和振镜组合运动例程

// ScanStartInList.cpp : 定义控制台应用程序的入口点。
//

#include "stdafx.h"

// 该例程仅用于功能演示，请保证安全的情况下使用

// 测试功能：在指令流中启动振镜，实现振镜和轴的组合运动
// 测试平台：网络型运动控制器
// 测试环境：Windows
// 测试流程：
//           （1）初始化控制器
//           （2）压振镜数据
//           （3）压运动数据
//           （4）启动指令流
//           （5）检测指令流是否执行完成
//           （6）执行完成，关闭控制器
// 注意事项：
//           （1）本例程使用的“例程专用.xml”、“例程专用.cfg”，仅用于本例程
//           （2）实际使用时，需要使用MotionStudio生成网络配置xml
//           （3）实际使用时，必须确认网络上接了支持振镜功能的从站！！！

// 加载固高运动控制库头文件
#include "gxn.h"
// 动态加载固高运动控制gxn.lib库
#pragma comment(lib,"gxn.lib")

/**
 * @brief 指令出错打印函数
 * @param command 打印信息字符串
 * @param error 错误码
 * @return 错误码
*/
short CommandHandler(char* command, short error)
{
    printf("%s = %d\n", command, error);
    getchar();

    return error;
}

/**
 * @brief 初始化运动控制器（开卡 + 初始化网络拓扑 + 初始化轴）
 * @param core 需要初始化的核号，从1开始
 * @param axis 需要初始化的轴起始索引，从1开始
 * @param axisCount 需要初始化的轴数量，从起始索引axis开始计数，必须大于0
 * @return 0表示初始化成功，非0表示初始化失败
*/
short InitMc(short core,short axis,short axisCount)
{
    short rtn;
    short overTime;
    long status;

    // 打开运动控制器
    rtn = GTN_OpenCard(CHANNEL_PCIE,NULL,NULL);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_OpenCard",rtn);
    }
    printf("Open Card Success !\n");

    // 初始化网络
    // 注意：（1）“例程专用.xml”仅用于本例程
    //       （2）实际使用时，需要使用MotionStudio生成对应的网络配置文件
    // overTime：网络初始化超时时间，单位：秒
    overTime = 120;
    rtn = GTN_NetInit(NET_INIT_MODE_XML_STRICT,"例程专用.xml",overTime,&status);
    if ( 0 != rtn )
    {
        printf("status = %d\n",status);
        return CommandHandler("GTN_NetInit",rtn);
    }
    printf("Init Net Success !\n");

    // 加载配置文件到控制器
    // 注意：（1）“例程专用.cfg”仅用于本例程
    //       （2）实际使用时，需要使用MotionStudio生成对应的配置文件
    rtn = GTN_LoadConfig(core,"例程专用.cfg");
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_LoadConfig(\"例程专用.cfg\")",rtn);
    }

    // 清除轴状态
    rtn = GTN_ClrSts(core,axis,axisCount);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_ClrSts",rtn);
    }
    printf("Init Mc Config Success !\n");

    return rtn;
}

/**
 * @brief 压振镜运动数据
 * @param core 需要初始化的核号，从1开始
 * @param scanList 振镜数据需要压的指令号
 * @param scan 振镜坐标系号
 * @return 0表示压数据成功，非0表示压数据失败
*/
short PushScanData(short core,short scanList,short scan)
{
    short rtn;
    TListInfo listInfo;
    memset(&listInfo,0,sizeof(listInfo));
    TScanLinearMotionPro linearMotion;
    memset(&linearMotion,0,sizeof(TScanLinearMotionPro));

    listInfo.list = scanList;
    listInfo.segNum += 1;
    rtn = GTN_ScanInitPro(core,scan,0,&listInfo);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_ScanInitPro",rtn);
    }

    rtn = GTN_ClearCommandListData(core,listInfo.list,0);
    if( 0 != rtn )
    {
        return CommandHandler("GTN_ClearCommandListData",rtn);
    }

    linearMotion.motionPrm.velMode.vel = 100;
    linearMotion.motionPrm.velMode.acc = 100;
    linearMotion.motionPrm.velMode.dec = 100;
    linearMotion.pos[0] = 30000;
    linearMotion.pos[1] = 30000;
    linearMotion.pos[2] = 0;
    listInfo.segNum += 1;
    rtn  = GTN_ScanLinearPro(core,scan,SCAN_MOTION_MODE_JUMP,&linearMotion,&listInfo);
    linearMotion.pos[0] = -30000;
    linearMotion.pos[1] = 30000;
    linearMotion.pos[2] = 0;
    listInfo.segNum += 1;
    rtn += GTN_ScanLinearPro(core,scan,SCAN_MOTION_MODE_JUMP,&linearMotion,&listInfo);
    linearMotion.pos[0] = 0;
    linearMotion.pos[1] = 0;
    linearMotion.pos[2] = 0;
    listInfo.segNum += 1;
    rtn += GTN_ScanLinearPro(core,scan,SCAN_MOTION_MODE_JUMP,&linearMotion,&listInfo);
    if( 0 != rtn )
    {
        return CommandHandler("GTN_ScanLinearPro",rtn);
    }

    do 
    {
        rtn = GTN_CommandListDataEnd(core,listInfo.list);
    } while ( 1 == rtn );

    printf("Push Scan Data Success !\n");

    return 0;
}

/**
 * @brief 压运动数据
 * @param core 需要初始化的核号，从1开始
 * @param list 运动数据需要压的指令号
 * @param profile 运动轴轴号
 * @param scan 配合轴运动的振镜坐标系号
 * @return 0表示压数据成功，非0表示压数据失败
*/
short PushMotionData(short core,short list,short profile,short scan)
{
    short rtn;
    TListInfo listInfo;
    memset(&listInfo,0,sizeof(listInfo));
    TMoveContinuousAbsolutePrm moveContinuousAbsolutePrm;
    memset(&moveContinuousAbsolutePrm,0,sizeof(moveContinuousAbsolutePrm));
    TWatchConditionVar watchConditionVar;
    TWaitTimeout waitTimeout;
    TWatchVar waitResultVar;
    memset(&watchConditionVar,0,sizeof(watchConditionVar));
    memset(&waitTimeout,0,sizeof(waitTimeout));
    memset(&waitResultVar,0,sizeof(waitResultVar));

    listInfo.list = list;

    rtn = GTN_ClearCommandListData(core,listInfo.list,0);
    if( 0 != rtn )
    {
        return CommandHandler("GTN_ClearCommandListData",rtn);
    }

    // 轴运动到指定位置
    moveContinuousAbsolutePrm.pos = 5;
    moveContinuousAbsolutePrm.vel = 10;
    moveContinuousAbsolutePrm.velEnd = 0;
    moveContinuousAbsolutePrm.acc = 500;
    moveContinuousAbsolutePrm.dec = 500;
    moveContinuousAbsolutePrm.direction = 0;
    moveContinuousAbsolutePrm.overrideSelect = 0;
    moveContinuousAbsolutePrm.velProfileMode = 1;

    listInfo.modal = 1;
    listInfo.segNum += 1;
    rtn = GTN_MoveContinuousAbsolute(core,profile,&moveContinuousAbsolutePrm,&listInfo,0);
    if( 0 != rtn )
    {
        return CommandHandler("GTN_MoveContinuousAbsolute",rtn);
    }

    // 启动振镜运动
    listInfo.modal = 0;
    listInfo.reserve1[1] = SCAN_START_IN_LIST_MODE_CHECK;
    listInfo.segNum += 1;
    rtn = GTN_ScanCrdStartPro(core,scan,&listInfo);
    if( 0 != rtn )
    {
        return CommandHandler("GTN_ScanCrdStartPro",rtn);
    }

    // 振镜运动同时，轴也继续运动
    moveContinuousAbsolutePrm.pos += 10;
    listInfo.modal = 1;
    listInfo.segNum += 1;
    rtn = GTN_MoveContinuousAbsolute(core,profile,&moveContinuousAbsolutePrm,&listInfo,0);
    if( 0 != rtn )
    {
        return CommandHandler("GTN_MoveContinuousAbsolute",rtn);
    }

    // 等待振镜运动完成
    watchConditionVar.var.type = WATCH_VAR_SCAN_RUN;
    watchConditionVar.var.index = scan;
    watchConditionVar.var.id = 1;
    watchConditionVar.condition = WATCH_CONDITION_EQ;
    watchConditionVar.value = 0;

    waitTimeout.mode = WAIT_TIMEOUT_MODE_STOP;
    waitTimeout.time = 10000;

    listInfo.modal = 1;
    listInfo.segNum += 1;
    rtn += GTN_WaitForConditionVar(core,&watchConditionVar,1,0,&waitTimeout,&waitResultVar,&listInfo);
    if( CMD_SUCCESS != rtn )
    {
        return CommandHandler("GTN_WaitForConditionVar",rtn);
    }

    // 振镜运动完成后，轴再继续运动
    moveContinuousAbsolutePrm.pos += 5;
    listInfo.modal = 1;
    listInfo.segNum += 1;
    rtn = GTN_MoveContinuousAbsolute(core,profile,&moveContinuousAbsolutePrm,&listInfo,0);
    if( 0 != rtn )
    {
        return CommandHandler("GTN_MoveContinuousAbsolute",rtn);
    }

    // 将所有数据都压入控制器的指令流缓冲区
    do 
    {
        rtn = GTN_CommandListDataEnd(core,listInfo.list);
    } while ( 11700 == rtn );

    printf("Push Motion Data Success !\n");

    return 0;
}

int _tmain(int argc, _TCHAR* argv[])
{
    short rtn;                         // 指令返回值
    short core;                        // 需要执行例程的运动控制器核号
    short axis;                        // 需要初始化的轴起始索引号
    short axisCount;                   // 需要初始化的轴数量，从轴起始索引号开始算起

    // 初始化运动控制器
    // 开卡 + 初始化网络拓扑 + 初始化核1的1-8轴
    core = 1;
    axis = 1;
    axisCount = 8;
    rtn = InitMc(core,axis,axisCount);
    if ( 0 != rtn )
    {
        return rtn;
    }

    short list;                        // 压轴运动数据的指令流
    short scanList;                    // 压振镜运动数据的指令流
    short profile;                     // 参与运动的轴号
    short scan;                        // 振镜坐标系号

    list = 1;
    scanList = 2;
    profile = 1;
    scan = 1;

    // 压振镜数据
    rtn = PushScanData(core,scanList,scan);
    if ( 0 != rtn )
    {
        return rtn;
    }

    // 压运动数据
    rtn = PushMotionData(core,list,profile,scan);
    if ( 0 != rtn )
    {
        return rtn;
    }

    // 启动指令流
    rtn = GTN_StartCommandList(core,list,0);
    if( 0 != rtn )
    {
        return CommandHandler("GTN_StartCommandList",rtn);
    }

    double prfPos;
    short scanPos[2];
    short scanRun,cmdId;
    TCommandListStatus listSts;
    do 
    {
        rtn = GTN_GetPrfPos(core,profile,&prfPos,1,0);
        rtn += GTN_ScanGetCrdPos(core,scanPos,scan);
        rtn += GTN_ScanCrdStatus(core,&scanRun,&cmdId,scan);
        rtn += GTN_GetCommandListStatus(core,list,&listSts);
        if ( 0 != rtn )
        {
            return CommandHandler("GTN_GetCommandListStatus",rtn);
        }

        printf("List%d execute:%d remainderCount:%ld: Scan%d run:%d x:%d y:%d; Profile%d pos:%lf\r",
            list,listSts.execute,listSts.remainderSegCount,
            scan,scanRun,scanPos[0],scanPos[1],
            profile,prfPos);
    } while ( 1 == listSts.execute );

    printf("List Execute Done !\n");

    // 停止指令流
    rtn  = GTN_StopCommandList(core,list,0,0);
    rtn += GTN_StopCommandList(core,scanList,0,0);
    if( 0 != rtn )
    {
        return CommandHandler("GTN_StopCommandList",rtn);
    }

    // 关闭控制器
    rtn = GTN_Close();
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_Close",rtn);
    }

    printf("Press Any Key To Exit !\n");
    getchar();

    return 0;
}