电子凸轮双FIFO运动例程

电子凸轮双FIFO运动例程

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

#include "stdafx.h"
#include "conio.h"

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

// 测试功能：Follow双FIFO切换功能
// 测试平台：网络型运动控制器
// 测试环境：Windows
// 测试流程：
//           （1）初始化控制器
//           （2）将主轴切换为jog运动模式，设置主轴运动参数，将从轴设置为Follow跟随运动模式，启动主轴运动
//                从轴在调用指令GTN_FollowStart后立即启动跟随。
//                从轴在运动时更换跟随策略，其速度规划经过一个过渡的数据段，然后变成一个新的梯形曲线，并且无限次循环
//           （3）运动完成，关闭控制器
// 注意事项：
//           （1）本例程使用的“例程专用.xml”、“例程专用.cfg”，仅用于本例程
//           （2）实际使用时，需要使用MotionStudio生成网络配置xml

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

#define STAGE_FIFO0             1
#define STAGE_TO_FIFO1          2
#define STAGE_TO_FIFO0          3
#define STAGE_END               4

/**
 * @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;
}

int _tmain(int argc, _TCHAR* argv[])
{
    short rtn;                          // 指令返回值
    short core;                         // 需要执行例程的运动控制器核号
    short axis;                         // 需要初始化的轴起始索引号
    short axisCount;                    // 需要初始化的轴数量，从轴起始索引号开始算起
    long axisSts[2];                    // 轴状态
    unsigned long clock;                // 读取的控制器时钟
    double prfVel[2],prfPos[2];         // 读取的规划速度和位置
    TJogPrm jogPrm;                     // Jog运动参数
    double targetVel;                   // 目标速度
    short updateVelFlag=1;              // 更新速度的标致
    short stopFlag=1;                   // 更新速度的标致
    long mask,stopOption;

    short masterAxis = 1;               // 主轴索引
    short slaveAxis = 2;                // 从轴索引
    short masterType;                   // 主轴类型。
    short masterItem=0;                 // 输出位置类型，当masterType=GEAR_MASTER_AXIS(3)时起作用。
    short masterDir = 1;
    long masterPos;
    double slavePos;
    short fifo0 = 0,fifo1 = 1;
    short space,stage,pressKey; 
    long loop;                          // 循环次数，0表示一直循环
    short moveDir;                      // 穿越方向
    long crossPos;                      // 穿越位置
    short percent;
    short segType;

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

    // 轴上使能，如果需要测试实际驱动器、电机的运动，请将此注释代码打开

    /*
    rtn = GTN_AxisOn(core,masterAxis);
    if(CMD_SUCCESS != rtn)
    {
        return CommandHandler("GTN_AxisOn",rtn);
    }

    rtn = GTN_AxisOn(core,slaveAxis);
    if(CMD_SUCCESS != rtn)
    {
        return CommandHandler("GTN_AxisOn",rtn);
    }
    */

    // 将主轴设置为Jog运动模式
    rtn = GTN_PrfJog(core,masterAxis);
    if ( CMD_SUCCESS != rtn )
    {
        return CommandHandler("GTN_PrfJog",rtn);
    }

    // 读取Jog运动参数(需要读取全部运动参数到上位机变量)
    memset(&jogPrm,0,sizeof(jogPrm));
    rtn = GTN_GetJogPrm(core,masterAxis,&jogPrm);
    if ( CMD_SUCCESS != rtn )
    {
        return CommandHandler("GTN_GetJogPrm",rtn);
    }

    // 设置需要修改的运动参数
    jogPrm.acc = 1;
    jogPrm.dec = 1;
    // 设置Jog运动参数
    rtn = GTN_SetJogPrm(core,masterAxis,&jogPrm);
    if ( CMD_SUCCESS != rtn )
    {
        return CommandHandler("GTN_SetJogPrm",rtn);
    }

    // 设置主轴的目标速度
    targetVel = 50;
    rtn = GTN_SetVel(core,masterAxis,targetVel);
    if ( CMD_SUCCESS != rtn )
    {
        return CommandHandler("GTN_SetVel",rtn);
    }

    // 只启动主轴的运动
    axisCount = 1;
    rtn = GTN_UpdatePro(core,&masterAxis,axisCount);
    if ( CMD_SUCCESS != rtn )
    {
        return CommandHandler("GTN_UpdatePro",rtn);
    }

    // 将从轴设为Follow运动模式
    rtn = GTN_PrfFollowEx(core,slaveAxis);
    if(CMD_SUCCESS != rtn)
    {
        return CommandHandler("GTN_PrfFollowEx",rtn);
    }

    // 清空从轴FIFO
    rtn = GTN_FollowClearEx(core,slaveAxis,fifo0);
    if(CMD_SUCCESS != rtn)
    {
        return CommandHandler("GTN_FollowClearEx",rtn);
    }

    // 清空从轴FIFO0
    rtn = GTN_FollowClearEx(core,slaveAxis,fifo0);
    if(CMD_SUCCESS != rtn)
    {
        return CommandHandler("GTN_FollowClearEx",rtn);
    }

    // 清空从轴FIFO1
    rtn = GTN_FollowClearEx(core,slaveAxis,fifo1);
    if(CMD_SUCCESS != rtn)
    {
        return CommandHandler("GTN_FollowClearEx",rtn);
    }

    // 设置主轴，默认跟随主轴规划位置
    masterType = FOLLOW_MASTER_PROFILE;
    rtn = GTN_SetFollowMasterEx(core,slaveAxis, masterAxis,masterType,masterItem);
    if(CMD_SUCCESS != rtn)
    {
        return CommandHandler("GTN_SetFollowMasterEx",rtn);
    }

    // 查询Follow模式FIFO0的剩余空间
    rtn = GTN_FollowSpaceEx(core,slaveAxis,&space,fifo0);
    if(CMD_SUCCESS != rtn)
    {
        return CommandHandler("GTN_FollowSpaceEx",rtn);
    }

    // 查询Follow模式FIFO1的剩余空间
    rtn = GTN_FollowSpaceEx(core,slaveAxis,&space,fifo1);
    if(CMD_SUCCESS != rtn)
    {
        return CommandHandler("GTN_FollowSpaceEx",rtn);
    }

    percent = 100;                 // S曲线
    fifo0 = 0;
    segType = FOLLOW_SEGMENT_NORMAL;

    // 1、向FIFO0压入过渡的数据段
    // 向FIFO中增加运动数据
    masterPos = 20000;slavePos = 10000;
    rtn = GTN_FollowDataPercentEx(core,slaveAxis,masterPos,slavePos,segType,percent,fifo0);
    if(CMD_SUCCESS != rtn)
    {
        return CommandHandler("GTN_FollowDataPercentEx",rtn);
    }

    // 向FIFO中增加运动数据
    masterPos+= 20000;slavePos += 20000;
    rtn = GTN_FollowDataPercentEx(core,slaveAxis,masterPos,slavePos,segType,percent,fifo0);
    if(CMD_SUCCESS != rtn)
    {
        return CommandHandler("GTN_FollowDataPercentEx",rtn);
    }

    // 向FIFO中增加运动数据
    masterPos += 20000;slavePos += 10000;
    rtn = GTN_FollowDataPercentEx(core,slaveAxis,masterPos,slavePos,segType,percent,fifo0);
    if(CMD_SUCCESS != rtn)
    {
        return CommandHandler("GTN_FollowDataPercentEx",rtn);
    }

    // 设置循环次数,0为无限循环
    loop = 0;
    rtn = GTN_SetFollowLoopEx(core,slaveAxis,loop);
    if(CMD_SUCCESS != rtn)
    {
        return CommandHandler("GTN_SetFollowLoopEx",rtn);
    }

    // 启动时间类型为FOLLOW_EVENT_START，调用GTN_FollowStartEx以后立即启动,moveDir、crossPos在该类型下无效
    moveDir = 1;
    crossPos = 50000;
    rtn = GTN_SetFollowEventEx(core,slaveAxis, FOLLOW_EVENT_START, moveDir, crossPos);
    if(CMD_SUCCESS != rtn)
    {
        return CommandHandler("GTN_SetFollowEventEx",rtn);
    }

    // 启动从轴Follow运动
    rtn = GTN_FollowStartEx(core,1<<(slaveAxis-1));
    if(CMD_SUCCESS != rtn)
    {
        return CommandHandler("GTN_FollowStartEx",rtn);
    }    

    pressKey = 0;
    axisCount = 2;
    stage = STAGE_FIFO0;
    while(1)
    {
        // 检查是否有按键
        if(kbhit())
        {
            getch();
            pressKey = 1;
        } 

        // 如果当前运行FIFO0中的数据并且检测到按键，则切换运行FIFO1中的数据
        if( STAGE_FIFO0 == stage )
        {
            if( 1 == pressKey )
            {
                pressKey = 0;
                // 准备切换到FIFO1运行
                stage = STAGE_TO_FIFO1;
                // 清空从轴FIFO1的数据
                rtn =GTN_FollowClearEx(core,slaveAxis,fifo1);
                if(CMD_SUCCESS != rtn)
                {
                    return CommandHandler("GTN_FollowClearEx",rtn);
                }

                // 向FIF1中增加运动数据
                segType = FOLLOW_SEGMENT_CONTINUE;
                masterPos = 20000;slavePos = 10000;
                rtn = GTN_FollowDataPercentEx(core,slaveAxis,masterPos,slavePos,segType,percent,fifo1);
                if(CMD_SUCCESS != rtn)
                {
                    return CommandHandler("GTN_FollowDataPercentEx",rtn);
                }

                // 向FIF1中增加运动数据
                segType = FOLLOW_SEGMENT_NORMAL;
                masterPos+= 20000;slavePos += 20000;
                rtn = GTN_FollowDataPercentEx(core,slaveAxis,masterPos,slavePos,segType,percent,fifo1);
                if(CMD_SUCCESS != rtn)
                {
                    return CommandHandler("GTN_FollowDataPercentEx",rtn);
                }

                // 向FIF1中增加运动数据
                segType = FOLLOW_SEGMENT_NORMAL;
                masterPos += 20000;slavePos += 16000;
                rtn = GTN_FollowDataPercentEx(core,slaveAxis,masterPos,slavePos,segType,percent,fifo1);
                if(CMD_SUCCESS != rtn)
                {
                    return CommandHandler("GTN_FollowDataPercentEx",rtn);
                }

                // 切换到FIFO1
                // 当前工作中的FIFO0的数据遍历完以后才会切换FIFO1
                rtn = GTN_FollowSwitchEx(core,1<<(slaveAxis-1));
                if(CMD_SUCCESS != rtn)
                {
                    return CommandHandler("GTN_FollowSwitchEx",rtn);
                }
            }
        }

        // 检查FIFO0中的数据段是否执行完成，如果已执行完成，则将新的速度规划传入FIFO0中，
        // FIFO1中的数据执行完成后，立即切换运行FIFO0中的数据。
        if( STAGE_TO_FIFO1 == stage )
        {
            // 查询FIFO0的剩余空间
            rtn = GTN_FollowSpaceEx(core,slaveAxis,&space,fifo0);
            if(CMD_SUCCESS != rtn)
            {
                return CommandHandler("GTN_FollowSpaceEx",rtn);
            }

            // 如果FIFO0执行结束，space = 16,说明已经切换到FIFO1
            if( 16 == space )
            {
                // 准备切换到FIFO0运行
                stage = STAGE_TO_FIFO0;

                // 向FIF0中增加运动数据
                segType = FOLLOW_SEGMENT_CONTINUE;
                masterPos = 20000;slavePos = 16000;
                rtn = GTN_FollowDataPercentEx(core,slaveAxis,masterPos,slavePos,segType,percent,fifo0);
                if(CMD_SUCCESS != rtn)
                {
                    return CommandHandler("GTN_FollowDataPercentEx",rtn);
                }

                // 向FIF0中增加运动数据
                segType = FOLLOW_SEGMENT_NORMAL;
                masterPos+= 20000;slavePos += 20000;
                rtn = GTN_FollowDataPercentEx(core,slaveAxis,masterPos,slavePos,segType,percent,fifo0);
                if(CMD_SUCCESS != rtn)
                {
                    return CommandHandler("GTN_FollowDataPercentEx",rtn);
                }

                // 向FIF0中增加运动数据
                segType = FOLLOW_SEGMENT_NORMAL;
                masterPos += 20000;slavePos += 16000;
                rtn = GTN_FollowDataPercentEx(core,slaveAxis,masterPos,slavePos,segType,percent,fifo0);
                if(CMD_SUCCESS != rtn)
                {
                    return CommandHandler("GTN_FollowDataPercentEx",rtn);
                }

                // 切换到FIFO0
                // 当前工作中的FIFO1的数据遍历完以后才会切换FIFO0
                rtn = GTN_FollowSwitchEx(core,1<<(slaveAxis-1));
                if(CMD_SUCCESS != rtn)
                {
                    return CommandHandler("GTN_FollowSwitchEx",rtn);
                }
            }
        }

        if( STAGE_TO_FIFO0 == stage )
        {
            // 查询FIFO1的剩余空间
            rtn = GTN_FollowSpaceEx(core,slaveAxis,&space,fifo1);
            if(CMD_SUCCESS != rtn)
            {
                return CommandHandler("GTN_FollowSpaceEx",rtn);
            }

            // 如果FIFO1执行结束，space = 16,说明已经切换到FIFO0
            if( 16 == space )
            {
                stage = STAGE_END;
            }
        }

        if( STAGE_END == stage )
        {
            if( 1 == pressKey )
            {
                pressKey = 0;
                break;
            }
        }

        // 读取该轴的轴状态
        rtn = GTN_GetSts(core,axis,&axisSts[0],axisCount,&clock);
        if ( CMD_SUCCESS != rtn )
        {
            return CommandHandler("GTN_GetSts",rtn);
        }

        // 读取该轴的规划速度
        rtn =GTN_GetPrfVel(core,axis,&prfVel[0],axisCount,&clock);
        if ( CMD_SUCCESS != rtn )
        {
            return CommandHandler("GTN_GetPrfVel",rtn);
        }

        // 读取该轴的规划位置
        rtn =GTN_GetPrfPos(core,axis,&prfPos[0],axisCount,&clock);
        if ( CMD_SUCCESS != rtn )
        {
            return CommandHandler("GTN_GetPrfPos",rtn);
        }

        printf("主轴prfPos=%-10.1lf,主轴prfVel=%-10.1lf,从轴prfPos=%-10.1lf,从轴prfVel=%-10.1lf\r", 
            prfPos[0], prfVel[0], prfPos[1], prfVel[1]);

    }

    // 平滑停止
    mask = 1 << (masterAxis - 1);
    mask |= 1 << (slaveAxis - 1);
    stopOption = 0;  
    rtn = GTN_Stop(core,mask,stopOption);
    if ( CMD_SUCCESS != rtn )
    {
        return CommandHandler("GTN_Stop",rtn);
    }

    // 轴规划完成后，下使能(默认代码屏蔽，接实际电机时才调用)
    // 轴下使能，如果需要测试实际驱动器、电机的运动，请将此注释代码打开
    /*
    rtn = GTN_AxisOff(core,slaveAxis);
    if (CMD_SUCCESS != rtn)
    {
        return CommandHandler("GTN_AxisOn", rtn);
    }

    rtn = GTN_AxisOff(core,masterAxis);
    if (CMD_SUCCESS != rtn)
    {
        return CommandHandler("GTN_AxisOn", rtn);
    }
    */

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

    return 0;
}