#include "StdAfx.h"
|
#include "CHMotorControls/MotorControl_eXcom.h"
|
#include "eXcomDevice.h"
|
|
CMotorControl_eXcom::CMotorControl_eXcom(int nIndex, DWORD dwPeriod, int nThreadCount) : CMotorControl(nIndex, dwPeriod, nThreadCount)
|
{
|
m_peXcomDevice = new CeXcomDevice(nIndex);
|
}
|
|
CMotorControl_eXcom::~CMotorControl_eXcom(void)
|
{
|
//AfxMessageBox(_T("CMotorControl_eXcom"));
|
|
Disconnect();
|
|
if (m_peXcomDevice)
|
{
|
delete m_peXcomDevice;
|
m_peXcomDevice = NULL;
|
}
|
|
m_bCheckConnect = FALSE;
|
}
|
|
int CMotorControl_eXcom::Connect(const CMotorControlInfo* pControlInfo)
|
{
|
if (pControlInfo==NULL) return 0;
|
|
// set motor control info
|
m_ControlInfo = *pControlInfo;
|
|
if(m_peXcomDevice == NULL) return 0;
|
|
int nPort;
|
CString strIPAddress;
|
DWORD dwIPAddress;
|
|
nPort = 1500;
|
strIPAddress = m_ControlInfo.GetConnectionPort();
|
|
TransformIP_CStringToDWORD(strIPAddress, dwIPAddress);
|
|
m_bConnected = FALSE;
|
|
m_bConnected = m_peXcomDevice->Connect(dwIPAddress, nPort);
|
|
if (m_bConnected==FALSE) 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_eXcom::Disconnect()
|
{
|
if(m_peXcomDevice == NULL) return;
|
|
if (m_bConnected)
|
{
|
m_peXcomDevice->Close();
|
m_bConnected = FALSE;
|
}
|
}
|
|
BOOL CMotorControl_eXcom::ReadAddressValue(int nAddress, double &dValue)
|
{
|
if (!m_bConnected) return FALSE;
|
if (nAddress<0) return FALSE;
|
if (m_peXcomDevice==NULL) return FALSE;
|
|
float fValue = 0.0f;
|
if (!m_peXcomDevice->ReadOneData(nAddress, fValue))
|
{
|
return FALSE;
|
}
|
|
dValue = fValue;
|
|
return TRUE;
|
}
|
|
BOOL CMotorControl_eXcom::ReadAddressValue(int nAddress, long &nValue)
|
{
|
if (!m_bConnected) return FALSE;
|
if (nAddress<0) return FALSE;
|
if (m_peXcomDevice==NULL) return FALSE;
|
|
return m_peXcomDevice->ReadOneData(nAddress, nValue);
|
}
|
|
BOOL CMotorControl_eXcom::ReadAddressValue(int nAddress, float &fValue)
|
{
|
if (!m_bConnected) return FALSE;
|
if (nAddress<0) return FALSE;
|
if (m_peXcomDevice==NULL) return FALSE;
|
|
return m_peXcomDevice->ReadOneData(nAddress, fValue);
|
}
|
|
BOOL CMotorControl_eXcom::WriteAddressValue(int nAddress, float fValue)
|
{
|
if (!m_bConnected) return FALSE;
|
if (nAddress<0) return FALSE;
|
if (m_peXcomDevice==NULL) return FALSE;
|
|
return m_peXcomDevice->WriteOneData(nAddress, fValue);
|
}
|
|
BOOL CMotorControl_eXcom::WriteAddressValue(int nAddress, double dValue)
|
{
|
if (!m_bConnected) return FALSE;
|
if (nAddress<0) return FALSE;
|
if (m_peXcomDevice==NULL) return FALSE;;
|
|
float fValue = (float)dValue;
|
return m_peXcomDevice->WriteOneData(nAddress, fValue);
|
}
|
|
BOOL CMotorControl_eXcom::WriteAddressValue(int nAddress, long nValue)
|
{
|
if (!m_bConnected) return FALSE;
|
if (nAddress<0) return FALSE;
|
if (m_peXcomDevice==NULL) return FALSE;
|
|
return m_peXcomDevice->WriteOneData(nAddress, nValue);
|
}
|
|
BOOL CMotorControl_eXcom::ReadAddressValue(long nAddress, long *pArrayData, int nArrayCount)
|
{
|
if (!m_bConnected) return FALSE;
|
if (nAddress<0) return FALSE;
|
if (m_peXcomDevice==NULL) return FALSE;
|
|
return m_peXcomDevice->ReadMultyData(nAddress, pArrayData, nArrayCount);
|
}
|
|
BOOL CMotorControl_eXcom::WriteAddressValue(long nAddress, long *pArrayData, int nArrayCount)
|
{
|
if (!m_bConnected) return FALSE;
|
if (nAddress<0) return FALSE;
|
if (m_peXcomDevice==NULL) return FALSE;
|
|
return m_peXcomDevice->WriteMultyData(nAddress, pArrayData, nArrayCount);
|
}
|
|
// use read position func
|
BOOL CMotorControl_eXcom::ReadAddressValue(long nAddress, float *pArrayData, int nArrayCount)
|
{
|
if (!m_bConnected) return FALSE;
|
if (nAddress<0) return FALSE;
|
if (m_peXcomDevice==NULL) return FALSE;
|
|
BOOL bRet = m_peXcomDevice->ReadMultyData(nAddress, pArrayData, nArrayCount);
|
|
m_bCheckConnect = bRet;
|
|
return bRet;
|
}
|
|
BOOL CMotorControl_eXcom::WriteAddressValue(long nAddress, float *pArrayData, int nArrayCount)
|
{
|
if (!m_bConnected) return FALSE;
|
if (nAddress<0) return FALSE;
|
if (m_peXcomDevice==NULL) return FALSE;
|
|
return m_peXcomDevice->WriteMultyData(nAddress, pArrayData, nArrayCount);
|
}
|
|
int CMotorControl_eXcom::GetStatus( int& nStatusCode, CString& strStatusMessage )
|
{
|
m_bCheckConnect = FALSE;
|
|
Sleep(500);
|
if (m_bCheckConnect==FALSE) // Å×½ºÆ® ÇÊ¿ä
|
{
|
nStatusCode = MotorStatus_NotConnected;
|
strStatusMessage = _T("Not_Connected");
|
return 0;
|
}
|
|
nStatusCode = MotorStatus_Connected;
|
strStatusMessage = _T("Connected");
|
|
return 1;
|
}
|
