PT动态FIFO例程

PT动态FIFO例程

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

#include "stdafx.h"
#include "math.h"

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

// 测试功能：PT动态FIFO运动示例
// 测试平台：网络型运动控制器
// 测试环境：Windows
// 测试流程：
//           （1）初始化控制器
//           （2）规划运动
//           （3）运动完成，关闭控制器
// 注意事项：
//           （1）本例程使用的“例程专用.xml”、“例程专用.cfg”，仅用于本例程
//           （2）实际使用时，需要使用MotionStudio生成网络配置xml

// 加载固高运动控制库头文件
#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;
}

int _tmain(int argc, _TCHAR* argv[])
{
    short rtn;                         // 指令返回值
    short core;                        // 需要执行例程的运动控制器核号
    short axis;                        // 需要初始化的轴起始索引号
    short axisCount;                   // 需要初始化的轴数量，从轴起始索引号开始算起
    double prfPos;                     // 实时读取的规划位置
    double prfVel;                     // 实时读取的规划速度
    long axisSts;                      // 轴状态
    short space;                       // PT FIFO空间剩余数量
    short loop = 1;                     // 循环次数
    short A = 50;                       // 正弦速度曲线的幅值, 单位：脉冲
    short T = 1;                        // 正弦速度曲线的周期，单位：s
    double timeDelta = 0.016;           // 时间增量，          单位：s
    short loopMax = 2;                  // 循环最大次数
    double PI = 3.1415926;
    short ptFifo = 0;
    double pos = 0;
    double vel = 0;
    double velPre = 0;
    double time = 0;
    short start = 0;


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

    // 轴上使能，如果需要测试实际驱动器、电机的运动，请将此注释代码打开
    /*rtn = GTN_AxisOn(CORE,TRAP_MOTION_AXIS_NUMBER);
    if (CMD_SUCCESS != rtn)
    {
        return CommandHandler("GTN_AxisOn", rtn);
    }*/

    // 将该轴设为PT运动规划模式
    rtn =GTN_PrfPt(core,axis, PT_MODE_DYNAMIC);
    if(CMD_SUCCESS != rtn)
    {
        CommandHandler("GTN_PrfPt",rtn);
    }

    // 清空PT的FIFO
    rtn =GTN_PtClear(core,axis,ptFifo);
    if(CMD_SUCCESS != rtn)
    {
        CommandHandler("GTN_PtClear",rtn);
    }

    while(1)
    {
        // 查询PT模式FIFO的剩余空间
        rtn =GTN_PtSpace(core,axis, &space,ptFifo);
        if(CMD_SUCCESS != rtn)
        {
            CommandHandler("GTN_PtSpace",rtn);
        }

        if( space> 0 )
        {
            // 时间，单位:s
            time += timeDelta;

            // 计算段末速度，单位: 脉冲/s
            vel = A*sin((2*PI)/T*time);

            // 计算段内位移, 单位:脉冲
            pos += 1000*(vel+velPre)*timeDelta/2;

            velPre = vel;

            if(time<loop*T)
            {
                // 发送新数据.时间转换成ms单位，位置也同比放大1000倍
                rtn =GTN_PtData(core,axis,pos,(long)(time*1000),PT_SEGMENT_NORMAL,ptFifo);
                if(CMD_SUCCESS != rtn)
                {
                    return CommandHandler("GTN_PtData",rtn);
                }
            }
            else
            {
                // 发送终点数据,时间转换成ms单位，位置也同比放大1000倍
                rtn =GTN_PtData(core,axis, pos,loop*T*1000, PT_SEGMENT_STOP,ptFifo);
                if( CMD_SUCCESS != rtn)
                {
                    return CommandHandler("GTN_PtData",rtn);
                }

                pos = 0;
                time = loop*T;
                velPre = 0;
                ++loop;
                if( loop>loopMax )
                {
                    break;
                }
            }
        }
        else if( 0 == start )
        {
            // 启动PT运动
            rtn =GTN_PtStart(core,1<<(axis-1));
            if(CMD_SUCCESS != rtn)
            {
                return CommandHandler("GTN_PtStart",rtn);
            }
            start = 1;
        }

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

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

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

        printf("sts=0x%-10lx,prfVel=%-10.2lf,prfPos=%-10.1lf\r", axisSts, prfVel, prfPos);
    }

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

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

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

    return 0;
}