单轴手轮运动例程

单轴手轮运动例程

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

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

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

// 测试功能：单轴手轮引导功能，
// 测试平台：网络型运动控制器，等环网从站403模块、电机与手轮，
// 测试环境：Windows
// 测试流程：
//           （1）初始化控制器
//           （2）将轴1设置为从轴，MPG作为主轴的源，从轴会随着手轮转动而转动
//           （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;                       // 轴状态
    unsigned long clock;                // 读取的控制器时钟
    double encVel,encPos;               // 读取的编码器速度和位置
    long mask,stopOption;

    short master;                       // 主轴索引
    short slave;                        // 从轴索引
    short masterEven;                   // 主轴传动比系数，主轴的位移，
    short slaveEven;                    // 从轴传动比系数，从轴的位移,（两个系数的比值就是齿轮比）。
    short intervalTime;                 // 主轴位移采样时间间隔，取值范围：[1, 32767]，单位：规划周期。
    double acc;                         // 跟随过程中从轴的加速度，正值
    double dec;                         // 跟随过程中从轴的减速度，正值。
    double vel;                         // 跟随过程中从轴的最大速度，正值。
    short stopWaitTime;                 // 判断停止时间，取值范围：[1, 32767]，单位：规划周期。
                                        // 高速跟随时，主轴急停，从轴经过多少时间后才停止，防止从轴跟随冲击过大。

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

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

    //使用MPG作为主轴的源,初始化单轴手轮功能
    rtn = GTN_HandwheelInit(core,MC_MPG);  
    if ( CMD_SUCCESS != rtn )
    {
        return CommandHandler("GTN_HandwheelInit",rtn);
    }

     // 启动单轴手轮
    master = 1;                             // 使用第1路MPG作为主轴
    slave = axis;                           // 1轴为从轴
    masterEven = 1;                         // 主、从轴传动比为1：100
    slaveEven = 100;
    intervalTime = 1;                       // 主轴位移采样时间间隔为100个控制器周期，
    acc = 0.5;
    dec = 0.5;
    vel = 100;
    stopWaitTime = 200;                     // 手轮停止时间超过200个控制器周期，被判定为停止。
    rtn = GTN_StartHandwheel(core,slave,master,masterEven,slaveEven,intervalTime,acc,dec,vel,stopWaitTime);
    if ( CMD_SUCCESS != rtn )
    {
        return CommandHandler("GTN_StartHandwheel",rtn);
    }
/*
    do 
    {
        // 读取从轴的轴状态
        rtn = GTN_GetSts(core,axis,&axisSts,axisCount,&clock);
        if ( CMD_SUCCESS != rtn )
        {
            return CommandHandler("GTN_GetSts",rtn);
        }

        // 读取从轴的编码器速度
        rtn = GTN_GetEncVel(core,axis,&encVel,axisCount,&clock);
        if ( CMD_SUCCESS != rtn )
        {
            return CommandHandler("GTN_GetEncVel",rtn);
        }

        // 读取从轴的编码器位置
        rtn =GTN_GetEncPos(core,axis,&encPos,axisCount,&clock);
        if ( CMD_SUCCESS != rtn )
        {
            return CommandHandler("GTN_GetPrfPos",rtn);
        }

        printf("从轴状态sts=0x%-10lx从轴encPos=%-10.1lf从轴encVel=%-10.1lf\r", axisSts, encPos, encVel);

    } while ( 0x400 == (axisSts&0x400));
*/

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

    //退出单轴手轮
    rtn = GTN_EndHandwheel(core,slave);
    if ( CMD_SUCCESS != rtn )
    {
        return CommandHandler("GTN_EndHandwheel",rtn);
    }
    // 轴规划完成后，下使能(默认代码屏蔽，接实际电机时才调用)
    // 轴下使能，如果需要测试实际驱动器、电机的运动，请将此注释代码打开
    /*
    rtn = GTN_AxisOff(core,axis);
    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;
}