
 // 返回rtn判断处理
short gc_rtn_error(short rtn)
{
	if (rtn != RTN_CMD_SUCCESS)
	{
		return rtn; // 指令执行错误
	}
	return rtn;
}
 
 // 坐标系建立
short crd_build(void)
{
	rtn = NMC_CrdOpen(devHandle, &crdHandle); //打开坐标系
	gc_rtn_error(rtn);
	TCrdConfig crd;
	crd.axCnts = 2; // 设置2轴坐标系
	crd.pAxArray[0] = 0; // 轴端口0对应X轴
	crd.pAxArray[1] = 1; // 轴端口1对应Y轴
	crd.pAxArray[2] = -1; // 轴端口2对应Z轴
	crd.pAxArray[3] = -1; // 轴端口3对应A轴
	crd.port[0] = 0; // 端口映射表均设为0
	crd.port[1] = 0; // 端口映射表均设为0
	crd.port[2] = 0; // 端口映射表均设为0
	crd.port[3] = 0; // 端口映射表均设为0
	rtn = NMC_CrdConfig(crdHandle, &crd); //配置坐标系
	gc_rtn_error(rtn);
	TCrdPara crdPara;
	crdPara.orgFlag = 1; // 默认原点(0,0,0,0)
	crdPara.offset[0] = 0; // 坐标原点配置设为0
	crdPara.offset[1] = 0; // 坐标原点配置设为0
	crdPara.offset[2] = 0; // 坐标原点配置设为0
	crdPara.offset[3] = 0; // 坐标原点配置设为0
	crdPara.synAccMax = 50; //最大合成加速度p/ms^2,50已经比较大
	crdPara.synVelMax = 800; //最大合成速度,允许范围内，两个参数尽可能大
	rtn = NMC_CrdSetPara(crdHandle, &crdPara); 		// 设置运动参数
	gc_rtn_error(rtn);

	/******************* 以下配置可选 *******************/
	TCrdSafePara crdSafePara;
	crdSafePara.estpDec = 1000; //急停加速度
	crdSafePara.maxAcc = 50; //设置坐标系最大加速度50p/ms^2
	crdSafePara.maxVel = 800; //设置坐标系最大速度800p/ms
	rtn = NMC_CrdSetSafePara(crdHandle, &crdSafePara);//设置安全参数
	gc_rtn_error(rtn);

	/******************* 以上配置可选 *******************/
	rtn = NMC_CrdClrError(crdHandle); //清除坐标系错误状态
	gc_rtn_error(rtn);
	rtn = NMC_CrdBufClr(crdHandle); //压入指令之前需要清空缓存区
	gc_rtn_error(rtn);
	/**********************************************************************************
	* 以上为坐标系初始化完成，坐标系(插补)运动基本流程：
	* 打开坐标系->初始化坐标系->压入插补指令->结束压入->启动插补运动->查询坐标系状态->等
	* 待运动完成
	************************************************************************************/
	return rtn;
}

//直线插补， 本例默认使用前瞻， lookaheadDis = 0;
short crd_line_inter(void)
{
	long tgPos[2];
	// 压入直线
	tgPos[0] = 10000;
	tgPos[1] = 10000;
	rtn = NMC_CrdLineXYZEx(crdHandle, 0, 0x03, tgPos, 0, 10, 0.5, 0); 
	gc_rtn_error(rtn);
	
	// 压入直线
	tgPos[0] = 50000;
	tgPos[1] = 60000;
	rtn = NMC_CrdLineXYZEx(crdHandle, 0, 0x03, tgPos, 0, 10, 0.5, 0);
	gc_rtn_error(rtn);
	return rtn;
}

//结束压入数据，启动缓冲区运动，并等待运动完成
short crd_start(void)
{
	short crdsts = 0;
	double vel = 0;
	long segnum = 0;
	long fre = 0;
	rtn = NMC_CrdEndMtn(crdHandle); 	//结束压入
	gc_rtn_error(rtn);
	rtn = NMC_CrdStartMtn(crdHandle); 	//启动坐标系
	gc_rtn_error(rtn);
	//等待运动完成
	while (true)
	{
		rtn = NMC_CrdGetVel(crdHandle, &vel); //读取插补速度
		gc_rtn_error(rtn);
		rtn = NMC_CrdGetUserSegNo(crdHandle, &segnum); //读取正在运行的段号
		gc_rtn_error(rtn);
		rtn = NMC_CrdBufGetFree(crdHandle, &fre); //读取缓存区剩余空间
		gc_rtn_error(rtn);
		rtn = NMC_CrdGetSts(crdHandle, &crdsts); //读取坐标系状态
		gc_rtn_error(rtn);
		if ((crdsts & BIT_CORD_BUSY) == 0) {
			break; /*运动完成*/
		}
		if ((crdsts & BIT_CORD_MVERR) == 1) {
			/*运动出错，做处理，此处略*/
		}
	}
	return rtn;
}


// 高速二维位置比较
short set_hsio(void)
{
	TComp2DimensParam compara_2d;
	memset(&compara_2d, 0, sizeof(compara_2d));
	rtn = NMC_Comp2DimensGetParam(devHandle,0,&compara_2d,0);//读取第0组二维位置比较参数
	gc_rtn_error(rtn);
	rtn = NMC_Comp2DimensOnoff(devHandle, 0, 0, 0); //关闭比较输出
	gc_rtn_error(rtn);
	compara_2d.dir1No = 0; //轴1， -1不使用该轴
	compara_2d.dir2No = 1; //轴2， -1不使用该轴
	compara_2d.chnType[0] = 1; //通道0的硬件接口为控制器扩展口上的gate(hsio)引脚
	compara_2d.chnType[1] = 0; //通道1的硬件接口为gpo
	compara_2d.chnType[2] = 0; //通道2的硬件接口为gpo
	compara_2d.outputType[0] = 0; //通道0输出脉冲信号
	compara_2d.outputType[1] = 0; //通道1输出电平信号
	compara_2d.outputType[2] = -1; //通道2输出电平信号
	compara_2d.outputchn[0] = 0; //只在通道输出
	compara_2d.outputchn[1] = -1; //通道不处理
	compara_2d.outputchn[2] = -1; //通道不处理
	compara_2d.errZone = 20; //容差半径脉冲
	compara_2d.gateTime[0] = 10; //通道0脉冲输出时间us
	compara_2d.gateTime[1] = 10; //通道1(输出为GPO时该值无效)
	compara_2d.gateTime[2] = 10; //通道2(输出为GPO时该值无效)
	compara_2d.posSrc = 1; 		//比较源为编码器
	compara_2d.stLevel[0] = 0; 	//通道初始电平为低电平
	rtn = NMC_Comp2DimensSetParam(devHandle,0,&compara_2d, 0);//设置第0组二维位置比较参数
	gc_rtn_error(rtn);
	
	int count = 2;
	long* compos = new long[2 * count];
	for (int i = 0; i < count; i++) //设置第0组比较的数据
	{
	*(compos + i * 2) = 100 * i + 100; 			//X坐标
	*(compos + i * 2 + 1) = 100 * i + 100; 			//Y坐标
	}
        rtn = NMC_Comp2DimensSetData(devHandle, 0, compos, 0, 0); //先清空
	gc_rtn_error(rtn);
	rtn = NMC_Comp2DimensSetData(devHandle, 0, compos, count, 0);//再设置
	gc_rtn_error(rtn);
	rtn = NMC_Comp2DimensOnoff(devHandle, 0, 1, 0); //开始比较输出
	gc_rtn_error(rtn);
	//开启后可以通过NMC_Comp2DimensStatus读取比较的状态
	return 0;
}

//全部过程
void crd_run()
{
	crd_build();		// 建立坐标系
	crd_line_inter();	// 压入直线插补数据
	set_hsio();		// 设置高速位置比较数据
	crd_start();		// 启动坐标系运动
}