指令流激光能量跟随双表模式

指令流激光能量跟随双表模式例程

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

#include "stdafx.h"

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

// 测试功能：激光能量跟对功能示例，双表模式，输出能量为pwm的占空比和频率
// 测试平台：网络型运动控制器
// 测试环境：Windows
// 测试流程：
//           （1）初始化控制器
//           （2）Group初始化
//           （3）压指令流数据
//           （4）启动指令流，等待指令流执行完成
//           （5）执行完成，关闭控制器
// 注意事项：
//           （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 初始化一个三轴正交模型的group
 * @param core 需要初始化的核号，从1开始
 * @param axis 需要初始化的轴起始索引，从1开始
 * @param axisCount 需要初始化的轴数量，从起始索引axis开始计数，必须大于0
 * @param group 需要初始化的group号，从1开始
 * @return 0表示初始化成功，非0表示初始化失败
*/
short InitGroup(short core,short axis,short axisCount,short group)
{
    short rtn;
    short i;
    TScaleParameter scaleParameter;
    memset(&scaleParameter,0,sizeof(scaleParameter));
    TAxisMotionConstraint axisMotionConstraint;
    memset(&axisMotionConstraint,0,sizeof(axisMotionConstraint));

    // 设置轴当量
    scaleParameter.count = 1;
    scaleParameter.alpha[0] = 1;
    scaleParameter.beta[0] = 1000;

    // 设置轴运动约束
    axisMotionConstraint.velMax = 2000;
    axisMotionConstraint.accMax = 5000;
    axisMotionConstraint.decMax = 5000;
    axisMotionConstraint.jerkMax = 1000000;
    axisMotionConstraint.dvMax = 10;
    axisMotionConstraint.reverseLimitMode = 0;

    for (i=axis;i<(axis+axisCount);++i)
    {
        rtn = GTN_SetScaleParameter(core,MC_PROFILE,i,&scaleParameter,0);
        if ( 0 != rtn )
        {
            return CommandHandler("GTN_SetScaleParameter",rtn);
        }

        rtn = GTN_SetAxisMotionConstraint(core,i,&axisMotionConstraint,0);
        if ( 0 != rtn )
        {
            return CommandHandler("GTN_SetAxisMotionConstraint",rtn);
        }
    }

    TGroupMotionConstraint groupMotionConstraint;
    memset(&groupMotionConstraint,0,sizeof(groupMotionConstraint));
    TVelProfileMode velProfileMode;
    memset(&velProfileMode,0,sizeof(velProfileMode));
    TGroupStopParameter stopPrm;
    memset(&stopPrm,0,sizeof(stopPrm));
    TGroupStopParameter abruptStopPrm;
    memset(&abruptStopPrm,0,sizeof(abruptStopPrm));
    TKinematicTransform kinematicTransform;
    memset(&kinematicTransform,0,sizeof(TKinematicTransform));

    // 设置group运动约束
    groupMotionConstraint.velMax = 500;
    groupMotionConstraint.accMax = 1000;
    groupMotionConstraint.decMax = 1000;

    // 设置group速度规划模式
    velProfileMode.mode = VEL_PROFILE_MODE_SMOOTH;
    velProfileMode.parameter.smooth.accTime = 40;
    velProfileMode.parameter.smooth.k = 0;

    // 设置group平滑停止参数
    stopPrm.deceleration = 2000;
    stopPrm.jerk = 4000;

    // 设置group紧急停止参数
    abruptStopPrm.deceleration = 2000;
    abruptStopPrm.jerk = 5000;

    // 设置group运动学模型
    kinematicTransform.type = KIN_TYPE_ORTHOGONAL;

    // 建立三维正交模型group
    for (i=1;i<=3;++i)
    {
        rtn = GTN_AddAxisToGroup(core,group,i+axis,i);
        if ( 0 != rtn )
        {
            return CommandHandler("GTN_AddAxisToGroup",rtn);
        }
    }

    rtn = GTN_SetGroupMotionConstraint(core,group,&groupMotionConstraint);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_SetGroupMotionConstraint",rtn);
    }

    rtn = GTN_SetGroupVelProfileMode(core,group,&velProfileMode);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_SetGroupVelProfileMode",rtn);
    }

    rtn = GTN_SetGroupStopParameter(core,group,0,&stopPrm);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_SetGroupStopParameter",rtn);
    }

    rtn = GTN_SetGroupStopParameter(core,group,1,&abruptStopPrm);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_SetGroupStopParameter",rtn);
    }

    rtn = GTN_SetGroupKinematicTransform(core,group,&kinematicTransform,0);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_SetGroupKinematicTransform",rtn);
    }

    rtn = GTN_GroupEnable(core,group);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_GroupEnable",rtn);
    }
    printf("Init Group Success !\n");

    return rtn;
}

/**
 * @brief 获取指令流执行状态
 * @param core 核号，从1开始
 * @param list 指令流号，从1开始
 * @param group group号，从1开始
 * @param pExecute 获取的指令流执行状态
 * @return 0表示获取成功，非0表示获取失败
*/
short GetListExecuteInfo(short core,short list,short group,short *pExecute)
{
    short rtn;
    TCommandListStatus listStatus;
    TGroupStatus groupStatus;
    double acsPos[8];

    rtn = GTN_GetCommandListStatus(core,list,&listStatus);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_GetCommandListStatus",rtn);
    }

    rtn = GTN_GetGroupStatus(core,group,&groupStatus);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_GetGroupStatus",rtn);
    }

    rtn = GTN_GetGroupProfilePos(core,group,1,acsPos,8,COORD_SYSTEM_ACS);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_GetGroupProfilePos",rtn);
    }

    printf("List: Execute = %d, Remainder = %d, acsPos[0] = %lf, acsPos[1] = %lf\r",
        listStatus.execute,listStatus.remainderSegCount,acsPos[0],acsPos[1]);

    *pExecute = listStatus.execute;

    if ( 0 == listStatus.execute )
    {
        if ( 0 == listStatus.remainderSegCount )
        {
            printf("\nList Execute Success !\n");
        }
        else
        {
            printf("\nList Execute Error !\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 group;                       // group号
    short dir[8]={0,0,0,0,0,0,0,0};    // group方向参数
    double pos[8]={0,0,0,0,0,0,0,0};   // group位置参数
    TGroupMoveParameter groupMoveParameter;
    memset(&groupMoveParameter,0,sizeof(groupMoveParameter));

    // 初始化group
    group = 1;
    groupMoveParameter.velocity = 10;
    groupMoveParameter.acceleration = 500;
    groupMoveParameter.deceleration = 500;
    groupMoveParameter.overrideSelect = 0;
    rtn = InitGroup(core,axis,axisCount,group);;
    if ( 0 != rtn )
    {
        return rtn;
    }

    short list;                        // 指令流号
    TListInfo listInfo;
    memset(&listInfo,0,sizeof(listInfo));

    // 初始化指令流
    list = 1;
    listInfo.list = list;
    listInfo.modal = 0;
    listInfo.segNum = 0;
    rtn = GTN_ClearCommandListData(core,list,0);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_ClearCommandListData",rtn);
    }

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

    short laserChannel;                // 激光通道号
    double laserOnDelay;               // 激光开光延时
    double laserOffDelay;              // 激光关光延时
    TLaserPwmPrmPro laserPwmPrmPro;    // 激光pwm信号能量值限制参数
    TLaserFollowDuoTablePrmPro laserFollowDuoTablePrmPro;

    long i;
    short tableId;                     // 能量跟随表1表号
    short tableId2;                    // 能量跟随表2表号
    long tableCount;                   // 能量跟随表大小
    double synVel[100];                // 能量跟随表中的合成速度
    double power[100];                 // 能量跟随表1中的能量
    double power2[100];                // 能量跟随表2中的能量

    laserChannel = 0;
    laserPwmPrmPro.minDuty = 0;
    laserPwmPrmPro.maxDuty = 80;
    laserPwmPrmPro.minFrequency = 0;
    laserPwmPrmPro.maxFrequency = 50;
    laserPwmPrmPro.minPulseWidth = 0;
    laserPwmPrmPro.maxPulseWidth = 20;

    laserOnDelay = 0;
    laserOffDelay = 100;

    tableId = 1;
    tableId2 = 2;
    tableCount = 100;
    for (i=0;i<tableCount;++i)
    {
        synVel[i] = i;

        if ( i <= 20 )
        {
            power[i] = 20;
        }
        else if ( i <= 40 )
        {
            power[i] = 40;
        }
        else if ( i <= 60 )
        {
            power[i] = 60;
        }
        else if ( i <= 80 )
        {
            power[i] = 80;
        }
        else
        {
            power[i] = 100;
        }

        power2[i] = i;
    }
    laserFollowDuoTablePrmPro.group = group;
    laserFollowDuoTablePrmPro.source = LASER_FOLLOW_SYNCH_VEL_SOURCE_PROFILE;
    laserFollowDuoTablePrmPro.coordSystem = COORD_SYSTEM_ACS;
    laserFollowDuoTablePrmPro.dutyTableId = tableId;
    laserFollowDuoTablePrmPro.frequencyTableId = tableId2;
    laserFollowDuoTablePrmPro.minDuty = 0;
    laserFollowDuoTablePrmPro.maxDuty = 100;
    laserFollowDuoTablePrmPro.minFrequency = 5;
    laserFollowDuoTablePrmPro.maxFrequency = 100;

    // 设置能量跟随表
    rtn = GTN_ClearLaserFollowTablePro(core,laserChannel,tableId);
    rtn += GTN_SetLaserFollowTablePro(core,laserChannel,tableId,tableCount,synVel,power);
    rtn += GTN_ClearLaserFollowTablePro(core,laserChannel,tableId2);
    rtn += GTN_SetLaserFollowTablePro(core,laserChannel,tableId2,tableCount,synVel,power2);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_SetLaserFollowTablePro",rtn);
    }

    // 压指令流数据
    pos[0] = 10;
    pos[1] = 0;
    listInfo.modal = 1;
    ++ listInfo.segNum;
    rtn = GTN_MoveLinearAbsolute(core,group,pos,dir,&groupMoveParameter,&listInfo);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_MoveLinearAbsolute",rtn);
    }

    listInfo.modal = 0;
    ++ listInfo.segNum;
    rtn = GTN_SetLaserPwmPrmPro(core,laserChannel,&laserPwmPrmPro,&listInfo);
    ++ listInfo.segNum;
    rtn += GTN_SetLaserDelayPro(core,laserChannel,laserOnDelay,laserOffDelay,&listInfo);
    ++ listInfo.segNum;
    rtn += GTN_SetLaserFollowDuoTablePrmPro(core,laserChannel,&laserFollowDuoTablePrmPro,&listInfo);
    ++ listInfo.segNum;
    rtn += GTN_SetLaserFollowEnablePro(core,laserChannel,1,&listInfo);
    ++ listInfo.segNum;
    rtn += GTN_SetLaserEnablePro(core,laserChannel,1,LASER_ON_MODE_DEFAULT,&listInfo);
    if ( 0 != rtn )
    {
        return CommandHandler("SetLaserPrm",rtn);
    }

    pos[0] = 20;
    pos[1] = 0;
    listInfo.modal = 1;
    ++ listInfo.segNum;
    rtn = GTN_MoveLinearAbsolute(core,group,pos,dir,&groupMoveParameter,&listInfo);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_MoveLinearAbsolute",rtn);
    }

    pos[0] = 20;
    pos[1] = 10;
    listInfo.modal = 1;
    ++ listInfo.segNum;
    rtn = GTN_MoveLinearAbsolute(core,group,pos,dir,&groupMoveParameter,&listInfo);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_MoveLinearAbsolute",rtn);
    }

    pos[0] = 10;
    pos[1] = 10;
    listInfo.modal = 1;
    ++ listInfo.segNum;
    rtn = GTN_MoveLinearAbsolute(core,group,pos,dir,&groupMoveParameter,&listInfo);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_MoveLinearAbsolute",rtn);
    }

    pos[0] = 10;
    pos[1] = 0;
    listInfo.modal = 1;
    ++ listInfo.segNum;
    rtn = GTN_MoveLinearAbsolute(core,group,pos,dir,&groupMoveParameter,&listInfo);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_MoveLinearAbsolute",rtn);
    }

    listInfo.modal = 0;
    ++ listInfo.segNum;
    rtn = GTN_SetLaserFollowEnablePro(core,laserChannel,0,&listInfo);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_SetLaserFollowEnablePro",rtn);
    }

    ++ listInfo.segNum;
    rtn = GTN_SetLaserEnablePro(core,laserChannel,0,LASER_OFF_MODE_POWER_OFF,&listInfo);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_SetLaserEnablePro",rtn);
    }

    pos[0] = 0;
    pos[1] = 0;
    listInfo.modal = 1;
    ++ listInfo.segNum;
    rtn = GTN_MoveLinearAbsolute(core,group,pos,dir,&groupMoveParameter,&listInfo);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_MoveLinearAbsolute",rtn);
    }

    rtn = GTN_CommandListDataEnd(core,list);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_CommandListDataEnd",rtn);
    }
    printf("Push List Data Finish !\n");

    rtn = GTN_StartCommandList(core,listInfo.list);
    if ( 0 != rtn )
    {
        return CommandHandler("GTN_StartCommandList",rtn);
    }
    printf("Start List !\n");

    short execute;
    execute = 0;
    do 
    {
        rtn = GetListExecuteInfo(core,listInfo.list,group,&execute);
        if ( 0 != rtn )
        {
            return rtn;
        }
    } while ( 1 == execute );

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

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

    return 0;
}