// PosCompareAddition.cpp : 定义控制台应用程序的入口点。
//
#include "stdafx.h"
#include <math.h>
// 该例程仅用于功能演示，请保证安全的情况下使用
// 测试功能：新架构五轴PSO功能示例，硬件连接方式为GSN+GNM601
// 测试平台：网络型运动控制器
// 测试环境：Windows
// 测试流程：
// （1）初始化控制器
// （2）轴初始化
// （3）group初始化
// （4）初始化前瞻
// （5）设置位置比较模式以及叠加参数
// （6）设置指令坐标系与规划坐标系与坐标系偏移
// （7）压入MoveLinearAbsolute
// （8）启动PSO输出
// （9）推送数据到DSP
// （10）启动指令流
// （11）读取group状态信息与指令流信息
// （12）关闭控制器
// 注意事项：
// （1）本例程使用的“例程专用.xml”、“例程专用.cfg”，仅用于本例程
// （2）实际使用时，需要使用MotionStudio生成网络配置xml
// （3）实际使用时，必须确认网络上接了支持位置比较功能的从站！！！
// 加载固高运动控制库头文件
#include "gts.h"
// 动态加载固高运动控制gxn.lib库
#pragma comment(lib,"gts.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(99,"例程专用.xml",overTime,&status);
if ( 0 != rtn )
{
printf("status = %d\n",status);
return CommandHandler("GTN_NetInit",rtn);
}
printf("Init Net Success !\n");
rtn = GTN_Reset(core);
// 加载配置文件到控制器
// 注意：（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 获取插补运动状态
* @param core 插补坐标系所在的核号
* @param crd 插补坐标系号
* @param fifo 插补坐标系缓存区号
* @param refCompareIndex 参考坐标系位置比较索引
* @param syncCompareIndex 同步坐标系位置比较索引
* @param pCrdRun 插补坐标系运动状态变量指针
* @return 0表示获取插补运动状态成功，非0表示获取插补运动状态出错
*/
short GetMotionInfo(short core,short crd,short fifo,short refCompareIndex,short syncCompareIndex,short *pCrdRun)
{
short rtn;
long crdExecuteSegNum; // 插补已经执行的段数
long crdRemainder; // 插补剩余未执行段数
double crdPos[8]; // 插补规划位置
TPosCompareStatus refCmpStatus; // 参考坐标系位置比较状态
TPosCompareStatus syncCmpStatus; // 同步坐标系位置比较状态
// 运动过程中读取当前状态
rtn = GTN_CrdStatus(core,crd,pCrdRun,&crdExecuteSegNum,fifo);
rtn += GTN_GetCrdPos(core,crd,&crdPos[0]);
rtn += GTN_PosCompareStatus(core,refCompareIndex,&refCmpStatus);
rtn += GTN_PosCompareStatus(core,syncCompareIndex,&syncCmpStatus);
if ( 0 != rtn )
{
return CommandHandler("GTN_CrdStatus",rtn);
}
printf("crdRun:%d, crdExecuteSegNum:%d, xPos:%.3lf, yPos:%.3lf, refComparePulseCount:%d syncComparePulseCount:%d \r",
*pCrdRun,crdExecuteSegNum,crdPos[0],crdPos[1],refCmpStatus.pulseCount,syncCmpStatus.pulseCount);
if ( 0 == *pCrdRun )
{
rtn = GTN_GetRemainderSegNum(core,crd,&crdRemainder,fifo);
if ( 0 != rtn )
{
return CommandHandler("GTN_GetRemainderSegNum",rtn);
}
if ( 0 != crdRemainder )
{
printf("\nCrd Run Empty Error!\n");
}
else
{
printf("\nCrd Motion Done !\n");
}
}
return 0;
}
/**
* @brief 位置比较初始化
* @param core 插补坐标系所在的核号
* @param station 位置比较输出的站号
* @param additionAxis 位置叠加轴
* @param posCompMode 位置比较模式
* @param index 控制权硬件索引
* @param posCompareIndex 位置比较输出通道索引
* @param count 控制权输出通道数
* @param pAddition 叠加参数
* @return 0表示获取插补运动状态成功，非0表示获取插补运动状态出错
*/
short PosCompareInit(short core,short station,short additionAxis,short posCompMode,short index,short posCompareIndex,short count,TAddition *pAddition)
{
short rtn;
//第一步：setAxisAddtion
TAddition Addition;
memset(&Addition,0,sizeof(Addition));
Addition.index[0] = pAddition->index[0];
Addition.type = pAddition->type;
rtn = GTN_SetAxisAddition(core, additionAxis, MC_PROFILE, &Addition); //给空闲轴
if (0 != rtn)
{
return CommandHandler("GTN_SetAxisAddition()",rtn);
}
short permit = 2;
rtn = GTN_SetTerminalPermitEx(core, station,MC_HSO, &permit, index, count);
if (0 != rtn)
{
return CommandHandler("GTN_SetTerminalPermitEx()",rtn);
}
//第二步，设置模式，使用GTN_SetPosCompareModeEx
TPosCompareModeEx posCompareMode;
memset(&posCompareMode,0,sizeof(posCompareMode));
posCompareMode.mode = posCompMode; //模式3
posCompareMode.dimension = 1;
posCompareMode.sourceMode = 21; //注意此处需更改为21号模式
posCompareMode.source[0] = additionAxis; //空闲轴轴号
posCompareMode.source[1] = additionAxis;
posCompareMode.outputMode = 0;
posCompareMode.outputPulseWidth = 1000;
posCompareMode.outputCounter = 1;
posCompareMode.errorBand = 50;
rtn = GTN_SetPosCompareModeEx(core, posCompareIndex, &posCompareMode);
if (0 != rtn)
{
return CommandHandler("GTN_SetPosCompareModeEx()",rtn);
}
//第三步 设置比较参数
TPosComparePsoPrm posComparePsoPrm;
posComparePsoPrm.gpo = 0xff;
posComparePsoPrm.hso = 0xff;
posComparePsoPrm.syncPos = 100;
posComparePsoPrm.time = 0;
posComparePsoPrm.count = 1;
rtn = GTN_SetPosComparePsoPrm(core, posCompareIndex, &posComparePsoPrm);
if (0 != rtn)
{
return CommandHandler("GTN_SetPosComparePsoPrm()",rtn);
}
//第四步 设置指令流内比较源
TPosCompareReferencePrm prefPrm;
memset(&prefPrm,0,sizeof(prefPrm));
prefPrm.referenceX = additionAxis; //空闲轴号
prefPrm.referenceY = additionAxis; //与上面相同
rtn = GTN_SetPosCompareReference(core, posCompareIndex, &prefPrm);
if (0 != rtn)
{
return CommandHandler("GTN_SetPosCompareReference()",rtn);
}
//启动PSO
rtn = GTN_PosCompareStart(core, posCompareIndex);
if (0 != rtn)
{
return CommandHandler("GTN_PosCompareStart()",rtn);
}
printf("PosCompareInit Success!\n");
return rtn;
}
/**
* @brief 轴初始化，设置轴当量、约束与平滑
* @param core group所在的核号
* @param axis 起始轴号
* @param axisCount 需要初始化的轴数量，从起始索引axis开始计数，必须大于0
* @return 0表示获取插补运动状态成功，非0表示获取插补运动状态出错
*/
short AxisInit(short core,short axis,short axisCount)
{
short rtn;
short i;
TProfileScale scale; //轴当量信息
TAxisMotionConstraint axisMotionConstraint; //轴运动约束信息
double smoothTime; //轴平滑时间
double smoothK; //轴平滑系数
memset(&scale,0,sizeof(scale));
scale.count = 1;
scale.alpha[0] = 1; // 脉冲当量，alpha可以认为是mm的单位，beta是脉冲的单位。beta / alpha
scale.beta[0] = 1000;
memset(&axisMotionConstraint,0,sizeof(axisMotionConstraint));
// 设置轴运动约束参数，规划器运动的最大参数，GT_运动指令中的运动参数大于这些参数数，约束到该最大值
axisMotionConstraint.velMax = 200; // 轴的最大速度，单位：mm/s 或者 度/s
axisMotionConstraint.accMax = 1000; // 轴的最大加速度，单位：mm/s^2 或者 度/s^2
axisMotionConstraint.decMax = 1000; // 轴的最大减速度，单位：mm/s^2 或者 度/s^2
axisMotionConstraint.jerkMax = 100000; // 轴的最大加加速度，单位：mm/s^3 或者 度/s^3 -暂时不生效
axisMotionConstraint.dvMax = 1; // 轴的最大速度跳变量,单位：mm/s 或者 度/s
smoothTime = 20; //轴平滑时间为20，单位：ms
smoothK = 16; //轴平滑系数为16
for (i = axis;i < axis + axisCount;i++)
{
rtn = GTN_SetAxisScale(core,i,&scale,NULL);
if (0 != rtn)
{
return CommandHandler("GTN_SetAxisScale()",rtn);
}
rtn = GTN_SetAxisMotionConstraint(core,i,&axisMotionConstraint,NULL);
if (0 != rtn)
{
return CommandHandler("GTN_SetAxisMotionConstraint()",rtn);
}
rtn = GTN_SetAxisMotionSmooth(core,i,smoothTime,smoothK);
if (0 != rtn)
{
return CommandHandler("GTN_SetAxisMotionSmooth()",rtn);
}
}
rtn = GTN_ZeroPos(core,axis,axisCount);
if ( 0 != rtn)
{
return CommandHandler("GTN_ZeroPos()",rtn);
}
printf("Axis Init Success\n");
return rtn;
}
/**
* @brief group初始化
* @param core group所在的核号
* @param group 轴组号
* @param axis 添加到group中的规划轴起始轴号
* @param axisCount 需要初始化的轴数量，从起始索引axis开始计数，必须大于0
* @return 0表示获取插补运动状态成功，非0表示获取插补运动状态出错
*/
short GroupInit(short core,short group,short axis,short axisCount)
{
short rtn;
short i;
short identInGroup = 1; //axis添加到group组内对应轴号
rtn = GTN_GroupDisable(core,group,NULL);
if (0 != rtn)
{
return CommandHandler("GTN_GroupDisable()",rtn);
}
rtn = GTN_UngroupAllAxes(core,group,NULL);
if (0 != rtn)
{
return CommandHandler("GTN_UngroupAllAxes()",rtn);
}
// 建立五轴模型
TKinematicTransform kinematicTransform; // 五轴模型参数
memset(&kinematicTransform, 0, sizeof(kinematicTransform));
kinematicTransform.type = KIN_TYPE_FIVE_AXIS; // 结构类型，五轴结构
kinematicTransform.kinPrm.fiveAxis.type = RW_C_ON_A; // 双转台模型,第一旋转轴为A轴(X)，第二旋转轴为C轴(Z)
kinematicTransform.kinPrm.fiveAxis.primaryAxisPoint[0]= 0; //
kinematicTransform.kinPrm.fiveAxis.primaryAxisPoint[1]= 0; //
kinematicTransform.kinPrm.fiveAxis.primaryAxisPoint[2]= 0; // 第一旋转轴的旋转中心在MCS的坐标
kinematicTransform.kinPrm.fiveAxis.slaveAxisPoint[0]= 0;
kinematicTransform.kinPrm.fiveAxis.slaveAxisPoint[1]= 0;
kinematicTransform.kinPrm.fiveAxis.slaveAxisPoint[2]= 0; // 第二旋转轴的旋转中心在MCS的坐标
kinematicTransform.kinPrm.fiveAxis.toolLocationPoint[0]= 0;
kinematicTransform.kinPrm.fiveAxis.toolLocationPoint[1]= 0;
kinematicTransform.kinPrm.fiveAxis.toolLocationPoint[2]= 0; // 刀具坐标系中心在MCS的坐标
kinematicTransform.kinPrm.fiveAxis.dirMode= 0; // 轴方向描述模式,0,通过描述是否满足右手坐标系来确定轴方向，仅支持正交模型，1：通过轴矢量描述轴方向，可支持正交与非正交模型
for(i=0;i<5;i++)
{
kinematicTransform.kinPrm.fiveAxis.dir[i]= 0; // 各轴方向，0满足右手坐标系，1不满足右手坐标系
}
rtn = GTN_SetGroupKinematicTransform(core,group,&kinematicTransform);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetGroupKinematicTransform()",rtn);
}
// axis添加到group组内对应轴号
for (i = 0;i < axisCount;i++)
{
rtn = GTN_AddAxisToGroup(core,group,axis + i,identInGroup,NULL);
if (0 != rtn)
{
return CommandHandler("GTN_AddAxisToGroup()",rtn);
}
identInGroup += 1;
}
rtn = GTN_GroupEnable(core,group,NULL);
if (0 != rtn)
{
return CommandHandler("GTN_GroupEnable()",rtn);
}
printf("Init group Success !\n");
return rtn;
}
/**
* @brief 前瞻初始化，开启前瞻并设置前瞻参数
* @param core group所在的核号
* @param group 新架构轴组号
* @return 0表示获取插补运动状态成功，非0表示获取插补运动状态出错
*/
short GroupLookAheadInit(short core,short group)
{
short rtn;
TGroupLookAheadParameter groupLookAheadParameter; //group前瞻参数
memset(&groupLookAheadParameter, 0, sizeof(groupLookAheadParameter));
//打开group前瞻功能
rtn = GTN_GroupLookAheadEnable(core,group);
if ( 0 != rtn )
{
return CommandHandler("GTN_GroupLookAheadEnable()",rtn);
}
//设置group前瞻参数
groupLookAheadParameter.lookAheadNum = 200; //前瞻段数
groupLookAheadParameter.time = 0.01; //转角系数
groupLookAheadParameter.radiusRatio = 1; //圆弧系数
rtn = GTN_SetGroupLookAheadParameter(core,group,&groupLookAheadParameter);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetGroupLookAheadParameter()",rtn);
}
printf("GroupLookAheadParameter Success!\n");
return rtn;
}
/**
* @brief 获取插补运动状态
* @param core group所在的核号
* @param list 指令流号
* @param group 新架构轴组号
* @param axis 添加到group的规划轴起始轴号
* @param axisCount 添加到group的规划轴轴数
* @param runStatus group与List以及轴的状态
* @param posCompareIndex 位置比较通道索引
* @return 0表示获取group运动状态成功，非0表示获取group运动状态出错
*/
short GetListAndGroupMotionInfo(short core,short list,short group,short axis,short axisCount,short posCompareIndex,short *pRunStatus)
{
short rtn;
short groupIdentCount = axisCount; // 获取规划轴信息与group轴信息的轴数量
double acsPos[8] = {0,0,0,0,0,0,0,0}; // group轴的ACS位置信息
double pcsPos[8] = {0,0,0,0,0,0,0,0}; // group轴的PCS位置信息
TGroupStatus groupStatus; // group轴的状态信息
TCommandListStatus commandListStatus; // 指令流的状态信息
unsigned long clock; // 时钟
long sts[8]; // 规划轴的状态信息
short axisRun;
short i;
// Group运动过程中读取规划轴运动状态
rtn = GTN_GetSts(core,axis,sts,axisCount,&clock);
if ( 0 != rtn )
{
return CommandHandler("GTN_GetSts()",rtn);
}
axisRun = 0;
for ( i = 0;i < axisCount;++i)
{
axisRun = axisRun || (sts[i] & 0x400);
}
// 读取group组内ACS坐标系轴的位置信息
rtn = GTN_GetGroupProfilePos(core,group,1,&acsPos[0],groupIdentCount,COORD_SYSTEM_ACS,ORI_MODE_NONE);
if (rtn)
{
return CommandHandler("GTN_GetGroupProfilePos",rtn);
}
// Group运动过程中指令流的信息
rtn = GTN_GetCommandListStatus(core,list,&commandListStatus);
if ( 0 != rtn )
{
return CommandHandler("GTN_GetCommandListStatus()",rtn);
}
// Group运动过程中读取group信息
rtn = GTN_GetGroupStatus(core,group,&groupStatus);
if ( 0 != rtn )
{
return CommandHandler("GTN_GetGroupStatus()",rtn);
}
TPosCompareStatus posCompareStatus;
rtn = GTN_PosCompareStatus(core,posCompareIndex,&posCompareStatus);
if ( 0 != rtn )
{
return CommandHandler("GTN_PosCompareStatus()",rtn);
}
printf("groupRun = %d,listExecute = %d,posCmpPulseCount = %d,acsPos[0] = %lf,acsPos[1] = %lf,acsPos[2] = %lf,acsPos[3] = %lf,acsPos[4] = %lf\r\n",
groupStatus.run,commandListStatus.execute,posCompareStatus.pulseCount,acsPos[0],acsPos[1],acsPos[2],acsPos[3],acsPos[4]);
*pRunStatus = ((1 == groupStatus.run)||(1 == commandListStatus.execute)||(1 == axisRun));
if ( 0 == commandListStatus.execute)
{
if ( 0 != commandListStatus.remainderSegCount)
{
printf("Motion Error commandListStatus.stopInfo = %d,groupStatus.stopInfo = %d\n",commandListStatus.stopInfo,groupStatus.stopInfo);
return CommandHandler("commandList Motion Error!",1);
}
}
return 0;
}
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;
}
axisCount = 5;
rtn = AxisInit(core,axis,axisCount);
if ( 0 != rtn )
{
return rtn;
}
short group;
group = 1;
rtn = GroupInit(core,group,axis,axisCount);
if ( 0 != rtn )
{
return rtn;
}
rtn = GroupLookAheadInit(core,group);
if ( 0 != rtn )
{
return rtn;
}
short station = 1; // PCS指令坐标系变化使能参数，0：不使能，1;使能
short additionAxis = 6;
short posCompareMode = 3;
short index = 1;
short posCompareIndex = 1;
short count = 1;
TAddition addition;
memset(&addition,0,sizeof(addition));
addition.index[0] = group;
addition.type = 510;
rtn = PosCompareInit(core,station,additionAxis,posCompareMode,index,posCompareIndex,count,&addition);
if ( 0 != rtn )
{
return rtn;
}
// 设置工件坐标系PCS的偏移参数
short enablePcs; // PCS指令坐标系变化使能参数，0：不使能，1;使能
enablePcs = 1;
TCartesianParameter pcsPrm;
memset(&pcsPrm,0,sizeof(pcsPrm));
pcsPrm.transX = 0; // PCS坐标系X轴偏移60
pcsPrm.transY = 0; // PCS坐标系X轴偏移80
pcsPrm.transZ = 0; // PCS坐标系X轴偏移100
pcsPrm.rotAngle1 = 0; // PCS坐标系绕Z轴的旋转角度，旋转角度必须为0°
pcsPrm.rotAngle2 = 0; // PCS坐标系绕Y轴的旋转角度，旋转角度必须为0°
pcsPrm.rotAngle3 = 0; // PCS坐标系绕X轴的旋转角度，旋转角度必须为0°
rtn = GTN_SetGroupCartesianTransform(core,group,COORD_SYSTEM_PCS,enablePcs,&pcsPrm,NULL);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetGroupCartesianTransform()",rtn);
}
// 设置工具TCS坐标系的偏移参数
TCartesianParameter tcsPrm;
short enableTcs; // TCS指令坐标系变化使能参数，0：不使能，1;使能
enableTcs = 1;
memset(&tcsPrm,0,sizeof(tcsPrm));
tcsPrm.transX = 0; // TCS坐标系X轴偏移0
tcsPrm.transY = 0; // TCS坐标系X轴偏移0
tcsPrm.transZ = 0; // TCS坐标系X轴偏移0
tcsPrm.rotAngle1 = 0; // TCS坐标系绕Z轴的旋转角度，旋转角度必须为0°
tcsPrm.rotAngle2 = 0; // TCS坐标系绕Y轴的旋转角度，旋转角度必须为0°
tcsPrm.rotAngle3 = 0; // TCS坐标系绕X轴的旋转角度，旋转角度必须为0°
rtn = GTN_SetGroupCartesianTransform(core,group,COORD_SYSTEM_TCS,enableTcs,&tcsPrm,NULL);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetGroupCartesianTransform()",rtn);
}
// 设置轴ACS坐标系的偏移参数
double acsOffset[8] = {0,0,0,0,0,0,0,0};
acsOffset[0] = 0; // ACS坐标系中轴1的偏移0mm
acsOffset[1] = 0; // ACS坐标系中轴2的偏移0mm
acsOffset[2] = 0; // ACS坐标系中轴3的偏移0mm
acsOffset[3] = 0; // ACS坐标系中轴4的偏移0°
acsOffset[4] = 0; // ACS坐标系中轴5的偏移0°
rtn = GTN_SetGroupAcsKinematicOffset(core,group,&acsOffset[0],NULL);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetGroupAcsKinematicOffset()",rtn);
}
short configIndex; // 构型解选择参数，pos修改为直线插补终点位置、dir修改为各轴的运动方向、仅对设置为旋转轴模式时生效
configIndex = 0;
// 设置指令坐标系为PCS
rtn = GTN_SetGroupCommandPosDefine(core,group,COORD_SYSTEM_PCS,ORI_MODE_ROTATE_AXIS_POS,configIndex,NULL);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetGroupCommandPosDefine()",rtn);
}
//设置规划坐标系为PCS
rtn = GTN_SetGroupProfileCoordinateSystem(core,group,COORD_SYSTEM_PCS,NULL);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetGroupProfileCoordinateSystem()",rtn);
}
TGroupMoveParameter groupMovePrm; // group运动指令参数
double pos[8] = {0,0,0,0,0,0,0,0}; // 指令流运动位移,单位mm
short dir[8] = {0,0,0,0,0,0,0,0}; // 指令流运动方向
double endPoint[8] = {0,0,0,0,0,0,0,0}; // 圆弧终点位置
TListInfo listInfo; // 指令流信息
short runStatus; // 运行状态，0：group与规划轴全部停止运动，1：group或者规划轴没有停止运动
short list; // 指令流号
list = 1; // 指令流号为1
memset(&groupMovePrm,0,sizeof(groupMovePrm));
groupMovePrm.velocity = 10; // 指令流运动速度10，单位：mm/s
groupMovePrm.acceleration = 500; // 指令流运动加速度500,单位：mm/s^2
groupMovePrm.deceleration = 500; // 指令流运动减速度500,单位：mm/s^2
memset(&circularParameter, 0, sizeof(circularParameter));
circularParameter.arcMode = CIRCULAR_MODE_SPACE_BORDER;
memset(&listInfo,0,sizeof(listInfo));
listInfo.list = list; // 指令流号为1
// 清除指令数据与状态
rtn = GTN_ClearCommandListData(core,list,NULL);
if ( 0 != rtn )
{
return CommandHandler("GTN_ClearCommandListData()",rtn);
}
rtn = GTN_ClearCommandListStatus(core,list,NULL);
if ( 0 != rtn )
{
return CommandHandler("GTN_ClearCommandListStatus()",rtn);
}
listInfo.segNum++; // 指令段号加1
listInfo.modal = 1; // 指令类型为模态指令，1：模态指令，0：非模态指令
pos[0] = 100; // group组运动中X轴的运动目标位置为 100，单位：mm
pos[1] = 0; // group组运动中Y轴的运动目标位置为 0，单位：mm
pos[2] = 0; // group组运动中Z轴的运动目标位置为 0，单位：mm
pos[3] = 0; // group组运动中A轴的旋转角度为 0，单位：°
pos[4] = 0; // group组运动中C轴的旋转角度为 0，单位：°
rtn = GTN_MoveLinearAbsolute(core,group,pos,dir,&groupMovePrm,&listInfo);
if ( 0 != rtn )
{
return CommandHandler("GTN_MoveLinearAbsolute()",rtn);
}
TPosCompareEnablePro posCompareEnable;
memset(&posCompareEnable,0,sizeof(posCompareEnable));
posCompareEnable.index = posCompareIndex;
posCompareEnable.enable = 1;
listInfo.segNum++;
rtn = GTN_SetPosCompareEnablePro(core,group,&posCompareEnable,&listInfo);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetPosCompareEnablePro()",rtn);
}
listInfo.segNum++; //修改指令流段号
pos[0] = 218.300529;
pos[1] = -157.784038;
pos[2] = -110.705167;
pos[3] = 7.000000;
pos[4] = 6.000000;
rtn = GTN_MoveLinearAbsolute(core,group,pos,dir,&groupMovePrm,&listInfo); //直线插补运动
if ( 0 != rtn )
{
return CommandHandler("GTN_MoveLinearAbsolute()",rtn);
}
posCompareEnable.enable = 0;
listInfo.segNum++;
rtn = GTN_SetPosCompareEnablePro(core,group,&posCompareEnable,&listInfo);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetPosCompareEnablePro()",rtn);
}
//将指令压下去
do{
rtn = GTN_CommandListDataEnd(core,list);
}while( 10700 == rtn );
if ( 0 != rtn )
{
return CommandHandler("GTN_CommandListDataEnd()",rtn);
}
//启动指令流
rtn = GTN_StartCommandList(core,list,NULL);
if ( 0 != rtn )
{
return CommandHandler("GTN_StartCommandList()",rtn);
}
do
{
// 运动过程中获取Group与规划轴的位置信息与状态
rtn = GetListAndGroupMotionInfo(core,list,group,axis,axisCount,posCompareIndex,&runStatus);
if ( 0 != rtn )
{
return rtn;
}
} while ( 1 == runStatus);
// 关闭位置比较功能
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;
}