PVT Continuous描述方式例程

PVT_Continuous描述方式例程

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

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

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

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

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

#define PI 3.1415926

/**
 * @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];                   // 轴状态
    short tableId[2];                  // 读取的tableID
    double timeTemp[2];                // 读取的运动时间
    short dataCount;                   // pvt数据点数量
    unsigned long clock;               // 读取的控制器时钟
    double prfVel[2], prfPos[2];       // 读取的X轴、Y轴对应的规划速度和位置
    short table_X = 1;                 // X轴对应的pvt数据表
    short table_Y = 2;                 // Y轴对应的pvt数据表    
    short axisArray[2]={1,2};          // X轴、Y轴对应的轴号为1、2轴
    // X轴对应的pvt数据点参数
    double pos_x[2] = {0, 30000};
    double vel_x[2] = {0, 0};
    double percent_x[2] = {100, 100};
    double velMax_x[2] = {10, 10};
    double acc_x[2] = {0.01, 0.01};
    double dec_x[2] = {0.01, 0.01};
    double time_x[2];
    double timeBegin_x;
    // Y轴对应的pvt数据点参数
    double pos_y[2] = {0, 20000};
    double vel_y[2] = {0, 0};
    double percent_y[2] = {100, 100};
    double velMax_y[2] = {10, 10};
    double acc_y[2] = {0.01, 0.01};
    double dec_y[2] = {0.01, 0.01};
    double time_y[2];
    double timeBegin_y;

    // 初始化运动控制器
    // 开卡 + 初始化网络拓扑 + 初始化核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,axisArray[0]);
    if (CMD_SUCCESS != rtn)
    {
        return CommandHandler("GTN_AxisOn", rtn);
    }

    rtn = GTN_AxisOn(core,axisArray[1]);
    if (CMD_SUCCESS != rtn)
    {
        return CommandHandler("GTN_AxisOn", rtn);
    }
    */

    // X轴设置为PVT运动模式
    rtn =GTN_PrfPvt(core,axisArray[0]);
    if ( CMD_SUCCESS != rtn )
    {
        return CommandHandler("GTN_PrfPvt",rtn);
    }

    // Y轴设置为PVT运动模式
    rtn =GTN_PrfPvt(core,axisArray[1]);
    if ( CMD_SUCCESS != rtn )
    {
        return CommandHandler("GTN_PrfPvt",rtn);
    }

    //计算X轴运动时间，共2个数据点
    dataCount = 2;
    rtn = GTN_PvtContinuousCalculate(core,dataCount,&pos_x[0],&vel_x[0],&percent_x[0],
        &velMax_x[0],&acc_x[0],&dec_x[0],&time_x[0]);
    if ( CMD_SUCCESS != rtn )
    {
        return CommandHandler("GTN_PvtContinuousCalculate",rtn);
    }

    //计算Y轴运动时间，共2个数据点
    rtn = GTN_PvtContinuousCalculate(core,dataCount,&pos_y[0],&vel_y[0],&percent_y[0],
        &velMax_y[0],&acc_y[0],&dec_y[0],&time_y[0]);
    if ( CMD_SUCCESS != rtn )
    {
        return CommandHandler("GTN_PvtContinuousCalculate",rtn);
    }

    // 计算启动延时,达到同时停止的效果
    if( time_x[1] <time_y[1] )
    {
        timeBegin_x = time_y[1] - time_x[1];
        timeBegin_y = 0;
    }
    else
    {
        timeBegin_x = 0;
        timeBegin_y = time_x[1] - time_y[1];
    }

    // 向X轴的pvt数据表发送数据，共2个数据点
    rtn =GTN_PvtTableContinuous(core,table_X, dataCount, &pos_x[0], &vel_x[0], &percent_x[0],
        &velMax_x[0],&acc_x[0],&dec_x[0],timeBegin_x);
    if ( CMD_SUCCESS != rtn )
    {
        return CommandHandler("GTN_PvtTableContinuous",rtn);
    }

    // 向Y轴的pvt数据表发送数据，共2个数据点
    rtn =GTN_PvtTableContinuous(core,table_Y, dataCount, &pos_y[0], &vel_y[0], &percent_y[0],
        &velMax_y[0],&acc_y[0],&dec_y[0],timeBegin_y);
    if ( CMD_SUCCESS != rtn )
    {
        return CommandHandler("GTN_PvtTableContinuous",rtn);
    }

    // X轴选择数据表table_X
    rtn =GTN_PvtTableSelect(core,axisArray[0], table_X);
    if ( CMD_SUCCESS != rtn )
    {
        return CommandHandler("GTN_PvtTableSelect",rtn);
    }

    // Y轴选择数据表table_Y
    rtn =GTN_PvtTableSelect(core,axisArray[1], table_Y);
    if ( CMD_SUCCESS != rtn )
    {
        return CommandHandler("GTN_PvtTableSelect",rtn);
    }

    // 同时启动X轴和Y轴
    axisCount = 2;
    rtn =GTN_PvtStartPro(core,&axisArray[0],axisCount);
    if ( CMD_SUCCESS != rtn )
    {
        return CommandHandler("GTN_PvtStartPro",rtn);
    }

    axisCount = 2;
    do 
    {
        // 读取X轴和Y轴，两个轴的状态
        rtn = GTN_GetSts(core,axis,&axisSts[0],axisCount,&clock);
        if ( CMD_SUCCESS != rtn )
        {
            return CommandHandler("GTN_GetSts",rtn);
        }

        // 读取X轴和Y轴，两个轴的关联的数据表和运动时间
        rtn =GTN_PvtStatus(core,axis,&tableId[0],&timeTemp[0],axisCount);
        if ( CMD_SUCCESS != rtn )
        {
            return CommandHandler("GTN_PvtStatus",rtn);
        }

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

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

        printf("x轴表ID号:%2d 运动时间:%6.0lf 速度:%6.2lf 位置:%6.1lf y轴表ID号:%2d 运动时间:%6.0lf 速度:%6.2lf 位置:%6.1lf\r", 
            tableId[0], timeTemp[0], prfVel[0], prfPos[0], tableId[1], timeTemp[1], prfVel[1], prfPos[1]);

    } while ( (0x400 == (axisSts[0]&0x400)) || (0x400 == (axisSts[1]&0x400)));

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

    rtn = GTN_AxisOff(core,axisArray[1]);
    if (CMD_SUCCESS != rtn)
    {
        return CommandHandler("GTN_AxisOn", rtn);
    }
    */

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

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

    return 0;
}