updown/plugin/motor_gugao/Card.cpp

#include "Card.h"
#include "axis.h"
#include "gts.h"
#include "motor.h"
#include "log.h"
#include "config.h"

#define SAME_PULSE_OFFSET 10
#define AHEAD_BUFF  100     //前瞻缓冲区

CCard::CCard(ushort cardIndex)
{
    _bOpened = false;
    _cardIndex = cardIndex;
    for(ushort i = 0; i < MAXAXIS; i++)
    {
        _pAxis[i] = new CAxis(cardIndex, i);
    }
    memset(_crd, 0, sizeof(_crd));
    for(ushort i = 0; i < MAXCRD; i++)
    {
        _crd[i].crd = i + 1;    //crd 只能是1和2
        _crd[i].pCrdData = new CrdData[AHEAD_BUFF];
        memset(_crd[i].pCrdData, 0, sizeof(CrdData) * AHEAD_BUFF);
    }
}

CCard::~CCard()
{
    for (ushort i = 0; i < MAXAXIS; i++)
    {
        delete _pAxis[i];
    }
}

int CCard::Open()
{
    int ret = 0;
    char buff[64] = {0};

    RETURN_CHK_NOPRT(!_bOpened, 0);

    /*_bOpened = true;
    return 0;*/
    ret = GT_Open(_cardIndex);
    if (ret)
    {
        LOG_ERR("GT_Open return error:%d", ret);
        return ret;
    }

    cstr p = get_sys_cfg("control", "path");
    if (!p) p = "data";
    sprintf(buff, "%s\\card_%d.cfg", p, _cardIndex);
    ret = GT_LoadConfig(_cardIndex, buff);
    if (ret)
    {
        LOG_ERR("GT_LoadConfig return error:%d", ret);
        GT_Close(_cardIndex);
        return ret;
    }

    for (ushort i = 0; i < MAXAXIS; i++)
    {
        ret = _pAxis[i]->Init();
        if (ret)
        {//fail
            CloseAllAxis();
            GT_Close(_cardIndex);
            return ret;
        }
    }

    ret = GT_HomeInit(_cardIndex);
    if (ret)
    {
        LOG_ERR("GT_HomeInit return error:%d", ret);
        CloseAllAxis();
        GT_Close(_cardIndex);
        return ret;
    }

    _bOpened = true;
    return RETURN_OK;
}

int CCard::Close()
{
    RETURN_CHK_NOPRT(_bOpened, 0);

    CloseAllAxis();
    GT_Close(_cardIndex);
    _bOpened = false;
    return 0;
}

int CCard::Reset()
{//重置
    Close();
    return Open();
}

void CCard::CloseAllAxis()
{
    for(ushort i = 0; i < MAXAXIS; i++)
    {
        _pAxis[i]->Off();
    }
}

int CCard::AxisReset(ushort axisIndex)
{
    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);
    RETURN_CHK_NOPRT(axisIndex < MAXAXIS, ERR_INPUT_PARAM);
    return _pAxis[axisIndex]->Reset();
}

int CCard::AxisZeroPos(ushort axisIndex)
{
    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);
    RETURN_CHK_NOPRT(axisIndex < MAXAXIS, ERR_INPUT_PARAM);
    return _pAxis[axisIndex]->SetZeroPos();
}

int CCard::AxisIsMoving(ushort axisIndex)
{
    RETURN_CHK_NOPRT(_bOpened, 0);
    RETURN_CHK_NOPRT(axisIndex < MAXAXIS, 0);
    return _pAxis[axisIndex]->IsMoving();
}

int CCard::AxisStop(ushort axisIndex, int stopType)
{
    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);
    RETURN_CHK_NOPRT(axisIndex < MAXAXIS, ERR_INPUT_PARAM);
    return _pAxis[axisIndex]->Stop(stopType);
}

int CCard::AxisSetSpeed(ushort axisIndex, double vel)
{
    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);
    RETURN_CHK_NOPRT(axisIndex < MAXAXIS, ERR_INPUT_PARAM);
    return _pAxis[axisIndex]->SetSpeed(vel, TRUE);
}

int CCard::AxisSetPos(ushort axisIndex, long pos)
{
    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);
    RETURN_CHK_NOPRT(axisIndex < MAXAXIS, ERR_INPUT_PARAM);
    return _pAxis[axisIndex]->SetPos(pos);
}

int CCard::AxisGetPos(ushort axisIndex, EMPosType postype, double* pval)
{
    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);
    RETURN_CHK_NOPRT(pval && axisIndex < MAXAXIS, ERR_INPUT_PARAM);
    *pval = _pAxis[axisIndex]->GetCurPos(postype);
    return 0;
}

int CCard::AxisMoveJog(ushort axisIndex, int dir, double vel, double acc, double dec)
{
    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);
    RETURN_CHK_NOPRT(axisIndex < MAXAXIS, ERR_INPUT_PARAM);
    return _pAxis[axisIndex]->MoveJog(dir, vel, acc, dec);
}

int CCard::AxisMovePos(ushort axisIndex, long pos, double vel, double acc, double dec)
{
    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);
    RETURN_CHK_NOPRT(axisIndex < MAXAXIS, ERR_INPUT_PARAM);
    return _pAxis[axisIndex]->MovePos(pos, vel, acc, dec);
}

int CCard::AxisHome(ushort axisIndex, ushort homeType, long pos, long offset, double vel)
{
    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);
    RETURN_CHK_NOPRT(axisIndex < MAXAXIS, ERR_INPUT_PARAM);
    return _pAxis[axisIndex]->Home(homeType, pos, offset, vel);
}

int CCard::AxisMoveOffset(ushort axisIndex, long offset, double vel, double acc, double dec)
{
    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);
    RETURN_CHK_NOPRT(axisIndex < MAXAXIS, ERR_INPUT_PARAM);
    return _pAxis[axisIndex]->MoveOffset(offset, vel, acc, dec);
}

int CCard::ComparePos(ushort axisCnt, short time, long *pBuf1, short count1, long *pBuf2, short count2)
{
    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);

    short ret = GT_CompareData(_cardIndex, axisCnt, 1, 0, 0, time, pBuf1, 1, NULL, 0);   //脉冲输出
    if(ret)
    {
        LOG_ERR("GT_CompareData return error:%d", ret);
    }
    return ret;
}

int CCard::CreateCrd(ushort *pAxisIndex, ushort nsize, int& crd)
{
    int ret = 0;

    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);
    RETURN_CHK_NOPRT(pAxisIndex && nsize < MAXAXIS, ERR_INPUT_PARAM);

    crd = GetExistCrd();    //获取空闲坐标系

    RETURN_CHK_NOPRT(crd >= 0, ERR_SET_CRD);

    _crd[crd].axisCnt = nsize;
    for(ushort i = 0; i < nsize; i++)
    {//赋值轴
        _crd[crd].axisIndex[i] = pAxisIndex[i];
    }

    RETURN_CHK_NOPRT(!SetCrd(_crd[crd]), ERR_SET_CRD);

    _crd[crd].buse = true;    //标识使用中
    return 0;
}

int CCard::CloseCrd(int index)
{
    int ret = 0;

    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);
    RETURN_CHK_NOPRT(index >= 0 && index < MAXCRD, 0);
    RETURN_CHK_NOPRT(_crd[index].buse, 0);

    ret = GT_CrdClear(_cardIndex, _crd[index].crd, 0);
    if (ret)
    {
        LOG_ERR("GT_CrdClear return %d.", ret);
    }

    _crd[index].buse = false;   //标识不在使用中
    return ret;
}

int CCard::AddLinePos(int index, double *ppos, double vel, double acc, double endvel)
{
    int ret = ERR_INPUT_PARAM;

    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);
    RETURN_CHK_NOPRT(ppos, ERR_INPUT_PARAM);
    RETURN_CHK_NOPRT(index >= 0 && index < MAXCRD, ERR_INPUT_PARAM);
    RETURN_CHK_NOPRT(_crd[index].buse, ERR_NO_INIT);

    switch (_crd[index].axisCnt)
    {
    case 2:
        ret = GT_LnXY(_cardIndex, _crd[index].crd, double_to_int(ppos[0]),
            double_to_int(ppos[1]), vel, acc, endvel);
        break;
    case 3:
        ret = GT_LnXYZ(_cardIndex, _crd[index].crd, double_to_int(ppos[0]),
            double_to_int(ppos[1]), double_to_int(ppos[2]), vel, acc, endvel);
        break;
    case 4:
        ret = GT_LnXYZA(_cardIndex, _crd[index].crd, double_to_int(ppos[0]),
            double_to_int(ppos[1]), double_to_int(ppos[2]), double_to_int(ppos[3]), vel, acc, endvel);
        break;
    default:
        break;
    }

    if (ret)
    {
        LOG_ERR("AddArcPos return %d", ret);
    }

    return ret;
}

int CCard::AddArcPos(int index, double *ppos1, double *ppos2, double *ppos3, double vel, double acc, double endvel)
{
    int rtn = 0;
    long realPos1[4];
    long realPos2[4];
    long realPos3[4];
    int arcType = 0;
    short arcDir = 0;   //默认圆弧运动方向 0 顺时针 1逆时针
    double x, y, r;

    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);
    RETURN_CHK_NOPRT(ppos1 && ppos2 && ppos3, ERR_INPUT_PARAM);
    RETURN_CHK_NOPRT(index >= 0 && index < MAXCRD, ERR_INPUT_PARAM);
    RETURN_CHK_NOPRT(_crd[index].buse, ERR_NO_INIT);

    for(int i = 0; i < _crd[index].axisCnt; i++)
    {
        realPos1[i] = double_to_int(ppos1[i]);
        realPos2[i] = double_to_int(ppos2[i]);
        realPos3[i] = double_to_int(ppos3[i]);
    }

    //计算圆弧运动类型，并按类型处理位置数据
    rtn = CalcArcType(realPos1, realPos2, realPos3, _crd[index].axisCnt, arcType);

    RETURN_CHK_NOPRT(!rtn, rtn);

    if (!IsSamePos(realPos1, realPos3, _crd[index].axisCnt))
    {//不是整圆，不能在一条线
        RETURN_CHK_NOPRT(!IsSameLine(realPos1, realPos2, realPos3), ERR_INPUT_PARAM);
        //计算圆弧运动方向 0 顺时针 1逆时针
        arcDir = CalcDir(realPos1, realPos2, realPos3) ? 0 : 1;
    }

    //计算圆心坐标
    CalcCenter(realPos1, realPos2, realPos3, x, y, r);
    switch (arcType)
    {
    case EM_ARC_XY:
        rtn = GT_ArcXYC(_cardIndex, _crd[index].crd, realPos3[0], realPos3[1], 
            x-realPos3[0], y-realPos3[1], arcDir, vel, acc, endvel);
        break;
    case EM_ARC_YZ:
        rtn = GT_ArcYZC(_cardIndex, _crd[index].crd, realPos3[0], realPos3[1], 
            x-realPos3[0], y-realPos3[1], arcDir, vel, acc, endvel);
        break;
    case EM_ARC_ZX:
        rtn = GT_ArcZXC(_cardIndex, _crd[index].crd, realPos3[0], realPos3[1], 
            x-realPos3[0], y-realPos3[1], arcDir, vel, acc, endvel);
        break;
    default:
        rtn = ERR_INPUT_PARAM;
        break;
    }

    if (rtn)
    {
        LOG_ERR("AddArcPos return %d", rtn);
    }
    return rtn;
}

int CCard::CrdStartMove(int index)
{
    int rtn = 0;

    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);
    RETURN_CHK_NOPRT(index >= 0 && index < MAXCRD, ERR_INPUT_PARAM);
    RETURN_CHK_NOPRT(_crd[index].buse, ERR_NO_INIT);

    //将前瞻缓冲区压入运动缓冲区
    rtn = GT_CrdData(_cardIndex, _crd[index].crd, NULL);
    if (rtn)
    {
        LOG_ERR("GT_CrdData return %d", rtn);
        return rtn;
    }

    //启动插补运动
    rtn = GT_CrdStart(_cardIndex, _crd[index].crd, 0);
    if (rtn)
    {
        LOG_ERR("GT_CrdStart return %d", rtn);
    }

    return rtn;
}

int CCard::CrdStatus(int index, int *progress)
{
    int ret = 0;
    long res = 0;
    short run = 0;

    RETURN_CHK_NOPRT(_bOpened, 0);
    RETURN_CHK_NOPRT(index >= 0 && index < MAXCRD, 0);
    RETURN_CHK_NOPRT(_crd[index].buse, 0);

    ret = GT_CrdStatus(_cardIndex, _crd[index].crd, &run, &res);
    if (ret)
    {
        LOG_ERR("GT_CrdStatus return %d.", ret);
    }

    if (progress) *progress = (int)res;   //已完成插补段数
    return run; //1标识运动中 0标识运动完成
}

int CCard::GetExistCrd()
{
    for(int i = 0; i < MAXCRD; i++)
    {
        if (!_crd[i].buse)
        {//没有坐标系数据
            return i;
        }
    }
    return -1;
}

int CCard::SetCrd(TCrd& crd)
{
    short rtn = 0;
    TCrdPrm crdPrm;

    memset(&crdPrm, 0, sizeof(TCrdPrm));
    crdPrm.dimension = crd.axisCnt;
    crdPrm.evenTime = 50;
    crdPrm.setOriginFlag = 1;
    for(int i = 0; i < crd.axisCnt; i++)
    {
        crdPrm.profile[crd.axisIndex[i]] = i + 1; //profile表示卡上所有轴，数组下标为每个轴索引， i + 1 表示坐标系轴 从1开始
        /*crdPrm.profile[i] = i + 1;*/
        crdPrm.originPos[i] = /*pAxisParam->homeOffset*/0;
    }
    crdPrm.synVelMax = 10000;
    crdPrm.synAccMax = 10000;
    rtn = GT_SetCrdPrm(_cardIndex, crd.crd, &crdPrm);
    if (rtn)
    {
        LOG_ERR("GT_SetCrdPrm return %d", rtn);
        return rtn;
    }

    rtn = GT_CrdClear(_cardIndex, crd.crd, 0); //清空FIFO0
    //rtn = GT_CrdClear(_cardIndex, crd.crd, 1); //清空FIFO1
    if (rtn)
    {
        LOG_ERR("GT_CrdClear return %d", rtn);
        return rtn;
    }

    rtn = GT_InitLookAhead(_cardIndex, crd.crd, 0, 5, 10000, AHEAD_BUFF, crd.pCrdData);
    if (rtn)
    {
        LOG_ERR("GT_InitLookAhead return %d", rtn);
    }
    return rtn;
}

bool CCard::IsSamePos(long* pos1, long* pos2, int axisCnt)
{
    for(int i = 0; i < axisCnt; i++)
    {
        if (abs(pos1[i] - pos2[i]) >= SAME_PULSE_OFFSET)
        {//差距在可允许范围内
            return false;
        }
    }
    return true;
}

bool CCard::IsSameLine(long* pos1, long* pos2,long* pos3)
{
    if ((pos2[0] - pos1[0])*(pos3[1] - pos2[1]) == (pos2[1] - pos1[1])*(pos3[0] - pos2[0]))
    {
        return true;
    }
    return false;
}

int CCard::CalcArcType(long* pos1, long* pos2, long* pos3, int axisCnt, int &arcType)
{
    int i = 0;

    //1-2 2-3不能重合
    RETURN_CHK_NOPRT(!IsSamePos(pos1, pos2, axisCnt), ERR_INPUT_PARAM);
    RETURN_CHK_NOPRT(!IsSamePos(pos2, pos3, axisCnt), ERR_INPUT_PARAM);

    for(i = 0; i < axisCnt; i++)
    {
        if (abs(pos1[i] - pos2[i]) < SAME_PULSE_OFFSET &&
            abs(pos1[i] - pos3[i]) < SAME_PULSE_OFFSET &&
            abs(pos2[i] - pos3[i]) < SAME_PULSE_OFFSET)
        {//差距在可允许范围内
            break;
        }
    }

    RETURN_CHK_NOPRT(i <= 2, ERR_INPUT_PARAM);    //圆弧插补只能前面3个轴之间任意两轴

    switch (i)
    {
    case 0:
        arcType = EM_ARC_YZ;    //把数据前移
        pos1[0] = pos1[1];
        pos1[1] = pos1[2];
        pos2[0] = pos2[1];
        pos2[1] = pos2[2];
        pos3[0] = pos3[1];
        pos3[1] = pos3[2];
        break;
    case 1:
        arcType = EM_ARC_ZX;    //把数据换一下顺序，否则参数不好传递
        pos1[1] = pos1[0];
        pos1[0] = pos1[2];
        pos2[1] = pos2[0];
        pos2[0] = pos2[2];
        pos3[1] = pos3[0];
        pos3[0] = pos3[2];
        break;
    case 2:
        arcType = EM_ARC_XY;
        break;
    default:
        break;
    }
    return 0;
}

void CCard::CalcCenter(long* pos1, long* pos2, long* pos3, double &x, double &y, double&r)
{
    double a,b,c,d,e,f;
    double x1,x2,x3,y1,y2,y3;

    x1 = pos1[0];
    x2 = pos2[0];
    x3 = pos3[0];
    y1 = pos1[1];
    y2 = pos2[1];
    y3 = pos3[1];

    a = 2 * (x2 - x1);
    b = 2 * (y2 - y1);
    c = x2 * x2 + y2 * y2 - x1 * x1 - y1 * y1;
    d = 2 * (x3 - x2);
    e = 2 * (y3 - y2);
    f = x3 * x3 + y3 * y3 - x2 * x2 - y2 * y2;
    x = (b * f - e * c) / (b * d - e * a);
    y = (d * c - a * f) / (b * d - e * a);
    r = sqrt((x - x1) * (x - x1) + (y - y1) * (y - y1));
}

bool CCard::CalcDir(long *pos1, long* pos2, long* pos3)
{
    //a b c 顺序3个点，网上参考答案
    //(b.x - a.x) * (c.y - b.y) - (b.y - a.y) * (c.x - b.x)
    long rtn = (pos2[0] - pos1[0]) * (pos3[1] - pos2[1]) - (pos2[1] - pos1[1]) * (pos3[0] - pos2[0]);

    //大于0 顺时针, 小于0 逆时针
    return rtn > 0 ? true : false;
}

short CCard::TransfIOType(short di_type)
{
    int dioType[] = {
        MC_GPI,
        MC_GPO,
        MC_LIMIT_POSITIVE,
        MC_LIMIT_NEGATIVE,
        MC_ALARM,
        MC_HOME,
        MC_ENABLE,
        MC_CLEAR
    };
    return dioType[di_type];
}

const int g_diotype[] = {
    MC_GPI,
    MC_GPO,
    MC_LIMIT_POSITIVE,
    MC_LIMIT_NEGATIVE,
    MC_ALARM,
    MC_HOME,
    MC_ENABLE,
    MC_CLEAR
};

int CCard::ReadIn(EIOType di_type, long& val)
{
    int ret = -1;

    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);

    ret = GT_GetDi(_cardIndex, g_diotype[di_type], &val);
    if (ret)
    {
        LOG_ERR("GT_GetDi return error:%d", ret);
    }
    return ret;
}

int CCard::ReadOut(EIOType do_type, long& val)
{
    int ret = -1;

    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);

    ret = GT_GetDo(_cardIndex, g_diotype[do_type], &val);
    if (ret)
    {
        LOG_ERR("GT_GetDo return error:%d", ret);
    }
    return ret;
}

int CCard::ReadBit(ushort in_index, EIOType di_type)
{
    long val = 0;

    RETURN_CHK_NOPRT(in_index < MAXIO, 0);

    if (IOT_COMOUT == di_type || IOT_SEVON == di_type || IOT_CLEARALARM == di_type)
    {
        return ReadOutBit(in_index, di_type);
    }

    if (ReadIn(di_type, val))
    {//读取错误
        return 0;
    }

    return 0x01 & (val>>in_index);
}

int CCard::ReadOutBit(ushort out_index, EIOType do_type)
{
    long val = 0;

    RETURN_CHK_NOPRT(out_index < MAXIO, 0);

    if (ReadOut(do_type, val))
    {//读取错误
        return 0;
    }

    return 0x01 & (val>>out_index);
}

int CCard::WriteOutBit(ushort out_index, ushort val, EIOType do_type)
{
    int ret = ERR_INPUT_PARAM;

    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);

    if (IOT_SEVON == do_type)
    {//伺服使能
        RETURN_CHK_NOPRT(out_index < MAXAXIS, ERR_INPUT_PARAM);
        return val ? _pAxis[out_index]->On() : _pAxis[out_index]->Off();
    }
    else if (IOT_COMOUT == do_type || IOT_CLEARALARM == do_type)
    {//输出
        RETURN_CHK_NOPRT(out_index < MAXIO, ERR_INPUT_PARAM);
        ret = GT_SetDoBit(_cardIndex, g_diotype[do_type], out_index + 1, val);
        if (ret)
        {
            LOG_ERR("GT_SetDoBit return error:%d", ret);
        }
    }

    return ret;
}

int CCard::ReadADC(ushort index, double* pval)
{
    int ret = 0;

    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);
    RETURN_CHK_NOPRT(pval && index < MAXAD, ERR_INPUT_PARAM);

    ret = GT_GetAdc(_cardIndex, index + 1, pval);
    if (ret)
    {
        LOG_ERR("GT_GetAdc return error:%d", ret);
    }
    return ret;
}

int CCard::WriteADC(ushort index, double val)
{
    int ret = 0;
    short rtn = 0;

    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);
    RETURN_CHK_NOPRT(index < MAXAD, ERR_INPUT_PARAM);

    rtn = (short)double_to_int(val);
    ret = GT_SetDac(_cardIndex, index + 1, &rtn);
    if (ret)
    {
        LOG_ERR("GT_SetDac return error:%d", ret);
    }
    return ret;
}

#define RESET       "reset"     //重置卡
#define PAUSE       "pause"     //暂停运动
#define CONTINUE    "continue"  //继续运动
#define BACKLASH    "backlash"  //反向间隙补偿
#define ARRIVE      "arrive"    //设置到位信号
#define SETAD       "writeda"   //设置电压值

int CCard::SendCustomCommand(const char* msg, char* res/* = 0*/)
{
    char* ptmp = NULL;
    char buff[256] = {0};
    char* pnext = buff;

    RETURN_CHK_NOPRT(msg, ERR_INPUT_PARAM);
    RETURN_CHK_NOPRT(_bOpened, ERR_NO_INIT);

    strcpy(buff, msg);  //拷贝指令
    ptmp = sys_strtok(NULL, ", ", &pnext);

    RETURN_CHK_NOPRT(ptmp, ERR_INPUT_PARAM);

    if (0 == sys_stricmp(RESET, ptmp))
    {//reset
        RETURN_CHK_NOPRT(!pnext || 0 == *pnext, ERR_INPUT_PARAM);   //结束
        return Reset();
    }
    else if (0 == sys_stricmp(PAUSE, ptmp))
    {//pause
        RETURN_CHK_NOPRT(!pnext || 0 == *pnext, ERR_INPUT_PARAM);   //结束
        return 0;
    }
    else if (0 == sys_stricmp(CONTINUE, ptmp))
    {//continue
        RETURN_CHK_NOPRT(!pnext || 0 == *pnext, ERR_INPUT_PARAM);   //结束
        return 0;
    }
    else if (0 == sys_stricmp(BACKLASH, ptmp))
    {//负向间隙补偿
        return SetBacklash(pnext);
    }
    else if (0 == sys_stricmp(ARRIVE, ptmp))
    {//到位信号
        return SetArrive(pnext);
    }
    else if (0 == sys_stricmp(SETAD, ptmp))
    {
        return SetAD(pnext);
    }
    return ERR_INPUT_PARAM;
}

int CCard::RecvCustomCommand(const char* msg, char* res/* = 0*/, int timeout/* = 0*/)
{
    return ERR_INPUT_PARAM;
}

int CCard::ParseCompare(char* param)
{
    char *ptm = NULL;
    char *pNextToken = param;
    short chn,mode,style,encx,ency,count;
    long pbuffx[MAX_BUF_LEN], pbuffy[MAX_BUF_LEN];

    ptm = sys_strtok(NULL,",",&pNextToken);
    RETURN_CHK_NOPRT(ptm,ERR_INPUT_PARAM);
    chn = atoi(ptm);

    ptm = sys_strtok(NULL,",",&pNextToken);
    RETURN_CHK_NOPRT(ptm,ERR_INPUT_PARAM);
    mode = atoi(ptm);

    ptm = sys_strtok(NULL,",",&pNextToken);
    RETURN_CHK_NOPRT(ptm,ERR_INPUT_PARAM);
    style = atoi(ptm);

    if(0 == style)
    {
        ptm = sys_strtok(NULL,",",&pNextToken);
        RETURN_CHK_NOPRT(ptm,ERR_INPUT_PARAM);
        encx = atoi(ptm);

        ptm = sys_strtok(NULL,",",&pNextToken);
        RETURN_CHK_NOPRT(ptm,ERR_INPUT_PARAM);
        ency = atoi(ptm);
        return SetComparePrm(chn, mode, encx, ency);
    }

    ptm = sys_strtok(NULL,",",&pNextToken);
    RETURN_CHK_NOPRT(ptm,ERR_INPUT_PARAM);
    count = atoi(ptm);

    for(int i = 0;i< count;i++)
    {
        ptm = sys_strtok(NULL,",",&pNextToken);
        RETURN_CHK_NOPRT(ptm,ERR_INPUT_PARAM);
        pbuffx[i] = atoi(ptm);

        ptm = sys_strtok(NULL,",",&pNextToken);
        RETURN_CHK_NOPRT(ptm,ERR_INPUT_PARAM);
        pbuffy[i] = atoi(ptm);
    }
    return StartCompare(chn, mode, count, pbuffx, pbuffy);
}

int CCard::SetComparePrm(short chn, short mode, short encx, short ency)  //ency 为比较轴
{
    int ret;
    T2DComparePrm prm;
    //     ret = GT_2DCompareStop(cardIndex,chn);   
    //     if(ret)
    //     {
    //         LOG_ERR("GT_CompareData return error:%d",ret);
    //         return ERR_MOTOR_API;
    //     }
    ret = GT_2DCompareClear(_cardIndex, chn);               //清空二维位置比较输出数据
    if(ret)
    {
        LOG_ERR("GT_CompareData return error:%d",ret);
        return ret;
    }
    ret = GT_2DCompareMode(_cardIndex, chn, mode);  //设置位置比较输出模式 0:1D 1:2D
    if(ret)
    {
        LOG_ERR("GT_CompareData return error:%d",ret);
        return ret;
    }
    prm.encx=encx;     							 //2关联比较轴x
    prm.ency=ency;     							 //1关联比较轴y   一维时只能比较Y轴,此时此处关联X轴号
    prm.maxerr=150; 							 //最大允许误差(单位:pulse)
    prm.outputType=0;							 //输出类型: 0脉冲;1电平
    prm.source=1;   							 //比较源:0规划,1编码器
    prm.startLevel=0;							 //初始电平是否反转：0复位电平，1反转
    prm.threshold=10;							 //最优算法阈值
    prm.time=200;   							 //脉冲方式脉冲时间(单位us)
    ret = GT_2DCompareSetPrm(_cardIndex,chn,                 //通道号 0： HSIO0 通道号 1： HSIO1
        &prm);                                   
    if(ret)
    {
        LOG_ERR("GT_CompareData return error:%d",ret);
    }
    return ret;
}

int CCard::StartCompare(short chn, short mode, int count, long *px, long *py)
{
    short ret = 1;
    T2DCompareData databuf[MAX_BUF_LEN];
    short pStatus,pFifo,pFifoCount,pBufCount;
    long pCount;
    double pValue;
    for(int i=0;i<count;i++)
    {
        if(0 == mode)
        {
            ret = GT_GetEncPos(1,1,&pValue); 
            py[i]=py[i]-(long)pValue;
            databuf[i].px = 0;                       //第一比较点(10000,10000)
        }
        else
        {
            ret = GT_GetEncPos(1,1,&pValue);    
            px[i]=px[i]-(long)pValue;
            ret = GT_GetEncPos(1,2,&pValue);    
            py[i]=py[i]-(long)pValue;
            databuf[i].px = px[i];                       //第一比较点(10000,10000)
        }
        databuf[i].py = py[i];

    }

    ret = GT_2DCompareStatus(_cardIndex,chn,&pStatus,      //读取位置比较状态这里主要获取空闲fifo
        &pCount,&pFifo,&pFifoCount,&pBufCount);
    if(ret)
    {
        LOG_ERR("GT_CompareData return error:%d",ret);
        return ret;
    }
    ret=GT_2DCompareData(_cardIndex,chn,                   // 通道号 0： HSIO0 通道号 1： HSIO1
        count,                     //位置比较数据个数
        databuf,               // 数据
        pFifo);                //空闲FIFO
    //commandhandler("GT_2DCompareData", sRtn);
    if(ret)
    {
        LOG_ERR("GT_CompareData return error:%d",ret);
        return ret;
    }
    ret=GT_2DCompareStart(_cardIndex,chn);                 //启动位置比较
    if(ret)
    {
        LOG_ERR("GT_CompareData return error:%d",ret);
        return ret;
    }
    return  0;
}

int CCard::ParseGearMove(char* param)
{
    char *pTmp = NULL;
    char *pNextToken = param;
    ushort masterAxis = 0;
    ushort slaveAxis = 0;
    int iGearType = 0;
    double ratio = 0;
    long masterSlope = 0;
    int rtn = 0;

    pTmp =  sys_strtok(NULL, ",", &pNextToken);
    RETURN_CHK_NOPRT(pTmp, ERR_INPUT_PARAM);
    masterAxis = atoi(pTmp);

    pTmp =  sys_strtok(NULL, ",", &pNextToken);
    RETURN_CHK_NOPRT(pTmp, ERR_INPUT_PARAM);
    slaveAxis = atoi(pTmp);

    pTmp =  sys_strtok(NULL, ",", &pNextToken);
    RETURN_CHK_NOPRT(pTmp, ERR_INPUT_PARAM);
    iGearType = atoi(pTmp);

    pTmp =  sys_strtok(NULL, ",", &pNextToken);
    RETURN_CHK_NOPRT(pTmp, ERR_INPUT_PARAM);
    ratio = atof(pTmp);

    pTmp =  sys_strtok(NULL, ",", &pNextToken);
    RETURN_CHK_NOPRT(pTmp, ERR_INPUT_PARAM);
    masterSlope = atoi(pTmp);

    RETURN_CHK_NOPRT(masterAxis < MAXAXIS && slaveAxis < MAXAXIS, ERR_INPUT_PARAM);

    return _pAxis[slaveAxis]->GearMove(_pAxis[masterAxis], iGearType, ratio, masterSlope);
}

//backlash,axis,offset;
int CCard::SetBacklash(char* param)
{
    int ret = 0;
    short axis = 0;
    char* ptmp = NULL;
    long compValue = 0;

    ptmp =  sys_strtok(NULL, ", ", &param);
    RETURN_CHK_NOPRT(EMSTR_INT == get_str_type(ptmp), ERR_INPUT_PARAM);
    axis = (short)atoi(ptmp);

    RETURN_CHK_NOPRT(axis < MAXAXIS, ERR_INPUT_PARAM);
    RETURN_CHK_NOPRT(EMSTR_INT == get_str_type(param), ERR_INPUT_PARAM);

    compValue = atoi(param);
    ret = GT_SetBacklash(_cardIndex, axis, compValue, (double)compValue * 0.175, 0);
    if (ret)
    {
        LOG_ERR("GT_SetBacklash return error:%d", ret);
    }
    return ret;
}

//arrive,axis,1;
int CCard::SetArrive(char* param)
{
    int flag = 0;
    char* ptmp = NULL;
    ushort axisIndex = 0;

    ptmp = sys_strtok(NULL, ", ", &param);
    RETURN_CHK_NOPRT(EMSTR_INT == get_str_type(ptmp), ERR_INPUT_PARAM);
    axisIndex = (ushort)atoi(ptmp);
    RETURN_CHK_NOPRT(axisIndex < MAXAXIS, ERR_INPUT_PARAM);
    RETURN_CHK_NOPRT(EMSTR_INT == get_str_type(param), ERR_INPUT_PARAM);
    flag = atoi(param);
    RETURN_CHK_NOPRT(flag <= 1 && flag >= 0, ERR_INPUT_PARAM);

    LOG_INF1("arrive(%d,%d) = 0", axisIndex, flag);
    _pAxis[axisIndex]->SetArrive(flag ? true : false);
    return 0;
}

//writeda,adindex,val
int CCard::SetAD(char* param)
{
    short val = 0;
    char* ptmp = NULL;
    ushort adIndex = 0;

    ptmp = sys_strtok(NULL, ", ", &param);
    if (EMSTR_INT != get_str_type(ptmp) ||
        EMSTR_INT != get_str_type(param))
    {
        LOG_ERR("input param error. adindex/val param type.");
        return ERR_INPUT_PARAM;
    }

    adIndex = (ushort)atoi(ptmp);
    val = (short)atoi(param);
    return WriteADC(adIndex, val);
}