// PosCompareAddition-GSHD.cpp : 定义控制台应用程序的入口点。
//
#include "stdafx.h"
#include <math.h>
// 该例程仅用于功能演示，请保证安全的情况下使用
// 测试功能：位置比较叠加功能二维pso模式示例，硬件连接方式为GSN+GSHD+GSHD+GSHD+GSHD+GNM403+GNM403
// 测试平台：网络型运动控制器
// 测试环境：Windows
// 背景描述：（1）双工位机床
// （2）工位1为主工位，插补运动，为参考坐标系,标记为（x1,y1）
// （3）工位2为从工位，同步坐标系补偿运动，为同步坐标系，标记为（x2,y2）
// （4）x2轴为独立轴
// （5）y2为非独立轴，安装在y1上。即y1运动时，会带着y2一起移动
// （6）同步坐标系配置pso功能时，y2轴必须配置叠加轴，比较的轨迹才是真实空间轨迹
// 测试流程：
// （1）初始化控制器
// （2）建立插补坐标系，即参考坐标系
// （3）配置同步坐标系补偿功能（包括将同步坐标系的轴移动到定位点）
// （4）配置参考坐标系的位置比较pso功能
// （5）配置同步坐标系的位置比较pso功能，即位置比较叠加功能
// （6）压插补数据
// （7）启动插补运动
// （8）检测插补是否运动完成
// （9）运动完成，关闭控制器
// 注意事项：
// （1）本例程使用的“例程专用.xml”、“例程专用.cfg”，仅用于本例程
// （2）实际使用时，需要使用MotionStudio生成网络配置xml
// （3）实际使用时，必须确认网络上接了支持位置比较功能的从站！！！
// （4）！！！参考坐标系编码器必须在接在第一个403的1、2轴上，同步坐标系编码器必须在第二个403的1、2轴上，参考坐标系y轴编码器必须在第二个403的3轴上！！！
// （4）！！！参考坐标系编码器必须在接在第一个403的1、2轴上，同步坐标系编码器必须在第二个403的1、2轴上，参考坐标系y轴编码器必须在第二个403的3轴上！！！
// （4）！！！参考坐标系编码器必须在接在第一个403的1、2轴上，同步坐标系编码器必须在第二个403的1、2轴上，参考坐标系y轴编码器必须在第二个403的3轴上！！！
// （4）！！！参考坐标系编码器必须在接在第一个403的1、2轴上，同步坐标系编码器必须在第二个403的1、2轴上，参考坐标系y轴编码器必须在第二个403的3轴上！！！
// （4）！！！参考坐标系编码器必须在接在第一个403的1、2轴上，同步坐标系编码器必须在第二个403的1、2轴上，参考坐标系y轴编码器必须在第二个403的3轴上！！！
// （4）！！！参考坐标系编码器必须在接在第一个403的1、2轴上，同步坐标系编码器必须在第二个403的1、2轴上，参考坐标系y轴编码器必须在第二个403的3轴上！！！
// 加载固高运动控制库头文件
#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;
}
/**
* @brief 初始化插补坐标系和前瞻
* @param core 需要初始化的插补坐标系所在的核号，从1开始
* @param crd 需要初始化的插补坐标系号，从1开始
* @param fifo 需要初始化的插补坐标系缓冲区号
* @return 0表示初始化成功，非0表示初始化失败
*/
short InitCrdAndLa(short core,short crd,short fifo)
{
short rtn;
short i;
TCrdPrm crdPrm; // 插补坐标系参数
EMachineMode machineMode; // 插补坐标系机床模式
TLookAheadParameter laPrm; // 前瞻参数
short motionMode; // 前瞻运行模式
// 建立插补坐标系
memset(&crdPrm,0,sizeof(crdPrm)); // 将结构体参数全部初始化为0
crdPrm.dimension = 2; // 二维坐标系
crdPrm.profile[0] = CRD_AXIS_X; // x轴，此处为规划器1为坐标系的x轴
crdPrm.profile[1] = CRD_AXIS_Y; // y轴，此处为规划器2为坐标系的y轴
crdPrm.synVelMax = 500; // 最大合成速度，单位：脉冲/ms
crdPrm.synAccMax = 200; // 最大合成加速度，单位：脉冲/ms^2
crdPrm.evenTime = 0; // 最小匀速时间，单位：ms
crdPrm.setOriginFlag = 0; // 坐标系原点设置标志，0：使用当前规划位置作为坐标系原点
// 1：使用crdPrm.originPos[8]中设置的位置作为坐标系原点
rtn = GTN_SetCrdPrm(core,crd,&crdPrm);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetCrdPrm",rtn);
}
// 建立前瞻坐标系
machineMode = NORMAL_THREE_AXIS; // 标准三轴机床模式
rtn = GTN_SetupLookAheadCrd(core,crd,machineMode);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetupLookAheadCrd",rtn);
}
// 初始化前瞻参数
memset(&laPrm,0,sizeof(laPrm)); // 将结构体参数全部初始化为0
laPrm.lookAheadNum = 200; // 前瞻段数，前瞻工作区每攒够当前数量的运动段就会进行一次预处理，输出处理后的数据
laPrm.time = 0.001; // 时间常数，单位：ms
laPrm.radiusRatio = 1.5; // 曲率限制调节系数
for (i=0;i<8;++i) // 数据索引，i=0表示x轴，i=1表示y轴，i=7表示w轴
{
laPrm.vMax[i] = 500; // 当前轴最大速度限制，单位：mm/s
laPrm.aMax[i] = 2000; // 当前轴最大加速度限制，单位：mm/s^2
laPrm.DVMax[i] = 300; // 当前轴在时间常数内最大速度变化量
laPrm.scale[i] = 1000; // 当前轴当量，单位：脉冲/mm
laPrm.axisRelation[i] = i+1; // 一般按此配置，无需修改
}
motionMode = 0; // 保留参数，必须为0
// TPreStartPos为保留参数，必须为NULL
rtn = GTN_InitLookAheadEx(core,crd,&laPrm,fifo,motionMode,NULL);
if ( 0 != rtn )
{
return CommandHandler("GTN_InitLookAheadEx",rtn);
}
// 清空插补缓冲区
rtn = GTN_CrdClear(core,crd,fifo);
if ( 0 != rtn )
{
return CommandHandler("GTN_CrdClear",rtn);
}
printf("Init Crd And La Success !\n");
return 0;
}
/**
* @brief 将轴点位运动到目标位置
* @param core 核号，从1开始
* @param profile 规划器索引，从1开始
* @param targetVel 目标速度
* @param targetPos 目标位置
* @return 0表示成功，非0表示失败
*/
short TrapMove(short core,short profile,double targetVel,long targetPos)
{
short rtn;
TTrapPrm trap;
memset(&trap,0,sizeof(trap));
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);
}
long sts;
double prfPos;
do
{
rtn = GTN_GetSts(core,profile,&sts,1,0);
rtn += GTN_GetPrfPos(core,profile,&prfPos,1,0);
if ( 0 != rtn )
{
return CommandHandler("GTN_GetSts",rtn);
}
printf("Axis:%d status:0x%#x prfPos:%.3lf \r",profile,sts,prfPos);
} 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;
}
}
/**
* @brief 配置同步坐标系补偿功能（包括将同步坐标系的轴移动到定位点）
* @param core 需要配置功能的核号，从1开始
* @param index 同步坐标系补偿功能索引，从1开始
* @param refAxisX 参考坐标系x1轴的轴号
* @param refAxisY 参考坐标系y1轴的轴号
* @param compAxisX 同步坐标系x2轴的轴号
* @param compAxisY 同步坐标系y2轴的轴号
* @return 0表示配置成功，非0表示配置失败
*/
short SetCoordSyncCompFunction(short core,short index,short refAxisX,short refAxisY,short compAxisX,short compAxisY)
{
short rtn;
TCoordSyncCompPrm compPrm;
memset(&compPrm,0,sizeof(compPrm));
// 配置同步坐标系位置关系
compPrm.enable = 1;
compPrm.refType = MC_PROFILE; // 参考轴类型为规划器
compPrm.refIndex[0] = refAxisX; // 参考坐标系x1轴的轴号
compPrm.refIndex[1] = refAxisY; // 参考坐标系y1轴的轴号
// 参考坐标系与机床坐标系之间的变换关系
// 当前例程参考坐标系就是机床坐标系
compPrm.refTrans.translateion[0] = 0; // 参考坐标系与机床坐标系原点之间的x方向平移量，单位：脉冲
compPrm.refTrans.translateion[1] = 0; // 参考坐标系与机床坐标系原点之间的y方向平移量，单位：脉冲
compPrm.refTrans.theta = 0; // 参考坐标系与机床坐标系原点之间的旋转角度，单位：度
// 同步坐标系与机床坐标系之间的变换关系
compPrm.syncTrans.translateion[0] = 15000; // 同步坐标系与机床坐标系原点之间的x方向平移量，单位：脉冲
compPrm.syncTrans.translateion[1] = 10000; // 同步坐标系与机床坐标系原点之间的y方向平移量，单位：脉冲
compPrm.syncTrans.theta = 30; // 同步坐标系与机床坐标系原点之间的旋转角度，单位：度
compPrm.offset[0] = 15000; // 同步坐标系与参考坐标系原点之间的x方向平移量，单位：脉冲
compPrm.offset[1] = 10000; // 同步坐标系与参考坐标系原点之间的y方向平移量，单位：脉冲
rtn = GTN_SetCoordSyncCompPrm(core,index,&compPrm);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetCoordSyncCompPrm",rtn);
}
// 获取当前参考坐标系位置对应的同步坐标系位置
double refPosX; // 参考坐标系x轴的规划位置
double refPosY; // 参考坐标系y轴的规划位置
double compPosX; // 同步坐标系x轴对应的规划位置
double compPosY; // 同步坐标系y轴对应的规划位置
rtn = GTN_GetPrfPos(core,refAxisX,&refPosX,1,0);
rtn += GTN_GetPrfPos(core,refAxisY,&refPosY,1,0);
rtn += GTN_GetCoordSyncCompValue(core,index,refPosX,refPosY,&compPosX,&compPosY);
if ( 0 != rtn )
{
return CommandHandler("GTN_GetCoordSyncCompValue",rtn);
}
// 将同步坐标系的x2和y2轴移动到起点位置
// 在使能补偿前先移动到对应位置，可以避免使能瞬间的位置突跳
// x2位独立轴，起点位置为“x1位置+补偿值”
rtn = TrapMove(core,compAxisX,50,(long)(refPosX+compPosX));
if ( 0 != rtn )
{
return rtn;
}
// y2位叠加轴，起点位置为“补偿值”
rtn = TrapMove(core,compAxisY,50,(long)compPosY);
if ( 0 != rtn )
{
return rtn;
}
// 配置同步坐标系补偿轴
TPrfComp prfComp;
memset(&prfComp,0,sizeof(prfComp));
prfComp.type = COMP_TYPE_COORD_SYNC; // 补偿轴类型为同步坐标系
prfComp.index = index; // 同步坐标系变换索引
prfComp.subIndex = CRD_AXIS_X; // 当前轴为同步坐标系的x轴
rtn = GTN_PrfComp(core,compAxisX,&prfComp);
if ( 0 != rtn )
{
return CommandHandler("GTN_PrfComp",rtn);
}
prfComp.type = COMP_TYPE_COORD_SYNC; // 补偿轴类型为同步坐标系
prfComp.index = index; // 同步坐标系变换索引
prfComp.subIndex = CRD_AXIS_Y; // 当前轴为同步坐标系的y轴
rtn = GTN_PrfComp(core,compAxisY,&prfComp);
if ( 0 != rtn )
{
return CommandHandler("GTN_PrfComp",rtn);
}
printf("Set Coordinate Synchronization Compensation Success !\n");
return rtn;
}
/**
* @brief 配置参考坐标系位置比较pso功能
* @param core 需要配置功能的核号，从1开始
* @param posCompareIndex 需要配置的位置比较索引，从1开始
* @param posCompareEncX 位置比较源x轴编码器索引
* @param posCompareEncY 位置比较源y轴编码器索引
* @param refAxisX 参考坐标系x1轴的轴号
* @param refAxisY 参考坐标系y1轴的轴号
* @return 0表示配置成功，非0表示配置失败
*/
short SetRefCrdPosComparePsoMode(short core,short posCompareIndex,short posCompareEncX,short posCompareEncY,short refAxisX,short refAxisY)
{
short rtn;
short station; // 支持波形控制功能的模块的逻辑站号
short permit[2]; // 控制权
TPosCompareModeEx posCompareModeEx;
TPosComparePsoPrm posComparePsoPrm;
TPosCompareReferencePrm posCompareRefPrm;
memset(&posCompareModeEx,0,sizeof(posCompareModeEx));
memset(&posComparePsoPrm,0,sizeof(posComparePsoPrm));
memset(&posCompareRefPrm,0,sizeof(posCompareRefPrm));
// 配置hso口控制权
station = 5; // ！！！参考坐标系编码器必须接在第一个403上，站号为5！！！
// ！！！参考坐标系编码器必须接在第一个403上，站号为5！！！
// ！！！参考坐标系编码器必须接在第一个403上，站号为5！！！
permit[0] = 0x2; // 第一个hso口输出第一路位置比较
permit[1] = 0x4; // 第二个hso口输出第二路位置比较
rtn = GTN_SetTerminalPermitEx(core,station,MC_HSO,&permit[0],1,2);
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 = 3; // pso模式为3
posCompareModeEx.dimension = 2; // 二维
posCompareModeEx.sourceMode = 0; // 比较源：0：编码器，1：脉冲计数器，2：辅助编码器
posCompareModeEx.source[0] = posCompareEncX; // x轴比较源为参考坐标系x1轴的编码器
posCompareModeEx.source[1] = posCompareEncY; // y轴比较源为参考坐标系y1轴的编码器
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);
}
// 设置位置比较参考轴
// 参考轴是位置比较编码器对应的规划轴，用于获取位置信息与螺距误差等补偿表
// ！！！必须在配置了位置比较模式之后，再设置参考轴！！！
// ！！！必须在配置了位置比较模式之后，再设置参考轴！！！
// ！！！必须在配置了位置比较模式之后，再设置参考轴！！！
posCompareRefPrm.referenceX = refAxisX; // 位置比较x轴对应的参考轴，即第一个GSHD对应的逻辑轴号
posCompareRefPrm.referenceY = refAxisY; // 位置比较y轴对应的参考轴，即第二个GSHD对应的逻辑轴号
rtn = GTN_SetPosCompareReference(core,posCompareIndex,&posCompareRefPrm);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetPosCompareReference",rtn);
}
// 设置位置比较pso间距
posComparePsoPrm.gpo = 0; // 保留值，暂时无意义
posComparePsoPrm.hso = 0x1; // 保留值，暂时无意义
posComparePsoPrm.syncPos = 100; // pso输出等间距值，单位：脉冲
posComparePsoPrm.time = 0; // 保留值，暂时无意义
posComparePsoPrm.count = 1; // 保留值，暂时无意义，且必须大于0
rtn = GTN_SetPosComparePsoPrm(core,posCompareIndex,&posComparePsoPrm);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetPosComparePsoPrm",rtn);
}
// 启动位置比较功能
rtn = GTN_PosCompareStart(core,posCompareIndex);
if ( 0 != rtn )
{
return CommandHandler("GTN_PosCompareStart",rtn);
}
printf("Set Ref Crd PosCompare Pso Mode Success !\n");
return rtn;
}
/**
* @brief 配置同步坐标系位置比较pso功能
* @param core 需要配置功能的核号，从1开始
* @param posCompareIndex 需要配置的位置比较索引，从1开始
* @param posCompareEncX 位置比较源x轴编码器索引
* @param posCompareEncY 位置比较源y轴编码器索引
* @param compAxisX 同步坐标系x2轴的轴号
* @param compAxisY 同步坐标系y2轴的轴号
* @return 0表示配置成功，非0表示配置失败
*/
short SetSyncCrdPosCompareAdditionPsoMode(short core,short posCompareIndex,short posCompareEncX,short posCompareEncY,short compAxisX,short compAxisY)
{
short rtn;
short station; // 支持波形控制功能的模块的逻辑站号
short permit[2]; // 控制权
TPosCompareModeEx posCompareModeEx;
TPosComparePsoPrm posComparePsoPrm;
TPosCompareReferencePrm posCompareRefPrm;
TPosCompareAdditionPrm posCompareAdd;
memset(&posCompareModeEx,0,sizeof(posCompareModeEx));
memset(&posComparePsoPrm,0,sizeof(posComparePsoPrm));
memset(&posCompareRefPrm,0,sizeof(posCompareRefPrm));
memset(&posCompareAdd,0,sizeof(posCompareAdd));
// 配置hso口控制权
station = 6; // ！！！同步坐标系编码器必须接在第二个403上，站号为6！！！
// ！！！同步坐标系编码器必须接在第二个403上，站号为6！！！
// ！！！同步坐标系编码器必须接在第二个403上，站号为6！！！
permit[0] = 0x2; // 第一个hso口输出第一路位置比较
permit[1] = 0x4; // 第二个hso口输出第二路位置比较
rtn = GTN_SetTerminalPermitEx(core,station,MC_HSO,&permit[0],1,2);
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 = 3; // pso模式为3
posCompareModeEx.dimension = 2; // 二维
posCompareModeEx.sourceMode = 0; // 比较源：0：编码器，1：脉冲计数器，2：辅助编码器
posCompareModeEx.source[0] = posCompareEncX; // x轴比较源为同步坐标系x2轴的编码器
posCompareModeEx.source[1] = posCompareEncY; // y轴比较源为同步坐标系y2轴的编码器
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);
}
// 设置位置比较参考轴
// 参考轴是位置比较编码器对应的规划轴，用于获取位置信息与螺距误差等补偿表
// ！！！必须在配置了位置比较模式之后，再设置参考轴！！！
// ！！！必须在配置了位置比较模式之后，再设置参考轴！！！
// ！！！必须在配置了位置比较模式之后，再设置参考轴！！！
posCompareRefPrm.referenceX = compAxisX; // 位置比较x轴对应的参考轴，即第三个GSHD对应的逻辑轴号
posCompareRefPrm.referenceY = compAxisY; // 位置比较y轴对应的参考轴，即第四个GSHD对应的逻辑轴号
rtn = GTN_SetPosCompareReference(core,posCompareIndex,&posCompareRefPrm);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetPosCompareReference",rtn);
}
// 设置同步坐标系位置比较轴叠加关系
// 同步坐标系中x2轴为独立轴，不需要叠加
// 同步坐标系中y2轴为叠加轴，必须要叠加
// ！！！必须在设置完模式和参考轴后设置叠加轴关系！！！
// ！！！必须在设置完模式和参考轴后设置叠加轴关系！！！
// ！！！必须在设置完模式和参考轴后设置叠加轴关系！！！
posCompareAdd.sourceMode = 0;
posCompareAdd.additionX = 0; // 同步坐标系中x2为独立轴，不需要配置叠加关系
posCompareAdd.additionY = 11; // 同步坐标系中y2为叠加轴，必须要配置叠加关系
// ！！！参考坐标系y1轴的编码器必须要接到第二个403的“AXIS3”通道上！！！
// ！！！参考坐标系y1轴的编码器必须要接到第二个403的“AXIS3”通道上！！！
// ！！！参考坐标系y1轴的编码器必须要接到第二个403的“AXIS3”通道上！！！
rtn = GTN_SetPosCompareAddition(core,posCompareIndex,&posCompareAdd);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetPosCompareAddition",rtn);
}
// 设置位置比较pso间距
posComparePsoPrm.gpo = 0; // 保留值，暂时无意义
posComparePsoPrm.hso = 0x1; // 保留值，暂时无意义
posComparePsoPrm.syncPos = 100; // pso输出等间距值，单位：脉冲
posComparePsoPrm.time = 0; // 保留值，暂时无意义
posComparePsoPrm.count = 1; // 保留值，暂时无意义，且必须大于0
rtn = GTN_SetPosComparePsoPrm(core,posCompareIndex,&posComparePsoPrm);
if ( 0 != rtn )
{
return CommandHandler("GTN_SetPosComparePsoPrm",rtn);
}
// 启动位置比较功能
rtn = GTN_PosCompareStart(core,posCompareIndex);
if ( 0 != rtn )
{
return CommandHandler("GTN_PosCompareStart",rtn);
}
printf("Set Sync Crd PosCompare Addition Pso Mode 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;
}
int _tmain(int argc, _TCHAR* argv[])
{
short rtn; // 指令返回值
short core; // 需要执行例程的运动控制器核号
short axis; // 需要初始化的轴起始索引号
short axisCount; // 需要初始化的轴数量，从轴起始索引号开始算起
// 初始化运动控制器
// 开卡 + 初始化网络拓扑 + 初始化核1的1-12轴
core = 1;
axis = 1;
axisCount = 12;
rtn = InitMc(core,axis,axisCount);
if ( 0 != rtn )
{
return rtn;
}
short crd; // 插补坐标系号
short fifo; // 插补数据使用的缓冲区号
// 初始化插补坐标系和前瞻
crd = 1; // 指定测试的插补坐标系号为1
fifo = 0; // 指定测试的插补坐标系缓冲区号为0
rtn = InitCrdAndLa(core,crd,fifo);
if ( 0 != rtn )
{
return rtn;
}
short index; // 同步坐标系补偿转换关系索引
short refAxisX; // 参考坐标系x1轴的轴号
short refAxisY; // 参考坐标系y1轴的轴号
short compAxisX; // 同步坐标系x2轴的轴号
short compAxisY; // 同步坐标系y2轴的轴号
// 配置同步坐标系补偿功能（包括将同步坐标系的轴移动到定位点）
index = 1;
refAxisX = 1; // 参考坐标系x1轴为第一个GSHD，并且编码器接到第一个403的“AXIS1”口上
refAxisY = 2; // 参考坐标系y1轴为第二个GSHD，并且编码器接到第一个403的“AXIS2”口上，同时接到第二个403的“AXIS3”口上
compAxisX = 3; // 同步坐标系x2轴为第三个GSHD，并且编码器接到第二个403的“AXIS1”口上
compAxisY = 4; // 同步坐标系y2轴为第四个GSHD，并且编码器接到第二个403的“AXIS2”口上
rtn = SetCoordSyncCompFunction(core,index,refAxisX,refAxisY,compAxisX,compAxisY);
if ( 0 != rtn )
{
return rtn;
}
short refCompareIndex; // 参考坐标系位置比较索引
short syncCompareIndex; // 同步坐标系位置比较索引
short refCompareEnc[2]; // 参考坐标系位置比较源编码器号索引
short compCompareEnc[2]; // 同步坐标系位置比较源编码器号索引
// 配置参考坐标系位置比较pso功能
refCompareIndex = 1; // 第一个403上的第一路位置比较逻辑索引为1
refCompareEnc[0] = 5; // 参考坐标系位置比较x轴的编码器索引，必须和位置比较功能在一个站上
refCompareEnc[1] = 6; // 参考坐标系位置比较y轴的编码器索引，必须和位置比较功能在一个站上
rtn = SetRefCrdPosComparePsoMode(core,refCompareIndex,refCompareEnc[0],refCompareEnc[1],refAxisX,refAxisY);
if ( 0 != rtn )
{
return rtn;
}
// 配置同步坐标系位置比较pso功能
syncCompareIndex = 3; // 第二个403上的第一路位置比较逻辑索引为3
compCompareEnc[0] = 9; // 同步坐标系位置比较x轴的编码器索引，必须和位置比较功能在一个站上
compCompareEnc[1] = 10; // 同步坐标系位置比较y轴的编码器索引，必须和位置比较功能在一个站上
rtn = SetSyncCrdPosCompareAdditionPsoMode(core,syncCompareIndex,compCompareEnc[0],compCompareEnc[1],compAxisX,compAxisY);
if ( 0 != rtn )
{
return rtn;
}
double endPos[2]; // 插补数据段目标位置
double synVel; // 插补数据段合成目标速度
double synAcc; // 插补数据段合成加速度
long segNum; // 插补数据段用户段号
short override2; // 插补运动倍率选择标志
// 压插补数据，包含pso开关指令
endPos[0] = 0; // 初始化x轴插补数据段目标位置为0
endPos[1] = 0; // 初始化y轴插补数据段目标位置为0
synVel = 100; // 插补数据段合成目标速度，单位：mm/s
synAcc = 50; // 插补数据段合成目标加速度，单位：mm/s^2
segNum = 0; // 初始化插补数据段段号为0
override2 = 0; // 插补运动倍率选择标志，0：选择第一个倍率，1：选择第二个倍率
rtn = GTN_BufPrfCompEnableEx(core,crd,fifo,compAxisX,1,3); // 同步坐标系x2使能独立轴，定位补偿模式
rtn += GTN_BufPrfCompEnableEx(core,crd,fifo,compAxisY,1,1); // 同步坐标系y2使能非独立轴，定位补偿模式
endPos[0] = 5; endPos[1] = 0; segNum = 1;
rtn = GTN_LnXYEx(core,crd,endPos[0],endPos[1],synVel,synAcc,segNum,override2,fifo);
rtn += GTN_BufPosCompareStartEx(core,crd,fifo,refCompareIndex);
rtn += GTN_BufPosCompareStartEx(core,crd,fifo,syncCompareIndex);
endPos[0] = 5; endPos[1] = 5; segNum += 1;
rtn += GTN_LnXYEx(core,crd,endPos[0],endPos[1],synVel,synAcc,segNum,override2,fifo);
endPos[0] = -5; endPos[1] = 5; segNum += 1;
rtn += GTN_LnXYEx(core,crd,endPos[0],endPos[1],synVel,synAcc,segNum,override2,fifo);
endPos[0] = -5; endPos[1] = -5; segNum += 1;
rtn += GTN_LnXYEx(core,crd,endPos[0],endPos[1],synVel,synAcc,segNum,override2,fifo);
endPos[0] = 5; endPos[1] = -5; segNum += 1;
rtn += GTN_LnXYEx(core,crd,endPos[0],endPos[1],synVel,synAcc,segNum,override2,fifo);
endPos[0] = 5; endPos[1] = 0; segNum += 1;
rtn += GTN_LnXYEx(core,crd,endPos[0],endPos[1],synVel,synAcc,segNum,override2,fifo);
rtn += GTN_BufPosCompareStopEx(core,crd,fifo,refCompareIndex);
rtn += GTN_BufPosCompareStopEx(core,crd,fifo,syncCompareIndex);
endPos[0] = 0; endPos[1] = 0; segNum += 1;
rtn += GTN_LnXYEx(core,crd,endPos[0],endPos[1],synVel,synAcc,segNum,override2,fifo);
rtn += GTN_BufPrfCompEnableEx(core,crd,fifo,compAxisX,0,3); // 关闭同步坐标系x2使能
rtn += GTN_BufPrfCompEnableEx(core,crd,fifo,compAxisY,0,1); // 关闭同步坐标系y2使能
if ( 0 != rtn )
{
return CommandHandler("PushCrdData",rtn);
}
do
{
rtn = GTN_CrdDataEx(core,crd,NULL,fifo);
} while ( 1 == rtn);
// 启动插补运动
rtn = GTN_CrdStart(core,1<<(crd-1),fifo<<(crd-1));
if ( 0 != rtn )
{
return CommandHandler("GTN_CrdStart",rtn);
}
printf("Start Crd Success !\n");
short crdRun; // 插补运动状态
do
{
// 运动过程中读取当前状态
rtn = GetMotionInfo(core,crd,fifo,refCompareIndex,syncCompareIndex,&crdRun);
if ( 0 != rtn )
{
return rtn;
}
} while ( 1 == crdRun );
// 关闭位置比较功能
rtn = GTN_PosCompareStop(core,refCompareIndex);
rtn += GTN_PosCompareStop(core,syncCompareIndex);
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;
}
