updown/plugin/motor_gugao/Axis.cpp

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

enum EM_GEAR_TYPE
{//<2F><><EFBFBD><EFBFBD>ģʽ
    EM_GEAR_ENCODER = 0,    //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
    EM_GEAR_PROFILE,        //<2F><><EFBFBD><EFBFBD><EFBFBD>滮<EFBFBD><E6BBAE>λ<EFBFBD><CEBB>
    EM_GEAR_AXIS_ENCODER,   //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
    EM_GEAR_AXIS_PROFILE,   //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>滮λ<E6BBAE><CEBB>
};

CAxis::CAxis(ushort cardIndex, ushort axisIndex)
{
    m_bInit = false;
    m_bCheckArrive = false;
    m_cardIndex = cardIndex;
    m_axisIndex = axisIndex;
    m_hQuitEvt = sys_CreateEvent(FALSE, FALSE, NULL);
    m_hHomeFinishedEvt = sys_CreateEvent(TRUE, TRUE, NULL);
}

CAxis::~CAxis()
{
    Off();
    sys_WaitForSingleObject(m_hHomeFinishedEvt);
    sys_CloseHandle(m_hQuitEvt);
    sys_CloseHandle(m_hHomeFinishedEvt);
}

int CAxis::Init()
{
    int ret = 0;

    ClearStatus();  //<2F><><EFBFBD><EFBFBD>״̬

    /*ret = GT_EncSns(cardIndex, 0);
    if (ret)
    {
    LOG_ERR("GT_EncSns return error:%d", ret);
    return ret;
    }*/
    
    ret = On();
    if (ret)
    {
        LOG_ERR("GT_AxisOn return error:%d", ret);
        return ret;
    }

    /*ret = GT_EncOn(cardIndex, axisIndex + 1);
    if (ret)
    {
    LOG_ERR("GT_EncOn return error:%d", ret);
    return ret;
    }*/

    return RETURN_OK;
}

int CAxis::On()
{
    int ret = 0;

    RETURN_CHK_NOPRT(!m_bInit, RETURN_OK);

    ret = GT_AxisOn(m_cardIndex, m_axisIndex + 1);
    if (ret)
    {
        LOG_ERR("GT_AxisOn return error:%d", ret);
        return ret;
    }
    m_bInit = true;
    return 0;
}

int CAxis::Off()
{
    int ret = 0;

    RETURN_CHK_NOPRT(m_bInit, RETURN_OK);

    ret = GT_AxisOff(m_cardIndex, m_axisIndex + 1);
    if (ret)
    {
        LOG_ERR("GT_AxisOff return error:%d", ret);
        return ret;
    }
    m_bInit = false;
    return 0;
}

int CAxis::Reset()
{
    /*Stop();
    sys_WaitForSingleObject(m_hHomeFinishedEvt);*/
    return ClearStatus();  //<2F><><EFBFBD><EFBFBD>״̬
}

bool CAxis::WaitExitMoveSign(int iWait)
{
    if (0 == sys_WaitForSingleObject(m_hQuitEvt, iWait))
    {//<2F><><EFBFBD>˳<EFBFBD><CBB3>ź<EFBFBD>
        StopMove();
        LOG_ERR("Gugao card:%d axis%d Have Exit Move Sign.", m_cardIndex, m_axisIndex);
        return true;
    }
    return false;
}

int CAxis::SetSpeed(double vel, int bchange)
{
    int ret = 0;

    RETURN_CHK_NOPRT(m_bInit, ERR_NO_INIT);
    RETURN_CHK_NOPRT(!ISZEROF(vel), ERR_INPUT_PARAM);

    ret = GT_SetVel(m_cardIndex, m_axisIndex + 1, vel);
    if (ret)
    {
        LOG_ERR("GT_SetVel return error:%d", ret);
    }

    RETURN_CHK_NOPRT(bchange, ret);

    //<2F><><EFBFBD>µ<EFBFBD><C2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> -- <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>
    ret = GT_Update(m_cardIndex, (0x01 << m_axisIndex));
    if (ret)
    {
        LOG_ERR("GT_Update return error:%d", ret);
    }
    return ret;
}

int CAxis::MoveOffset(long offset, double vel, double acc, double dec)
{
    long curpos = 0;
    int ret = 0;

    RETURN_CHK_NOPRT(m_bInit, ERR_NO_INIT);

    curpos = double_to_int(GetCurPos(EMPos_Profile));
    curpos += offset;
    return MovePos(curpos, vel, acc, dec);
}

int CAxis::SetPos(long pos)
{
    int ret = 0;

    RETURN_CHK_NOPRT(m_bInit, ERR_NO_INIT);

    ret = GT_SetPos(m_cardIndex, m_axisIndex + 1, pos);
    if (ret)
    {
        LOG_ERR("GT_SetPos return error:%d", ret);
        return ret;
    }

    ret = GT_Update(m_cardIndex, (0x01 << m_axisIndex));
    if (ret)
    {
        LOG_ERR("GT_Update return error:%d", ret);
    }

    return ret;
}

int CAxis::MovePos(long pos, double vel, double acc, double dec)
{
    TTrapPrm prm;
    int ret = 0;

    RETURN_CHK_NOPRT(m_bInit, ERR_NO_INIT);

    ret = GT_PrfTrap(m_cardIndex, m_axisIndex + 1);
    if (ret)
    {
        LOG_ERR("GT_PrfTrap return error:%d", ret);
        return ret;
    }

    ret = GT_GetTrapPrm(m_cardIndex, m_axisIndex + 1, &prm);
    if (ret)
    {
        LOG_ERR("GT_GetTrapPrm return error:%d", ret);
        return ret;
    }

    prm.acc = acc;
    prm.dec = dec;
    /*prm.velStart = 0;
    prm.smoothTime = 5;*/
    ret = GT_SetTrapPrm(m_cardIndex, m_axisIndex + 1, &prm);
    if (ret)
    {
        LOG_ERR("GT_SetTrapPrm(%.2lf,%.2lf) return error:%d", acc, dec, ret);
        return ret;
    }

    ret = SetSpeed(vel);

    RETURN_CHK_NOPRT(!ret, ret);

    return SetPos(pos);
}

int CAxis::MoveJog(int dir, double vel, double acc, double dec)
{
    TJogPrm prm;
    int ret = 0;

    RETURN_CHK_NOPRT(m_bInit, ERR_NO_INIT);

    //<2F><>ΪJogģʽ
    ret = GT_PrfJog(m_cardIndex, m_axisIndex + 1);
    if (ret)
    {
        LOG_ERR("GT_PrfJog return error:%d", ret);
        return ret;
    }

    //<2F><>ȡJog<6F>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>
    ret = GT_GetJogPrm(m_cardIndex, m_axisIndex + 1, &prm);
    if (ret)
    {
        LOG_ERR("GT_GetJogPrm return error:%d", ret);
        return ret;
    }

    prm.acc = acc;
    prm.dec = dec;  //<2F><><EFBFBD><EFBFBD>Jog<6F>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>
    ret = GT_SetJogPrm(m_cardIndex, m_axisIndex + 1, &prm);
    if (ret)
    {
        LOG_ERR("GT_SetJogPrm return error:%d", ret);
        return ret;
    }

    ret = SetSpeed(dir ? vel : vel * (-1));

    RETURN_CHK_NOPRT(!ret, ret);

    ret = GT_Update(m_cardIndex, (0x01 << m_axisIndex));
    if (ret)
    {
        LOG_ERR("GT_Update return error:%d", ret);
        return ret;
    }

    return RETURN_OK;
}

double CAxis::GetCurPos(EMPosType postype)
{
    double refpos = 0;
    int ret = 0;

    if (EMPos_Profile == postype)
    {//step
        ret = GT_GetPrfPos(m_cardIndex, m_axisIndex + 1, &refpos);
        if (ret)
        {
            LOG_ERR("GT_GetPrfPos return error:%d", ret);
            return 0;
        }
    }
    else
    {//encode
        ret = GT_GetEncPos(m_cardIndex, m_axisIndex + 1, &refpos);
        if (ret)
        {
            LOG_ERR("GT_GetEncPos return error:%d", ret);
            return 0;
        }
    }
    return refpos;
}

int CAxis::IsMoving()
{
    RETURN_CHK_NOPRT(!sys_WaitForSingleObject(m_hHomeFinishedEvt, 0), 1);

    return isRealMoving();
}

int CAxis::isRealMoving()
{
    long status = 0;
    int ret = GT_GetSts(m_cardIndex, m_axisIndex + 1, &status);
    if (ret)
    {
        LOG_ERR("GT_GetSts return error:%d", ret);
        return FALSE;
    }

    if (status & 0x400)
    {//moving
        return TRUE;
    }

    if (!m_bCheckArrive)
    {//<2F>޵<EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>
        return FALSE;
    }

    ret = GT_GetDi(m_cardIndex, MC_ARRIVE, &status);
    if (ret)
    {
        LOG_ERR("GT_GetDi return error:%d", ret);
        return FALSE;
    }

    if (status>>m_axisIndex & 0x01)
    {
        return FALSE;
    }

    //LOG_INFO("GT_GetDi return is moving...");
    return TRUE;
}

int CAxis::Stop(int type)
{
    sys_SetEvent(m_hQuitEvt);   //<2F><><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>ź<EFBFBD>
    return StopMove(type);
}

void CAxis::StopAndWait(int type)
{
    StopMove(type);
    while(isRealMoving())
    {//<2F>ȴ<EFBFBD><C8B4>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD>
        sys_Sleep(10);
    }
}

int CAxis::StopMove(int type)
{
    int ret = GT_Stop(m_cardIndex, (0x01 << m_axisIndex), type ? (0x01 << m_axisIndex) : 0);
    if (ret)
    {
        LOG_ERR("GT_Stop return error:%d", ret);
    }
    return ret;
}

int CAxis::ClearStatus()
{
    int ret = GT_AlarmOff(m_cardIndex, m_axisIndex + 1);
    if (ret)  //<2F>ر<EFBFBD><D8B1>ŷ<EFBFBD><C5B7><EFBFBD><EFBFBD><EFBFBD>
    {
        LOG_ERR("GT_AlarmOff return error:%d", ret);
    }

    ret = GT_LmtsOff(m_cardIndex, m_axisIndex + 1);
    if (ret)  //<2F>ر<EFBFBD><D8B1><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>
    {
        LOG_ERR("GT_LmtsOff return error:%d", ret);
    }

    ret = GT_ClrSts(m_cardIndex, m_axisIndex + 1, 1);
    if (ret)
    {
        LOG_ERR("GT_ClrSts return error:%d", ret);
    }
    return ret;
}

int CAxis::SetZeroPos()
{
    RETURN_CHK_NOPRT(!isRealMoving(), ERR_NOREADY);

    int ret = GT_ZeroPos(m_cardIndex, m_axisIndex + 1);
    if (ret)
    {
        LOG_ERR("GT_ZeroPos return error:%d", ret);
    }

    return ret;
}

int CAxis::HomeAsCapture(long homePos, long offset, double vel, int type)
{
    long pos = 0;
    short capture = 0;
    long status = 0;
    double prfPos = 0.0f;
    int ret = 0;
    long doffset = 0;

    if (EHOME_CAPTURE != type)
    {//<2F>ŷ<EFBFBD><C5B7><EFBFBD><EFBFBD><EFBFBD>
        doffset = (long)(GetCurPos(EMPos_Profile) - GetCurPos(EMPos_Encoder));
    }

    ret = GT_SetCaptureMode(m_cardIndex, m_axisIndex + 1, CAPTURE_HOME);  //<2F><><EFBFBD>û<EFBFBD>ԭģʽ
    if (ret)
    {
        LOG_ERR("GT_SetCaptureMode return error:%d", ret);
        return ret;
    }

    //ret = MovePos(homePos, vel, vel / 2, vel / 2);
    ret = MovePos(homePos, vel, vel / 10, vel / 10);

    RETURN_CHK_NOPRT(!ret, ret);

    LOG_INF1("GT_SetCaptureMode start.", m_cardIndex, m_axisIndex);
    do
    {// <20>ȴ<EFBFBD><C8B4><EFBFBD><EFBFBD>񴥷<EFBFBD>
        ret = GT_GetSts(m_cardIndex, m_axisIndex + 1, &status); //<2F><>ȡ<EFBFBD><C8A1>״̬
        ret = GT_GetCaptureStatus(m_cardIndex, m_axisIndex + 1, &capture, &pos);    //<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>״̬
        ret = GT_GetPrfPos(m_cardIndex, m_axisIndex + 1, &prfPos);  //<2F><>ȡ<EFBFBD>滮λ<E6BBAE><CEBB>
        //ret = GT_GetEncPos(m_cardIndex, m_axisParam.axisIndex + 1, &encPos);  //<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
        // <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD>س<EFBFBD><D8B3><EFBFBD><EFBFBD><EFBFBD>Ϣ
        if(0 == (status & 0x400))
        {
            LOG_ERR("GT_GetCaptureStatus return error.");
            return ERR_TIMEOUT;
        }

        RETURN_CHK_NOPRT(!WaitExitMoveSign(20), ERR_TIMEOUT);
    } while( 0 == capture);

    //StopAndWait();  //<2F><>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD>Ŀ<EFBFBD><C4BF>λ<EFBFBD><CEBB>

    ret = SetPos((EHOME_CAPTURE == type ? (long)prfPos : pos + doffset) +
        ((EHOME_EINDEX == type) ? (homePos > 0 ? 2000 : -2000) : offset));

    RETURN_CHK_NOPRT(!ret, ret);

    while (isRealMoving())
    {//<2F>ȴ<EFBFBD><C8B4>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD>
        RETURN_CHK_NOPRT(!WaitExitMoveSign(20), ERR_TIMEOUT);
    }

    if (EHOME_EINDEX == type)
    {//index <20><><EFBFBD><EFBFBD>
        sys_Sleep(200);
        return HomeAsIndex((homePos > 0 ? -20000 : 20000), offset, doffset);
    }

    return RETURN_OK;
}

int CAxis::HomeAsIndex(long homePos, long offset, long doffset)
{//<2F>ŷ<EFBFBD><C5B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    long pos = 0;
    short capture = 0;
    long status = 0;
    int ret = 0;

    ret = GT_SetCaptureMode(m_cardIndex, m_axisIndex + 1, CAPTURE_INDEX);  //<2F><><EFBFBD><EFBFBD>ģʽ
    if (ret)
    {
        LOG_ERR("GT_SetCaptureMode index return error:%d", ret);
        return ret;
    }

    ret = SetPos(homePos);

    RETURN_CHK_NOPRT(!ret, ret);

    LOG_INF1("GT_SetCaptureMode Card:%d,Axis:%d start home as index.", m_cardIndex, m_axisIndex);
    do
    {//<2F>ȴ<EFBFBD><C8B4><EFBFBD><EFBFBD>񴥷<EFBFBD>
        ret = GT_GetSts(m_cardIndex, m_axisIndex + 1, &status); //<2F><>ȡ<EFBFBD><C8A1>״̬
        ret = GT_GetCaptureStatus(m_cardIndex, m_axisIndex + 1, &capture, &pos);    //<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>״̬
        // <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD>س<EFBFBD><D8B3><EFBFBD><EFBFBD><EFBFBD>Ϣ
        if(0 == (status & 0x400))
        {
            LOG_ERR("GT_GetCaptureStatus return error.");
            return ERR_TIMEOUT;
        }

        RETURN_CHK_NOPRT(!WaitExitMoveSign(20), ERR_TIMEOUT);
    } while( 0 == capture);

    //StopAndWait();  //<2F><>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD>Ŀ<EFBFBD><C4BF>λ<EFBFBD><CEBB>

    ret = SetPos(pos + offset + doffset);

    RETURN_CHK_NOPRT(!ret, ret);

    while (isRealMoving())
    {//<2F>ȴ<EFBFBD><C8B4>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD>
        RETURN_CHK_NOPRT(!WaitExitMoveSign(20), ERR_TIMEOUT);
    }
    return 0;
}

int CAxis::HomeAsNormal(long homePos, long offset, double vel)
{
    ushort state = 0;

    int ret = GT_Home(m_cardIndex, m_axisIndex + 1, homePos, vel, vel / 10, offset);
    if (ret)
    {
        LOG_ERR("GT_Home return error:%d", ret);
        return ret;
    }

    LOG_INF1("GT_Home.card%d axis%d", m_cardIndex, m_axisIndex);
    while(true)
    {
        ret = GT_HomeSts(m_cardIndex, m_axisIndex + 1, &state);
        if (ret)
        {
            LOG_ERR("GT_HomeSts return error:%d", ret);
            return ret;
        }

        if (0 != state)
        {//<2F>Զ<EFBFBD><D4B6><EFBFBD>ԭִ<D4AD><D6B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.//if(1 == state)
            break;
        }

        //<2F>Զ<EFBFBD><D4B6><EFBFBD>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD>ִ<EFBFBD><D6B4><EFBFBD><EFBFBD>...
        RETURN_CHK_NOPRT(!WaitExitMoveSign(20), ERR_TIMEOUT);
    }

    while(isRealMoving())
    {
        RETURN_CHK_NOPRT(!WaitExitMoveSign(20), ERR_TIMEOUT);
    }

    return RETURN_OK;
}

int CAxis::Home(ushort homeType, long pos, long offset, double vel)
{
    int ret = ERR_INPUT_PARAM;

    RETURN_CHK_NOPRT(m_bInit, ERR_NO_INIT);

    sys_ResetEvent(m_hQuitEvt); //<2F><><EFBFBD>ź<EFBFBD>
    sys_ResetEvent(m_hHomeFinishedEvt);
    switch (homeType)
    {
    case EHOME_NORMAL:
        ret = HomeAsNormal(pos, offset, vel);
        break;
    case EHOME_CAPTURE:
        ret = HomeAsCapture(pos, offset, vel);
        break;
    case EHOME_ECAPTURE:
        ret = HomeAsCapture(pos, offset, vel, EHOME_ECAPTURE);
        break;
    case EHOME_EINDEX:
        ret = HomeAsCapture(pos, offset, vel, EHOME_EINDEX);
        break;
    default:
        break;
    }

    sys_SetEvent(m_hHomeFinishedEvt);
    LOG_INF1("Home(%d,%d,%d,%d,%d,%.2lf)=%d", m_cardIndex, m_axisIndex, homeType, pos, offset, vel, ret);
    return ret;
}

int CAxis::GearMove(CAxis* pAxis, int iGearType, double ratio, long masterSlope)
{
    short masterType = 0;
    short masterItem = 0;
    long mastertEven = 0;
    long slaveEven = 1;
    long tmp = 0;

    //<2F><><EFBFBD>ø<EFBFBD><C3B8><EFBFBD>ģʽ
    short rtn = GT_PrfGear(m_cardIndex, m_axisIndex + 1);
    if (rtn)
    {
        LOG_ERR("GT_PrfGear return error:%d", rtn);
        return rtn;
    }

    switch (iGearType)
    {
    case EM_GEAR_ENCODER:
        masterType = GEAR_MASTER_ENCODER;
        break;
    case EM_GEAR_PROFILE:
        masterType = GEAR_MASTER_PROFILE;
        break;
    case EM_GEAR_AXIS_ENCODER:
        masterType = GEAR_MASTER_AXIS;
        break;
    case EM_GEAR_AXIS_PROFILE:
        masterType = GEAR_MASTER_AXIS;
        masterItem = 1;
        break;
    default:
        break;
    }

    tmp = ((long)(ratio * 100)) % 100;  //<2F><>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2λ
    if (0 == tmp)
    {//<2F><><EFBFBD><EFBFBD>
        mastertEven = (long)ratio;
    }
    if (99 == tmp)
    {//<2F><>һλ<D2BB><CEBB><EFBFBD><EFBFBD>
        mastertEven = ((long)ratio) + 1;
    }
    else if (0 == tmp % 10)
    {//С<><D0A1><EFBFBD><EFBFBD>1λ
        mastertEven = (long)(ratio * 10);
        slaveEven = 10;
    }
    else
    {//С<><D0A1><EFBFBD><EFBFBD>2λ
        mastertEven = (long)(ratio * 100);
        slaveEven = 100;
    }

    //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ᣬĬ<E1A3AC>ϸ<EFBFBD><CFB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>滮λ<E6BBAE><CEBB>
    rtn = GT_SetGearMaster(m_cardIndex, m_axisIndex + 1, pAxis->GetAxisIndex() + 1, masterType, masterItem);
    if (rtn)
    {
        LOG_ERR("GT_SetGearMaster return error:%d", rtn);
        return rtn;
    }

    //<2F><><EFBFBD>ô<EFBFBD><C3B4><EFBFBD><EFBFBD>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD>Ⱥ<EFBFBD><C8BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    rtn = GT_SetGearRatio(m_cardIndex, m_axisIndex + 1, mastertEven, slaveEven, masterSlope);
    if (rtn)
    {
        LOG_ERR("GT_SetGearRatio return error:%d", rtn);
        return rtn;
    }

    //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    rtn = GT_GearStart(m_cardIndex, 1<<m_axisIndex);
    if (rtn)
    {
        LOG_ERR("GT_GearStart return error:%d", rtn);
        return rtn;
    }

    return RETURN_OK;
}