updown/plugin/motor_gugao/Axis.cpp

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

enum EM_GEAR_TYPE
{//跟随模式
    EM_GEAR_ENCODER = 0,    //跟随编码器位置
    EM_GEAR_PROFILE,        //跟随规划轴位置
    EM_GEAR_AXIS_ENCODER,   //跟随轴编码器位置
    EM_GEAR_AXIS_PROFILE,   //跟随轴规划位置
};

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();  //清除状态

    /*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();  //清除状态
}

bool CAxis::WaitExitMoveSign(int iWait)
{
    if (0 == sys_WaitForSingleObject(m_hQuitEvt, iWait))
    {//有退出信号
        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);

    //更新到控制器 -- 启动运动
    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);

    //设为Jog模式
    ret = GT_PrfJog(m_cardIndex, m_axisIndex + 1);
    if (ret)
    {
        LOG_ERR("GT_PrfJog return error:%d", ret);
        return ret;
    }

    //读取Jog运动参数
    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;  //设置Jog运动参数
    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)
    {//无到位检测
        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);   //给个退出信号
    return StopMove(type);
}

void CAxis::StopAndWait(int type)
{
    StopMove(type);
    while(isRealMoving())
    {//等待移动结束
        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)  //关闭伺服报警
    {
        LOG_ERR("GT_AlarmOff return error:%d", ret);
    }

    ret = GT_LmtsOff(m_cardIndex, m_axisIndex + 1);
    if (ret)  //关闭限位报警
    {
        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)
    {//伺服捕获
        doffset = (long)(GetCurPos(EMPos_Profile) - GetCurPos(EMPos_Encoder));
    }

    ret = GT_SetCaptureMode(m_cardIndex, m_axisIndex + 1, CAPTURE_HOME);  //设置回原模式
    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
    {// 等待捕获触发
        ret = GT_GetSts(m_cardIndex, m_axisIndex + 1, &status); //读取轴状态
        ret = GT_GetCaptureStatus(m_cardIndex, m_axisIndex + 1, &capture, &pos);    //读取捕获状态
        ret = GT_GetPrfPos(m_cardIndex, m_axisIndex + 1, &prfPos);  //读取规划位置
        //ret = GT_GetEncPos(m_cardIndex, m_axisParam.axisIndex + 1, &encPos);  //读取编码器位置
        // 如果运动停止，返回出错信息
        if(0 == (status & 0x400))
        {
            LOG_ERR("GT_GetCaptureStatus return error.");
            return ERR_TIMEOUT;
        }

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

    //StopAndWait();  //先停止，再走到目标位置

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

    RETURN_CHK_NOPRT(!ret, ret);

    while (isRealMoving())
    {//等待移动结束
        RETURN_CHK_NOPRT(!WaitExitMoveSign(20), ERR_TIMEOUT);
    }

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

    return RETURN_OK;
}

int CAxis::HomeAsIndex(long homePos, long offset, long doffset)
{//伺服才能这样回
    long pos = 0;
    short capture = 0;
    long status = 0;
    int ret = 0;

    ret = GT_SetCaptureMode(m_cardIndex, m_axisIndex + 1, CAPTURE_INDEX);  //设置模式
    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
    {//等待捕获触发
        ret = GT_GetSts(m_cardIndex, m_axisIndex + 1, &status); //读取轴状态
        ret = GT_GetCaptureStatus(m_cardIndex, m_axisIndex + 1, &capture, &pos);    //读取捕获状态
        // 如果运动停止，返回出错信息
        if(0 == (status & 0x400))
        {
            LOG_ERR("GT_GetCaptureStatus return error.");
            return ERR_TIMEOUT;
        }

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

    //StopAndWait();  //先停止，再走到目标位置

    ret = SetPos(pos + offset + doffset);

    RETURN_CHK_NOPRT(!ret, ret);

    while (isRealMoving())
    {//等待移动结束
        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)
        {//自动回原执行完成.//if(1 == state)
            break;
        }

        //自动回原正在执行中...
        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); //无信号
    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;

    //设置跟随模式
    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;  //求小数点后面2位
    if (0 == tmp)
    {//整数
        mastertEven = (long)ratio;
    }
    if (99 == tmp)
    {//加一位整数
        mastertEven = ((long)ratio) + 1;
    }
    else if (0 == tmp % 10)
    {//小数点1位
        mastertEven = (long)(ratio * 10);
        slaveEven = 10;
    }
    else
    {//小数点2位
        mastertEven = (long)(ratio * 100);
        slaveEven = 100;
    }

    //设置主轴，默认跟随主轴规划位置
    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;
    }

    //设置从轴的传动比和离合区
    rtn = GT_SetGearRatio(m_cardIndex, m_axisIndex + 1, mastertEven, slaveEven, masterSlope);
    if (rtn)
    {
        LOG_ERR("GT_SetGearRatio return error:%d", rtn);
        return rtn;
    }

    //启动从轴
    rtn = GT_GearStart(m_cardIndex, 1<<m_axisIndex);
    if (rtn)
    {
        LOG_ERR("GT_GearStart return error:%d", rtn);
        return rtn;
    }

    return RETURN_OK;
}