#include "GCMotion.h"
#include "string.h"
using namespace System;
using namespace System::Windows::Forms;

/*****************************************************************
* Function Name : 指令返回值
* Description   : 返回错误值 rtn
* Date			: 2022年8月3日
* Auther		: LWH
******************************************************************/
short gc_rtn_error(short rtn)
{
	if (rtn != RTN_CMD_SUCCESS) {
		return rtn;		// 指令执行错误
	}
}

/*****************************************************************
* Function Name : 坐标系运动完整案例
* Description   : 略
* Date			: 2022年8月3日
* Auther		: LWH
******************************************************************/
short gc_example(void)
{
    // 连接控制器
    short rtn;                              // 指令返回值
    short devNum = 0;                       // 设备序号，从0开始
    short axisNum = 0;                      // 轴号，从0开始
    HAND devHandle = 0;                     // 设备句柄
    HAND axisHandle[4];                     // 轴句柄
    HAND crdHandle = 0;                     // 坐标系句柄
    rtn = NMC_DevOpen(devNum, &devHandle); //打开控制器
    gc_rtn_error(rtn);
    for (short i = axisNum; i < 4; i++) {
        rtn = NMC_MtOpen(devHandle, i, &axisHandle[i]); //打开轴,并获得轴句柄
        gc_rtn_error(rtn);
        rtn = NMC_MtSetSvOn(axisHandle[i]);             //打开轴使能(根据实际情况使用)
        gc_rtn_error(rtn);
        rtn = NMC_MtZeroPos(axisHandle[i]);
        gc_rtn_error(rtn);
    }

    // 坐标系配置
    rtn = NMC_CrdOpen(devHandle, &crdHandle); //打开坐标系
    gc_rtn_error(rtn);
    TCrdConfig crdCfg;
    crdCfg.axCnts = 2;          // 设置3轴坐标系
    crdCfg.pAxArray[0] = 0;     // 轴端口0对应X轴
    crdCfg.pAxArray[1] = 1;     // 轴端口1对应Y轴
    crdCfg.pAxArray[2] = -1;    // 轴端口2对应Z轴
    crdCfg.pAxArray[3] = -1;    // 轴端口3对应A轴， -1为不使用
    crdCfg.port[0] = 0;         // 端口设为0
    crdCfg.port[1] = 0;         // 端口设为0
    crdCfg.port[2] = 0;         // 端口设为0
    crdCfg.port[3] = 0;         // 端口设为0
    rtn = NMC_CrdConfig(crdHandle, &crdCfg); //配置坐标系
    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 = 100;    //最大合成加速度p/ms^2
    crdPara.synVelMax = 500;    //最大合成速度,允许范围内，两个参数尽可能大
    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);

    // 跟随飞行配置
    int segNo = 0;
    long tgtPos[4] = { 0, 0, 0, 0 };
    long centPos[4] = { 0, 0, 0, 0 };
    TFlyFollowPara flyPrm;
    flyPrm.masterAxNo = 3;              // 传送带的轴
    flyPrm.followedAxNo = 1;            // 跟随哪个轴的方向运动，0:X，1:Y
    flyPrm.posCoe = 1;                  // 传送轴当量和跟随方向轴的当量比例，跟随当量 / 传送轴当量
    flyPrm.useEnc = 0;                  // 使用编码器，0：规划，1：编码器
    rtn = NMC_CrdSetFlyFollow(crdHandle, &flyPrm);
    gc_rtn_error(rtn);
    // 缓冲区等待特定位置，满足条件才执行下一条指令
    rtn = NMC_CrdBufWaitPos(crdHandle, segNo++, 3, 1, 5000, 1, 100000);
    gc_rtn_error(rtn);

    // 缓冲区设置飞行跟随功能的启动和关闭
    rtn = NMC_CrdBufEnableFlyFollow(crdHandle, segNo++, 1);
    gc_rtn_error(rtn);
    // 画正方型
    tgtPos[0] = 10000; tgtPos[1] = 10000; tgtPos[2] = 0;
    rtn = NMC_CrdLineXYZEx(crdHandle, segNo++, 0x07, tgtPos, 0.000, 50.000, 4.000, 0);
    tgtPos[0] = 20000; tgtPos[1] = 10000; tgtPos[2] = 0;
    rtn = NMC_CrdLineXYZEx(crdHandle, segNo++, 0x07, tgtPos, 0.000, 50.000, 4.000, 0);
    tgtPos[0] = 20000; tgtPos[1] = 20000; tgtPos[2] = 0;
    rtn = NMC_CrdLineXYZEx(crdHandle, segNo++, 0x07, tgtPos, 0.000, 50.000, 4.000, 0);
    tgtPos[0] = 10000; tgtPos[1] = 20000; tgtPos[2] = 0;
    rtn = NMC_CrdLineXYZEx(crdHandle, segNo++, 0x07, tgtPos, 0.000, 50.000, 4.000, 0);
    tgtPos[0] = 10000; tgtPos[1] = 10000; tgtPos[2] = 0;
    rtn = NMC_CrdLineXYZEx(crdHandle, segNo++, 0x07, tgtPos, 0.000, 50.000, 4.000, 0);

    // 结束(指令压入)缓冲区运动（等待运动完后才结束区运动，并置空闲标志）
    rtn = NMC_CrdEndMtn(crdHandle);
    gc_rtn_error(rtn);

    // 坐标系缓冲运动启动
    rtn = NMC_CrdStartMtn(crdHandle);
    gc_rtn_error(rtn);

    // 4轴走点位运动
    rtn = NMC_MtMovePtpRel(axisHandle[3], 1, 1, 0, 0, 1, 0, 90000);
    gc_rtn_error(rtn);
    // 等待运行完成
}
