// ParsingFileCrdMotion.cpp : 定义控制台应用程序的入口点。
//
#include "stdafx.h"
// 该例程仅用于功能演示，请保证安全的情况下使用
// 测试功能：从文件中解析数据插补运动示例
// 测试平台：网络型运动控制器
// 测试环境：Windows
// 测试流程：
// （1）初始化控制器
// （2）建立插补坐标系，规划器1为x轴，规划器2为y轴
// （3）初始化前瞻
// （4）开启DMA传送数据功能
// （5）压插补数据，直到满足启动条件
// （6）启动插补坐标系运动
// （7）继续压剩余插补数据
// （8）检测插补是否运动完成
// （9）运动完成，关闭控制器
// 注意事项：
// （1）本例程使用的“例程专用.xml”、“例程专用.cfg”，仅用于本例程
// （2）实际使用时，需要使用MotionStudio生成网络配置xml
// 加载固高运动控制库头文件
#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 插补坐标系所在的核号
* @param crd 插补坐标系号
* @param fifo 插补坐标系缓存区号
* @param pCrdRun 插补坐标系运动状态变量指针
* @return 0表示获取插补运动状态成功，非0表示获取插补运动状态出错
*/
short GetMotionInfo(short core,short crd,short fifo,short *pCrdRun)
{
short rtn;
long crdExecuteSegNum; // 插补已经执行的段数
long crdRemainder; // 插补剩余未执行段数
double crdPos[8]; // 插补规划位置
// 运动过程中读取当前状态
rtn = GTN_CrdStatus(core,crd,pCrdRun,&crdExecuteSegNum,fifo);
rtn += GTN_GetCrdPos(core,crd,&crdPos[0]);
if ( 0 != rtn )
{
return CommandHandler("GTN_CrdStatus",rtn);
}
printf("crdRun = %d, crdExecuteSegNum = %d, xPos = %lf, yPos = %lf\r",*pCrdRun,crdExecuteSegNum,crdPos[0],crdPos[1]);
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; // 需要初始化的轴数量，从轴起始索引号开始算起
FILE *pDataFile; // 插补数据文件指针
pDataFile = fopen("插补数据(xPos,yPos,SegNum).txt","r");
if ( NULL == pDataFile )
{
return CommandHandler("Open file \"插补数据(xPos,yPos,SegNum).txt\" failed",0);
}
// 初始化运动控制器
// 开卡 + 初始化网络拓扑 + 初始化核1的1-8轴
core = 1;
axis = 1;
axisCount = 8;
rtn = InitMc(core,axis,axisCount);
if ( 0 != rtn )
{
return rtn;
}
short i;
short crd; // 插补坐标系号
short fifo; // 插补数据使用的缓冲区号
TCrdPrm crdPrm; // 插补坐标系参数
EMachineMode machineMode; // 插补坐标系机床模式
TLookAheadParameter laPrm; // 前瞻参数
short motionMode; // 前瞻运行模式
short dmaBuf; // DMA功能通道号
unsigned short dmaThreshold; // DMA功能阈值
short lookAheadInMc; // 前瞻预处理执行模式
double endPos[2]; // 插补数据段目标位置
double synVel; // 插补数据段合成目标速度
double synAcc; // 插补数据段合成加速度
long segNum; // 插补数据段用户段号
short override2; // 插补运动倍率选择标志
short crdStartFlag; // 启动过插补运动标识
short crdStartSegNum; // 启动插补运动数据段数，当控制器接收到设定段数的插补数据时，开始启动运动
short endOfFile; // 文件结束标志，0：文件解析未结束，1：文件解析结束
long crdSpace; // 插补剩余空间
long crdRemainder; // 控制器剩余还未执行的插补数据段数
long pushCount; // 设定每次连续压插补数据数量
long realPushCount; // 实际每次连续压插补数据数量
short crdRun; // 插补运动状态
crd = 1; // 指定测试的插补坐标系号为1
fifo = 0; // 指定测试的插补坐标系缓冲区号为0
// 注意：（1）每套插补坐标系只有fifo=0时支持前瞻，fifo=1时不支持前瞻
// （2）fifo=0一般用于轨迹加工
// （3）fifo=1用于辅助fifo=0，例如加工过程中需要暂停一会儿去走其他轨迹，可以用fifo=1实现
// 建立插补坐标系
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 = 10; // 最大合成加速度，单位：脉冲/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);
}
// 开启插补数据DMA传输功能
dmaBuf = 1; // DMA通道号，取值范围：[1,4]
dmaThreshold = 200; // DMA阈值，表示DMA缓存区每攒够阈值段数据，在控制器有空间的情况下，批量发送一批数据到控制器
lookAheadInMc = 0; // 前瞻预处理执行模式，0：前瞻预处理在库中执行，目前仅支持该模式
rtn = GTN_CrdHsOn(core,crd,fifo,dmaBuf,dmaThreshold,lookAheadInMc);
if ( 0 != rtn )
{
return CommandHandler("GTN_CrdHsOn",rtn);
}
printf("Init Crd Success !\n");
// 启动插补运动前压数据流程
crdStartFlag = 0; // 插补运动启动标识，0：还未启动过运动，1：已经启动了运动
crdStartSegNum = 1000; // 当控制器接收到超过当前设置的插补数据时，开始启动运动
endOfFile = 0; // 文件解析未结束
pushCount = 200; // 每次连续压入插补数据段数
endPos[0] = 0; // 初始化x轴插补数据段目标位置为0
endPos[1] = 0; // 初始化y轴插补数据段目标位置为0
synVel = 500; // 插补数据段合成目标速度，单位：mm/s
synAcc = 50; // 插补数据段合成目标加速度，单位：mm/s^2
segNum = 0; // 初始化插补数据段段号为0
override2 = 0; // 插补运动倍率选择标志，0：选择第一个倍率，1：选择第二个倍率
// 建议插补启动段数小于等于2000
if ( crdStartSegNum > 2000 )
{
return CommandHandler("Error crdStartSegNum over 2000",crdStartSegNum);
}
while ( 0 == endOfFile )
{
rtn = GTN_CrdSpace(core,crd,&crdSpace,fifo);
if ( 0 != rtn )
{
return CommandHandler("GTN_CrdSpace",rtn);
}
realPushCount = pushCount;
// 剩余空间不足，当前次压数最多压剩余空间数量
if ( realPushCount > crdSpace )
{
realPushCount = crdSpace;
}
for (i=0;i<realPushCount;++i)
{
if ( !feof(pDataFile) )
{
fscanf(pDataFile,"%lf,%lf,%ld\n",&endPos[0],&endPos[1],&segNum);
rtn = GTN_LnXYEx(core,crd,endPos[0],endPos[1],synVel,synAcc,segNum,override2,fifo);
// 如果压数据指令报错，并且错误码不是相同点（102），表示执行出错，停止继续压数，查找错误原因
if ( ( 0 != rtn ) && ( 102 != rtn ) )
{
return CommandHandler("GTN_LnXYEx",rtn);
}
}
else
{
fclose(pDataFile);
endOfFile = 1;
break;
}
}
// 插补运动还没启动过，需要检查是否满足启动条件
if ( 0 == crdStartFlag )
{
// 查询运动控制器剩余未执行的插补数据段数
rtn = GTN_GetRemainderSegNum(core,crd,&crdRemainder,fifo);
if ( 0 != rtn )
{
return CommandHandler("GTN_GetRemainderSegNum",rtn);
}
// 运动控制器中剩余未执行段数大于等于启动段数，启动插补运动
if ( crdRemainder >= crdStartSegNum )
{
// 运动控制器中剩余未执行段数大于等于启动段数，启动插补运动
rtn = GTN_CrdStart(core,1<<(crd-1),fifo<<(crd-1));
if ( 0 != rtn )
{
return CommandHandler("GTN_CrdStart",rtn);
}
printf("Start Crd Success !\n");
crdStartFlag = 1;
}
}
// 插补运动已经启动过，每次批量压后，获取一次当前状态
else
{
rtn = GetMotionInfo(core,crd,fifo,&crdRun);
if ( ( 0 != rtn ) || ( 0 == crdRun ) )
{
return rtn;
}
}
}
// 用户数据压入完成，将各级缓冲区中数据推入控制器
do
{
// 运动过程中读取当前状态
if ( 1 == crdStartFlag )
{
rtn = GetMotionInfo(core,crd,fifo,&crdRun);
if ( ( 0 != rtn ) || ( 0 == crdRun ) )
{
return rtn;
}
}
rtn = GTN_CrdDataEx(core,crd,NULL,fifo);
if ( 0 == rtn )
{
break;
}
if ( 1 != rtn )
{
return CommandHandler("GTN_CrdDataEx",rtn);
}
} while ( 1 == rtn );
// 数据全部压入控制器，插补仍然没有启动，直接启动插补运动
if ( 0 == crdStartFlag )
{
rtn = GTN_CrdStart(core,1<<(crd-1),fifo<<(crd-1));
if ( 0 != rtn )
{
return CommandHandler("GTN_CrdStart",rtn);
}
printf("Start Crd Success !\n");
crdStartFlag = 1;
}
do
{
// 运动过程中读取当前状态
rtn = GetMotionInfo(core,crd,fifo,&crdRun);
if ( 0 != rtn )
{
return rtn;
}
} while ( 1 == crdRun );
// 关闭DMA通道
rtn = GTN_CrdHsOff(core,crd,fifo);
if ( 0 != rtn )
{
return CommandHandler("GTN_CrdHsOff",rtn);
}
// 关闭控制器
rtn = GTN_Close();
if ( 0 != rtn )
{
return CommandHandler("GTN_Close",rtn);
}
printf("Press Any Key To Exit !\n");
getchar();
return 0;
}