SDC/A5_QD_CF_REVIEW.git

#include "StdAfx.h"
#include "CHMotorControls/MotorControl_Acs.h"
 
#define STR_SIZE_ADDRESS    10
#define STR_ERR_MESSAGE        100
 
#define MEMORY_MAX_SIZE        512
 
CMotorControl_Acs::CMotorControl_Acs(int nIndex, DWORD dwPeriod, int nThreadCount) : CMotorControl(nIndex, dwPeriod, nThreadCount)
{
    m_hAcsComm = ACSC_INVALID;
}
 
CMotorControl_Acs::~CMotorControl_Acs(void)
{
    //AfxMessageBox(_T("CMotorControl_Acs"));
 
    Disconnect();
}
 
int    CMotorControl_Acs::Connect(const CMotorControlInfo* pControlInfo)
{
    if (pControlInfo==NULL) return 0;
 
    // set motor control info
    m_ControlInfo = *pControlInfo;
 
    // initial handle
    m_hAcsComm = ACSC_INVALID;
 
    int nPort;
    char strIPAddress[50] = {0,};
 
    nPort = ACSC_SOCKET_STREAM_PORT;
    USES_CONVERSION;
    sprintf_s(strIPAddress, "%s", m_ControlInfo.GetConnectionPort());
 
    m_bConnected = FALSE;
 
    m_hAcsComm = acsc_OpenCommEthernetTCP(strIPAddress, nPort);
    if(m_hAcsComm == ACSC_INVALID) return 0;
 
    // alloc 
    m_nAxisCount = 0;
    for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
    {
        CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
        if (pGNode==NULL) continue;
        m_nAxisCount += pGNode->GetMotorAxisAddrCount();
    }
 
    if (m_nAxisCount<1) 
    {
        return -2;
    }
 
    if (m_pAxisStatus) delete  [] m_pAxisStatus;
    m_pAxisStatus = new long[m_nAxisCount];
    memset(m_pAxisStatus, 0, sizeof(long)*m_nAxisCount);
 
    if (m_pAxisPosition) delete  [] m_pAxisPosition;
    m_pAxisPosition = new float[m_nAxisCount];
    memset(m_pAxisPosition, -1, sizeof(float)*m_nAxisCount);
 
    if (m_pAxisStatusPrev) delete  [] m_pAxisStatusPrev;
    m_pAxisStatusPrev = new long[m_nAxisCount];
    memset(m_pAxisStatusPrev, 0, sizeof(long)*m_nAxisCount);
 
    if (m_pAxisPositionPrev) delete  [] m_pAxisPositionPrev;
    m_pAxisPositionPrev = new float[m_nAxisCount];
    memset(m_pAxisPositionPrev, -1, sizeof(float)*m_nAxisCount);
 
    if (m_pAxisStatusAddr) delete  [] m_pAxisStatusAddr;
    m_pAxisStatusAddr = new long[m_nAxisCount];
 
    if (m_pAxisPositionAddr) delete  [] m_pAxisPositionAddr;
    m_pAxisPositionAddr = new long[m_nAxisCount];
 
    int nAxisIndex = 0;
    for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
    {
        CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
        if (pGNode==NULL) continue;
 
        for (int nA_Idx=0; nA_Idx<pGNode->GetMotorAxisAddrCount(); nA_Idx++)
        {
            CMotorAxisAddr *pANode = pGNode->GetMotorAxisAddr(nA_Idx);
            if (pANode==NULL) continue;
 
            m_pAxisPositionAddr[nAxisIndex] = pANode->m_nPositionAddr;
            m_pAxisStatusAddr[nAxisIndex] = pANode->m_nStatusAddr;
            nAxisIndex++;
        }
    }
 
    m_axisThetaStatus            = 0;
    m_axisThetaPosition            = 0.f;
    m_axisThetaStatusPrev        = 0;
    m_axisThetaPositionPrev        = -999.f;
 
    m_bConnected = TRUE;
 
    return 1;
}
 
void CMotorControl_Acs::Disconnect()
{
    if(m_hAcsComm != ACSC_INVALID)
    {
        if (acsc_CloseComm(m_hAcsComm))
        {
            m_hAcsComm = ACSC_INVALID; 
            m_bConnected = FALSE;
        }
    }
}
 
// gantry
BOOL CMotorControl_Acs::GantryManualGo( int nGantryIdx, const VectorDouble& vectorPos )
{
    UINT nManualAddr = GetGantryManualGoAddr(nGantryIdx);
    if (nManualAddr==MOTOR_ADDRESS_NONE) return FALSE;
 
    int nAxisCount = 0;
    for (int i=0; i<nGantryIdx; i++)
    {
        nAxisCount += GetGantryAxisCount(i);
    }
 
    long nValue = 0;
    for (int i=0; i<(int)vectorPos.size(); i++)
    {
        UINT nGoPosAddr = GetAxisGoPositionAddr(nAxisCount+i);
        if (nGoPosAddr==MOTOR_ADDRESS_NONE) return FALSE;
 
        float dataPosition[2];
        ZeroMemory(dataPosition, sizeof(float)*2);
 
        dataPosition[0] = float(vectorPos[i]);
        if (WriteAddressValue(nGoPosAddr, dataPosition, 1)==FALSE)
        {
            return FALSE;
        }
 
        nValue = nValue | (1 << i);
    }
 
    return WriteAddressValue(nManualAddr, nValue);
}
 
BOOL CMotorControl_Acs::GantryManualGo( int nGantryIdx, const VectorDouble& vectorPos, int nMoveType )
{
    UINT nManualAddr = GetGantryManualGoAddr(nGantryIdx);
    if (nManualAddr==MOTOR_ADDRESS_NONE) return FALSE;
 
    int nAxisCount = 0;
    for (int i=0; i<nGantryIdx; i++)
    {
        nAxisCount += GetGantryAxisCount(i);
    }
 
    int nBitData = 1;
    for (int i=0; i<GetGantryAxisCount(nGantryIdx); i++)
    {
        if ( nBitData & nMoveType ) 
        {
            UINT nGoPosAddr = GetAxisGoPositionAddr(nAxisCount+i);
            if (nGoPosAddr!=MOTOR_ADDRESS_NONE) 
            {
                float dataPosition[2];
                ZeroMemory(dataPosition, sizeof(float)*2);
 
                dataPosition[0] = float(vectorPos[i]);
                if (WriteAddressValue(nGoPosAddr, dataPosition, 1)==FALSE)
                {
                    continue;
                }
            }
        }
        nBitData = nBitData << 1;
    }
    Sleep(700);
    BOOL bResult = WriteAddressValue(nManualAddr, (long) nMoveType);
 
    //return WriteAddressValue(nManualAddr, (long) nMoveType);
    return bResult;
}
 
BOOL CMotorControl_Acs::GantryManualGo(int nGantryIdx, double dPosX, double dPosY)
{
    UINT nManualAddr = GetGantryManualGoAddr(nGantryIdx);
    if (nManualAddr==MOTOR_ADDRESS_NONE) return FALSE;
 
    int nAxisCount = 0;
    for (int i=0; i<nGantryIdx; i++)
    {
        nAxisCount += GetGantryAxisCount(i);
    }
 
    UINT nXGoPosAddr = GetAxisGoPositionAddr(nAxisCount);
    if (nXGoPosAddr==MOTOR_ADDRESS_NONE) return FALSE;
 
    UINT nYGoPosAddr = GetAxisGoPositionAddr(nAxisCount+1);
 
    if (nYGoPosAddr==MOTOR_ADDRESS_NONE) return FALSE;
 
    float dataPosition[2];
    ZeroMemory(dataPosition, sizeof(float)*2);
 
    dataPosition[0] = float(dPosX);
    if (WriteAddressValue(nXGoPosAddr, dataPosition, 1)==FALSE)
    {
        return FALSE;
    }
 
    dataPosition[0] = float(dPosY);
    if (WriteAddressValue(nYGoPosAddr, dataPosition, 1)==FALSE)
    {
        return FALSE;
    }
 
    return WriteAddressValue(nManualAddr, (long)3);
}
 
BOOL CMotorControl_Acs::AxisManualGo(int nAxisIdx, double dPosition)
{
    int nGantryIdx = GetAxisGantryIndex(nAxisIdx);
 
    if (nGantryIdx<0) return FALSE;
 
    UINT nManualGoAddr = GetGantryManualGoAddr(nGantryIdx);
    if (nManualGoAddr==MOTOR_ADDRESS_NONE) return FALSE;
 
    UINT nGoPosAddr = GetAxisGoPositionAddr(nAxisIdx);
    if (nGoPosAddr==MOTOR_ADDRESS_NONE) return FALSE;
 
    float dataPosition[2];
    ZeroMemory(dataPosition, sizeof(float)*2);
 
    dataPosition[0] = float(dPosition);
    if (WriteAddressValue(nGoPosAddr, dataPosition, 1)==FALSE)
    {
        return FALSE;
    }
 
    int nAxisCount = 0;
    for (int i=0; i<nGantryIdx; i++)
    {
        nAxisCount += GetGantryAxisCount(i);
    }
 
    // ÇØ´ç ºñÆ®¸¸ set
    long nValue = 1 << (nAxisIdx-nAxisCount);
 
    return WriteAddressValue(nManualGoAddr, nValue);
}
 
BOOL CMotorControl_Acs::ReadAxisPosition( float *pAxisPos, int nAxisCount )
{
    BOOL bRet = FALSE;
    bRet = ReadAddressValue(m_pAxisPositionAddr[0], pAxisPos[0]);
    bRet = bRet | ReadAddressValue(m_pAxisPositionAddr[1], pAxisPos[1]);
 
    return bRet;
}
 
BOOL CMotorControl_Acs::ReadAxisStatus( long *pAxisStatus, int nAxisCount )
{
    BOOL bRet = TRUE;
 
    for (int nIdx=0; nIdx<nAxisCount; nIdx++)
    {
        bRet = bRet & ReadAddressValue(m_pAxisStatusAddr[nIdx], pAxisStatus[nIdx]);
    }
 
    return bRet;
}
 
BOOL CMotorControl_Acs::ReadAddressValue(int nAddress, double &dValue)
{
    if (!m_bConnected) return FALSE;    
    if (nAddress<0) return FALSE;
    if (m_hAcsComm==ACSC_INVALID) return FALSE;
 
    char strAddress[STR_SIZE_ADDRESS];
    ZeroMemory(strAddress, sizeof(char)*STR_SIZE_ADDRESS);
    sprintf_s(strAddress, "D%d", nAddress);
 
    if (acsc_ReadReal(m_hAcsComm, -1, strAddress, -1, -1, -1, -1, &dValue, NULL)==0) return FALSE;
 
    return TRUE;
}
 
BOOL CMotorControl_Acs::ReadAddressValue(int nAddress, long &nValue)
{
    if (!m_bConnected) return FALSE;    
    if (nAddress<0) return FALSE;
    if (m_hAcsComm==ACSC_INVALID) return FALSE;
 
    int dataValue = 0;
    char strAddress[STR_SIZE_ADDRESS];
    ZeroMemory(strAddress, sizeof(char)*STR_SIZE_ADDRESS);
    sprintf_s(strAddress, "D%d", nAddress);
 
    if (acsc_ReadInteger(m_hAcsComm, -1, strAddress, -1, -1, -1, -1, &dataValue, NULL)==0) return FALSE;
 
    nValue = long(dataValue);
 
    return TRUE;    
}
 
BOOL CMotorControl_Acs::ReadAddressValue(int nAddress, float &fValue)
{
    if (!m_bConnected) return FALSE;    
    if (nAddress<0) return FALSE;
    if (m_hAcsComm==ACSC_INVALID) return FALSE;
 
    double dataValue = 0;
    char strAddress[STR_SIZE_ADDRESS];
    ZeroMemory(strAddress, sizeof(char)*STR_SIZE_ADDRESS);
 
    sprintf_s(strAddress, "D%d", nAddress);
 
    if (acsc_ReadReal(m_hAcsComm, -1, strAddress, -1, -1, -1, -1, &dataValue, NULL)==0) return FALSE;
 
    fValue = float(dataValue);
 
    return TRUE;
}
 
BOOL CMotorControl_Acs::WriteAddressValue(int nAddress, float fValue)
{
    if (!m_bConnected) return FALSE;    
    if (nAddress<0) return FALSE;
    if (m_hAcsComm==ACSC_INVALID) return FALSE;
 
    double dataValue = double(fValue);
    char strAddress[STR_SIZE_ADDRESS];
    ZeroMemory(strAddress, sizeof(char)*STR_SIZE_ADDRESS);
    sprintf_s(strAddress, "D%d", nAddress);
 
    if (acsc_WriteReal(m_hAcsComm, -1, strAddress, -1, -1, -1, -1, &dataValue, NULL)==0) return FALSE;
 
    return TRUE;
}
 
BOOL CMotorControl_Acs::WriteAddressValue(int nAddress, double dValue)
{
    if (!m_bConnected) return FALSE;    
    if (nAddress<0) return FALSE;
    if (m_hAcsComm==ACSC_INVALID) return FALSE;
 
    char strAddress[STR_SIZE_ADDRESS];
    ZeroMemory(strAddress, sizeof(char)*STR_SIZE_ADDRESS);
    sprintf_s(strAddress, "D%d", nAddress);
 
    if (acsc_WriteReal(m_hAcsComm, -1, strAddress, -1, -1, -1, -1, &dValue, NULL)==0) return FALSE;
 
    return TRUE;
}
 
BOOL CMotorControl_Acs::WriteAddressValue(int nAddress, long nValue)
{
    if (!m_bConnected) return FALSE;    
    if (nAddress<0) return FALSE;
    if (m_hAcsComm==ACSC_INVALID) return FALSE;
 
    int dataValue = nValue;
    char strAddress[STR_SIZE_ADDRESS];
    ZeroMemory(strAddress, sizeof(char)*STR_SIZE_ADDRESS);
    sprintf_s(strAddress, "D%d", nAddress);
 
    int nRet = acsc_WriteInteger(m_hAcsComm, -1, strAddress, -1, -1, -1, -1, &dataValue, NULL);
    if (nRet==0) return FALSE;
 
    return TRUE;
}
 
BOOL CMotorControl_Acs::ReadAddressValue(long nAddress, long *pArrayData, int nArrayCount)
{
    if (!m_bConnected) return FALSE;    
    if (nAddress<0) return FALSE;
    if (m_hAcsComm==ACSC_INVALID) return FALSE;
 
    int pValue[MEMORY_MAX_SIZE];
    ZeroMemory(pValue, sizeof(int)*MEMORY_MAX_SIZE);
    char strAddress[STR_SIZE_ADDRESS];
    ZeroMemory(strAddress, sizeof(char)*STR_SIZE_ADDRESS);
    sprintf_s(strAddress, "D%d", nAddress);
 
    if (acsc_ReadInteger(m_hAcsComm, -1, strAddress, 0, nArrayCount, -1, -1, pValue, NULL)==0)
    {
        return FALSE;
    }
 
    for (int nIdx=0; nIdx<nArrayCount; nIdx++)
    {
        pArrayData[nIdx] = pValue[nIdx];
    }
 
    return TRUE;
}
 
BOOL CMotorControl_Acs::WriteAddressValue(long nAddress, long *pArrayData, int nArrayCount)
{
    if (!m_bConnected) return FALSE;    
    if (nAddress<0) return FALSE;
    if (m_hAcsComm==ACSC_INVALID) return FALSE;
 
    int pValue[MEMORY_MAX_SIZE];
    ZeroMemory(pValue, sizeof(int)*MEMORY_MAX_SIZE);
    char strAddress[STR_SIZE_ADDRESS];
    ZeroMemory(strAddress, sizeof(char)*STR_SIZE_ADDRESS);
    sprintf_s(strAddress, "D%d", nAddress);
 
    for (int nIdx=0; nIdx<nArrayCount; nIdx++)
    {
        pValue[nIdx] = pArrayData[nIdx];
    }
 
    if (acsc_WriteInteger(m_hAcsComm, -1, strAddress, 0, nArrayCount, -1, -1, pValue, NULL)==0)
    {
        return FALSE;
    }
 
    return TRUE;
}
 
BOOL CMotorControl_Acs::ReadAddressValue(long nAddress, float *pArrayData, int nArrayCount)
{
    if (!m_bConnected) return FALSE;    
    if (nAddress<0) return FALSE;
    if (m_hAcsComm==ACSC_INVALID) return FALSE;
 
    double pValue[MEMORY_MAX_SIZE];
    ZeroMemory(pValue, sizeof(double)*MEMORY_MAX_SIZE);
    char strAddress[STR_SIZE_ADDRESS];
    ZeroMemory(strAddress, sizeof(char)*STR_SIZE_ADDRESS);
 
    // find common address
    long nCommonAddress = -1;
    for (int nGIdx=0; nGIdx<m_ControlInfo.GetMotorGantryAddrCount(); nGIdx++)
    {
        const CMotorGantryAddr* pGantryAddrInfo = m_ControlInfo.GetMotorGantryAddr(nGIdx);
        if  (pGantryAddrInfo==NULL) continue;
 
        for (int nAxisIdx=0; nAxisIdx<pGantryAddrInfo->GetMotorAxisAddrCount(); nAxisIdx++)
        {
            const CMotorAxisAddr* pAxisAddrInfo = pGantryAddrInfo->GetMotorAxisAddr(nAxisIdx);
            if (pAxisAddrInfo==NULL) continue;
 
            if (nAddress == pAxisAddrInfo->m_nMovePositionAddr)
                nCommonAddress = pAxisAddrInfo->m_nMovePositionCommonAddr<=0 ? 300 : pAxisAddrInfo->m_nMovePositionCommonAddr;
        }
    }
 
    sprintf_s(strAddress, "D%d", nCommonAddress);
 
    if (acsc_ReadReal(m_hAcsComm, -1, strAddress, nAddress, nAddress+nArrayCount, -1, -1, pValue, NULL)==0)
    {
        return FALSE;
    }
 
    for (int nIdx=0; nIdx<nArrayCount; nIdx++)
    {
        pArrayData[nIdx] = float(pValue[nIdx]);
    }
 
    return TRUE;
}
 
BOOL CMotorControl_Acs::WriteAddressValue(long nAddress, float *pArrayData, int nArrayCount)
{
    if (!m_bConnected) return FALSE;    
    if (nAddress<0) return FALSE;
    if (m_hAcsComm==ACSC_INVALID) return FALSE;
 
    double pValue[MEMORY_MAX_SIZE];
    ZeroMemory(pValue, sizeof(double)*MEMORY_MAX_SIZE);
    char strAddress[STR_SIZE_ADDRESS];
    ZeroMemory(strAddress, sizeof(char)*STR_SIZE_ADDRESS);
 
    // find common address
    long nCommonAddress = -1;
    for (int nGIdx=0; nGIdx<m_ControlInfo.GetMotorGantryAddrCount(); nGIdx++)
    {
        const CMotorGantryAddr* pGantryAddrInfo = m_ControlInfo.GetMotorGantryAddr(nGIdx);
        if  (pGantryAddrInfo==NULL) continue;
 
        for (int nAxisIdx=0; nAxisIdx<pGantryAddrInfo->GetMotorAxisAddrCount(); nAxisIdx++)
        {
            const CMotorAxisAddr* pAxisAddrInfo = pGantryAddrInfo->GetMotorAxisAddr(nAxisIdx);
            if (pAxisAddrInfo==NULL) continue;
 
            if (nAddress == pAxisAddrInfo->m_nMovePositionAddr)
                nCommonAddress = pAxisAddrInfo->m_nMovePositionCommonAddr<=0 ? 300 : pAxisAddrInfo->m_nMovePositionCommonAddr;
        }
    }
 
    sprintf_s(strAddress, "D%d", nCommonAddress);
 
    for (int nIdx=0; nIdx<nArrayCount; nIdx++)
    {
        pValue[nIdx] = double(pArrayData[nIdx]);
    }
 
    if (acsc_WriteReal(m_hAcsComm, -1, strAddress, nAddress, nAddress+nArrayCount-1, -1, -1, pValue, NULL)==0)
    {
        return FALSE;
    }
 
    return TRUE;
}
 
int CMotorControl_Acs::GetStatus( int& nStatusCode, CString& strStatusMessage )
{
    if (GetConnected()==FALSE) 
    {
        nStatusCode = MotorStatus_NotConnected;
        strStatusMessage = _T("Not_Connected");
        return 0;
    }
 
    if (m_hAcsComm==ACSC_INVALID) return 0;
 
    int nStatus = 0;
    if (acsc_GetMotorState(m_hAcsComm, ACSC_AXIS_0, &nStatus, NULL)==0)
    {
        int nRecvCnt = 0;
        char strErr[STR_ERR_MESSAGE+1];
        ZeroMemory(strErr, sizeof(char)*(STR_ERR_MESSAGE+1));
        int nErrCode = acsc_GetLastError();
        acsc_GetErrorString(m_hAcsComm, nErrCode, strErr, STR_ERR_MESSAGE, &nRecvCnt);
 
        strStatusMessage = strErr;
        
        return 0;
    }
 
    if ((nStatus&ACSC_MST_ENABLE) || (nStatus&ACSC_MST_INPOS) || (nStatus&ACSC_MST_MOVE) || (nStatus&ACSC_MST_ACC))
    {
        nStatusCode = MotorStatus_Connected;
        strStatusMessage = _T("Connected");
    }
    else
    {
        return 0;
    }
 
    return 1;
}