// PosCompareLinearBuf-GTM.cpp : 定义控制台应用程序的入口点。
//
#include "stdafx.h"
#include <math.h>
// 该例程仅用于功能演示，请保证安全的情况下使用
// 测试功能：位置比较一维LinearBuf功能示例，硬件连接方式为GSN+GTM（两个位置比较子板+一个轴子版）
// 测试平台：网络型运动控制器
// 测试环境：Windows
// 测试流程：
// （1）初始化控制器
// （2）配置位置比较LinearBuf功能
// （3）轴运动，触发位置比较
// （4）位置比较完成，关闭控制器
// 注意事项：
// （1）本例程使用的“例程专用.xml”、“例程专用.cfg”，仅用于本例程
// （2）实际使用时，需要使用MotionStudio生成网络配置xml
// （3）实际使用时，必须确认网络上接了支持位置比较功能的从站！！！
// （4）！！！GTM必须是第一个站！！！
// （4）！！！GTM必须是第一个站！！！
// （4）！！！GTM必须是第一个站！！！
// （4）！！！GTM必须是第一个站！！！
// （4）！！！GTM必须是第一个站！！！
// （4）！！！GTM必须是第一个站！！！
// 加载固高运动控制库头文件
#include "gxn.h"
// 动态加载固高运动控制gxn.lib库
#pragma comment(lib,"gxn.lib")
#define POS_COMPARE_LINK_HSO_MAX (8)
/**
* @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 配置位置比较LinearBuf功能
* @param core 需要配置功能的核号，从1开始
* @param posCompareIndex 需要配置的位置比较索引，从1开始
* @param axisX 位置比较x轴的轴号
* @param pStartMotionSnedCount 启动轴运动时，位置比较已经发送的数据个数，防止跑空
* @param pLinear 配置成功的八段线性位置比较参数
* @return 0表示配置成功，非0表示配置失败
*/
short SetPosCompareLinearBufFunction(short core,short posCompareIndex,short axisX,long *pStartMotionSnedCount,TPosCompareLinear *pLinear)
{
short rtn;
short i;
short station; // 支持波形控制功能的模块的逻辑站号
short permit[POS_COMPARE_LINK_HSO_MAX]; // 控制权
long totalCount; // 设置的位置比较点总数
long startMotionSendCount; // 启动轴运动前，已经发送到GTM的位置比较点个数。已经发送的点数越多，越不容易跑空
TPosCompareModeEx posCompareModeEx;
TPosCompareLinear linear[POS_COMPARE_LINK_HSO_MAX];
memset(&posCompareModeEx,0,sizeof(posCompareModeEx));
memset(linear,0,sizeof(linear));
// 配置hso口控制权
station = 1; // ！！！GTM必须是第一个站！！！
// ！！！GTM必须是第一个站！！！
// ！！！GTM必须是第一个站！！！
for (i=0;i<POS_COMPARE_LINK_HSO_MAX;++i)
{
permit[i] = 0x2; // hso1-8口输出第一路位置比较
}
rtn = GTN_SetTerminalPermitEx(core,station,MC_HSO,&permit[0],1,POS_COMPARE_LINK_HSO_MAX);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetTerminalPermitEx",rtn);
}
// 关闭位置比较功能
rtn = GTN_PosCompareStop(core,posCompareIndex);
if ( 0 != rtn )
{
return CommandHandler("GTN_PosCompareStop",rtn);
}
// 清除位置比较数据
rtn = GTN_PosCompareClear(core,posCompareIndex);
if ( 0 != rtn )
{
return CommandHandler("GTN_PosCompareClear",rtn);
}
// 设置位置比较模式及参数
posCompareModeEx.mode = 1; // Linear模式为1
posCompareModeEx.dimension = 1; // 一维
posCompareModeEx.sourceMode = 1; // 比较源：0：编码器，1：脉冲计数器，2：辅助编码器
posCompareModeEx.source[0] = axisX; // x轴比较源索引
posCompareModeEx.outputMode = 0; // 输出模式，0：脉冲，1：电平，2：电平自动翻转，4：一串脉冲
posCompareModeEx.outputPulseWidth = 100; // 输出脉冲宽度，单位：us
// 当outputMode = 0时，每次到位输出一个脉冲，脉宽为设定值
// 当outputMode = 4时，每次到位输出“outputCounter”个脉冲，脉冲低电平时间=高电平时间=outputPulseWidth/2
// 当outputMode为其他值时时，该变量无效
posCompareModeEx.outputCounter = 1; // 输出脉冲个数，当outputMode = 4时有效
posCompareModeEx.errorBand = 20; // 误差带，单位：脉冲
rtn = GTN_SetPosCompareModeEx(core,posCompareIndex,&posCompareModeEx);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetPosCompareModeEx",rtn);
}
totalCount = 0;
// 从相对起点位置0开始，以100为间距，从hso1口输出100个脉冲
linear[0].startPos = 0;
linear[0].count = 100;
linear[0].gpo = 0;
linear[0].hso = 0x1;
linear[0].interval = 100;
rtn = GTN_SetPosCompareLinearBuf(core,posCompareIndex,&linear[0]);
if( CMD_SUCCESS != rtn )
{
return CommandHandler("GTN_SetPosCompareLinearBuf",rtn);
}
totalCount += linear[0].count;
// 从相对起点位置10000开始，以200为间距，从hso2口输出50个脉冲
linear[1].startPos = 10000;
linear[1].count = 50;
linear[1].gpo = 0;
linear[1].hso = 0x2;
linear[1].interval = 200;
rtn = GTN_SetPosCompareLinearBuf(core,posCompareIndex,&linear[1]);
if( CMD_SUCCESS != rtn )
{
return CommandHandler("GTN_SetPosCompareLinearBuf",rtn);
}
totalCount += linear[1].count;
// 从相对起点位置20000开始，以300为间距，从hso3口输出25个脉冲
linear[2].startPos = 20000;
linear[2].count = 25;
linear[2].gpo = 0;
linear[2].hso = 0x4;
linear[2].interval = 300;
rtn = GTN_SetPosCompareLinearBuf(core,posCompareIndex,&linear[2]);
if( CMD_SUCCESS != rtn )
{
return CommandHandler("GTN_SetPosCompareLinearBuf",rtn);
}
totalCount += linear[2].count;
// 从相对起点位置30000开始，以400为间距，从hso4口输出25个脉冲
linear[3].startPos = 30000;
linear[3].count = 25;
linear[3].gpo = 0;
linear[3].hso = 0x8;
linear[3].interval = 400;
rtn = GTN_SetPosCompareLinearBuf(core,posCompareIndex,&linear[3]);
if( CMD_SUCCESS != rtn )
{
return CommandHandler("GTN_SetPosCompareLinearBuf",rtn);
}
totalCount += linear[3].count;
// 从相对起点位置40000开始，以500为间距，从hso5口输出20个脉冲
linear[4].startPos = 40000;
linear[4].count = 20;
linear[4].gpo = 0;
linear[4].hso = 0x10;
linear[4].interval = 500;
rtn = GTN_SetPosCompareLinearBuf(core,posCompareIndex,&linear[4]);
if( CMD_SUCCESS != rtn )
{
return CommandHandler("GTN_SetPosCompareLinearBuf",rtn);
}
totalCount += linear[4].count;
// 从相对起点位置50000开始，以600为间距，从hso6口输出15个脉冲
linear[5].startPos = 50000;
linear[5].count = 15;
linear[5].gpo = 0;
linear[5].hso = 0x20;
linear[5].interval = 600;
rtn = GTN_SetPosCompareLinearBuf(core,posCompareIndex,&linear[5]);
if( CMD_SUCCESS != rtn )
{
return CommandHandler("GTN_SetPosCompareLinearBuf",rtn);
}
totalCount += linear[5].count;
// 从相对起点位置60000开始，以700为间距，从hso6口输出10个脉冲
linear[6].startPos = 60000;
linear[6].count = 10;
linear[6].gpo = 0;
linear[6].hso = 0x40;
linear[6].interval = 700;
rtn = GTN_SetPosCompareLinearBuf(core,posCompareIndex,&linear[6]);
if( CMD_SUCCESS != rtn )
{
return CommandHandler("GTN_SetPosCompareLinearBuf",rtn);
}
totalCount += linear[6].count;
// 从相对起点位置70000开始，以800为间距，从hso6口输出10个脉冲
linear[7].startPos = 70000;
linear[7].count = 10;
linear[7].gpo = 0;
linear[7].hso = 0x80;
linear[7].interval = 800;
rtn = GTN_SetPosCompareLinearBuf(core,posCompareIndex,&linear[7]);
if( CMD_SUCCESS != rtn )
{
return CommandHandler("GTN_SetPosCompareLinearBuf",rtn);
}
totalCount += linear[7].count;
// 启动位置比较功能
rtn = GTN_PosCompareStart(core,posCompareIndex);
if ( 0 != rtn )
{
return CommandHandler("GTN_PosCompareStart",rtn);
}
if ( totalCount <= 128 )
{
startMotionSendCount = totalCount;
}
else
{
startMotionSendCount = 128;
}
*pStartMotionSnedCount = startMotionSendCount;
memcpy(pLinear,linear,sizeof(linear));
printf("Set PosCompare Linear Buf Function Success !\n");
return rtn;
}
/**
* @brief 将轴点位运动到目标位置
* @param core 核号，从1开始
* @param profile 规划器索引，从1开始
* @param targetVel 目标速度
* @param targetPos 目标位置
* @param posCompareIndex 位置比较索引
* @return 0表示成功，非0表示失败
*/
short TrapMoveWithPosCompare(short core,short profile,double targetVel,long targetPos,short posCompareIndex)
{
short rtn;
TTrapPrm trap;
TPosCompareStatus posCompareStatus;
memset(&trap,0,sizeof(trap));
memset(&posCompareStatus,0,sizeof(posCompareStatus));
rtn = GTN_PrfTrap(core,profile);
if ( 0 != rtn )
{
return CommandHandler("GTN_PrfTrap",rtn);
}
trap.acc = 10;
trap.dec = 20;
trap.velStart = 0;
trap.smoothTime = 0;
rtn = GTN_SetTrapPrm(core,profile,&trap);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetTrapPrm",rtn);
}
rtn = GTN_SetVel(core,profile,targetVel);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetVel",rtn);
}
rtn = GTN_SetPos(core,profile,targetPos);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetPos",rtn);
}
rtn = GTN_Update(core,1<<(profile-1));
if ( 0 != rtn )
{
return CommandHandler("GTN_Update",rtn);
}
long sts;
double prfPos;
do
{
rtn = GTN_GetSts(core,profile,&sts,1,0);
rtn += GTN_GetPrfPos(core,profile,&prfPos,1,0);
rtn += GTN_PosCompareStatus(core,posCompareIndex,&posCompareStatus);
if ( 0 != rtn )
{
return CommandHandler("GTN_GetSts",rtn);
}
printf("Axis:%d status:0x%#x prfPos:%.3lf pulseCount:%ld \r",profile,sts,prfPos,posCompareStatus.pulseCount);
} while ( 0x400 == (sts&0x400) );
if ( fabs(prfPos - targetPos) < 1.0 )
{
printf("\nAxis:%d moves to origin success!\n");
return 0;
}
else
{
printf("\nAxis:%d moves to origin failed!\n");
return 1;
}
}
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 posCompareIndex; // 位置比较索引
short axisX; // 位置比较的x轴
long startMotionSendCount; // 启动轴运动前，已经发送到GTM的位置比较点个数。已经发送的点数越多，越不容易跑空
TPosCompareLinear linear[POS_COMPARE_LINK_HSO_MAX];
TPosCompareInfo posCompareInfo;
posCompareIndex = 1; // 模块上的第一路位置比较逻辑索引为1
axisX = 1;
rtn = SetPosCompareLinearBufFunction(core,posCompareIndex,axisX,&startMotionSendCount,&linear[0]);
if ( 0 != rtn )
{
return rtn;
}
do
{
rtn = GTN_PosCompareInfo(core,posCompareIndex,&posCompareInfo);
if ( 0 != rtn )
{
return CommandHandler("GTN_PosCompareInfo",rtn);
}
printf("Successfully send %ld data to GTM \r",posCompareInfo.commandSend);
} while ( posCompareInfo.commandSend < startMotionSendCount );
printf("Successfully send %ld data to GTM \n",posCompareInfo.commandSend);
double startPrfPos; // 启动位置比较时，位置比较轴的规划位置
double targetVel; // 位置比较轴点位运动目标速度
long targetPos; // 位置比较轴点位运动目标位置
rtn = GTN_GetPrfPos(core,axisX,&startPrfPos,1,0);
if ( 0 != rtn )
{
return CommandHandler("GTN_GetPrfPos",rtn);
}
short i;
for (i=0;i<POS_COMPARE_LINK_HSO_MAX;++i)
{
printf("\n//------------------------------\n");
printf("linear[%d].hso = %#x\n",i,linear[i].hso);
printf("linear[%d].startPos = %ld\n",i,linear[i].startPos);
printf("linear[%d].count = %lu\n",i,linear[i].count);
printf("linear[%d].interval = %ld\n",i,linear[i].interval);
printf("Press any key to start motion for pos compare in hso %d !\n",(i+1));
getchar();
targetVel = 10;
targetPos = startPrfPos + linear[i].startPos + linear[i].interval * linear[i].count - linear[i].interval / 2;
rtn = TrapMoveWithPosCompare(core,axisX,targetVel,targetPos,posCompareIndex);
if ( 0 != rtn )
{
return rtn;
}
}
// 关闭位置比较功能
rtn = GTN_PosCompareStop(core,posCompareIndex);
if ( 0 != rtn )
{
return CommandHandler("GTN_PosCompareStop",rtn);
}
// 关闭控制器
rtn = GTN_Close();
if ( 0 != rtn )
{
return CommandHandler("GTN_Close",rtn);
}
printf("Press Any Key To Exit !\n");
getchar();
return 0;
}