#include "StdAfx.h"
|
#include "CHMotorControls/MotorControl_PowerPmacCmd.h"
|
|
#include "PowerPMAC/PowerPmac.h"
|
#include "PowerPMAC/PowerPmacDef.h"
|
|
CMotorControl_PowerPmacCmd::CMotorControl_PowerPmacCmd(int nIndex, DWORD dwPeriod, int nThreadCount) : CMotorControl(nIndex, dwPeriod, nThreadCount)
|
{
|
m_uDeviceID = 0;
|
}
|
|
CMotorControl_PowerPmacCmd::~CMotorControl_PowerPmacCmd(void)
|
{
|
//AfxMessageBox(_T("CMotorControl_PowerPmacCmd"));
|
|
Disconnect();
|
}
|
|
|
// CMotorControl
|
int CMotorControl_PowerPmacCmd::Connect(const CMotorControlInfo* pControlInfo)
|
{
|
if (pControlInfo==NULL) return 0;
|
|
// set motor control info
|
m_ControlInfo = *pControlInfo;
|
|
DWORD dwIPAddr = 0;
|
|
TransformIP_CStringToDWORD(m_ControlInfo.GetConnectionPort(), dwIPAddr);
|
|
int nPort = 0;
|
|
// Power PMAC Open
|
m_uDeviceID = DTKPowerPmacOpen(dwIPAddr, nPort);
|
|
m_bConnected = FALSE;
|
|
// Device Connect
|
if (DTKConnect(m_uDeviceID) != DS_Ok)
|
{
|
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 0;
|
}
|
|
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 *ANode = pGNode->GetMotorAxisAddr(nA_Idx);
|
if (ANode==NULL) continue;
|
|
m_pAxisPositionAddr[nAxisIndex] = ANode->m_nPositionAddr;
|
m_pAxisStatusAddr[nAxisIndex] = ANode->m_nStatusAddr;
|
nAxisIndex++;
|
}
|
}
|
|
m_axisThetaStatus = 0;
|
m_axisThetaPosition = 0.f;
|
m_axisThetaStatusPrev = 0;
|
m_axisThetaPositionPrev = -999.f;
|
|
m_bConnected = TRUE;
|
|
return 1;
|
}
|
|
void CMotorControl_PowerPmacCmd::Disconnect()
|
{
|
if (m_bConnected==FALSE) return;
|
|
// Power PMAC Close
|
DTKDisconnect(m_uDeviceID);
|
|
DTKPowerPmacClose(m_uDeviceID);
|
|
m_bConnected = FALSE;
|
}
|
|
BOOL CMotorControl_PowerPmacCmd::ReadAddressValue(int nAddress, long &nValue)
|
{
|
if (!m_bConnected) return FALSE;
|
|
TCHAR pchResponse[256];
|
CString str;
|
str.Format(_T("sys.idata[%d]"), nAddress);
|
UINT nResult = DTKGetResponseA(m_uDeviceID, (LPSTR)(LPCTSTR)str, pchResponse, 256);
|
|
CString strResponse;
|
strResponse.Format(_T("%s"), pchResponse);
|
|
if (nResult == DS_Ok)
|
{
|
if (strResponse.GetLength()>0)
|
{
|
strResponse=strResponse.Mid(strResponse.Find(_T("="))+1);
|
nValue = _ttof(strResponse);
|
}
|
return TRUE;
|
}
|
|
return FALSE;
|
}
|
|
BOOL CMotorControl_PowerPmacCmd::WriteAddressValue(int nAddress, long nValue)
|
{
|
if (!m_bConnected) return FALSE;
|
|
TCHAR pchResponse[256];
|
|
CString str;
|
str.Format(_T("sys.idata[%d]=%d"), nAddress, nValue);
|
UINT nResult = DTKGetResponseA(m_uDeviceID, (LPSTR)(LPCTSTR)str, pchResponse, 256);
|
|
if (nResult == DS_Ok)
|
return TRUE;
|
|
return FALSE;
|
}
|
|
BOOL CMotorControl_PowerPmacCmd::ReadAddressValue(int nAddress, float &fValue)
|
{
|
if (!m_bConnected) return FALSE;
|
|
TCHAR pchResponse[256];
|
CString str;
|
str.Format(_T("sys.fdata[%d]"), nAddress);
|
UINT nResult = DTKGetResponseA(m_uDeviceID, (LPSTR)(LPCTSTR)str, pchResponse, 256);
|
|
CString strResponse;
|
strResponse.Format(_T("%s"), pchResponse);
|
|
if (nResult == DS_Ok)
|
{
|
if (strResponse.GetLength()>0)
|
{
|
strResponse=strResponse.Mid(strResponse.Find(_T("="))+1);
|
fValue = (float)_ttof(strResponse);
|
}
|
return TRUE;
|
}
|
|
return FALSE;
|
}
|
|
BOOL CMotorControl_PowerPmacCmd::WriteAddressValue(int nAddress, float fValue)
|
{
|
if (!m_bConnected) return FALSE;
|
|
TCHAR pchResponse[256];
|
|
CString str;
|
str.Format(_T("sys.fdata[%d]=%f"), nAddress, fValue);
|
UINT nResult = DTKGetResponseA(m_uDeviceID, (LPSTR)(LPCTSTR)str, pchResponse, 256);
|
|
if (nResult == DS_Ok)
|
return TRUE;
|
|
return FALSE;
|
}
|
|
BOOL CMotorControl_PowerPmacCmd::ReadAddressValue(int nAddress, double &dValue)
|
{
|
if (!m_bConnected) return FALSE;
|
|
BOOL bSuccess = TRUE;
|
TCHAR pchResponse[256];
|
|
CString str;
|
str.Format(_T("sys.fdata[%d]"), nAddress);
|
UINT nResult = DTKGetResponseA(m_uDeviceID, (LPSTR)(LPCTSTR)str, pchResponse, 256);
|
|
CString strResponse;
|
strResponse.Format(_T("%s"), pchResponse);
|
|
if (nResult == DS_Ok)
|
{
|
if (strResponse.GetLength()>0)
|
{
|
strResponse=strResponse.Mid(strResponse.Find(_T("="))+1);
|
dValue = _ttof(strResponse);
|
}
|
return TRUE;
|
}
|
|
return FALSE;
|
}
|
|
BOOL CMotorControl_PowerPmacCmd::WriteAddressValue(int nAddress, double dValue)
|
{
|
if (!m_bConnected) return FALSE;
|
|
TCHAR pchResponse[256];
|
|
CString str;
|
str.Format(_T("sys.fdata[%d]=%lf"), nAddress, dValue);
|
UINT nResult = DTKGetResponseA(m_uDeviceID, (LPSTR)(LPCTSTR)str, pchResponse, 256);
|
|
if (nResult == DS_Ok)
|
return TRUE;
|
|
return FALSE;
|
}
|
|
BOOL CMotorControl_PowerPmacCmd::ReadAddressValue(long nAddress, long *pArrayData, int nArrayCount)
|
{
|
BOOL bResult = TRUE;
|
for (int i=0; i<nArrayCount; i++)
|
{
|
bResult = bResult && ReadAddressValue(nAddress+i, pArrayData[i]);
|
}
|
return bResult;
|
}
|
|
BOOL CMotorControl_PowerPmacCmd::WriteAddressValue(long nAddress, long *pArrayData, int nArrayCount)
|
{
|
BOOL bResult = TRUE;
|
for (int i=0; i<nArrayCount; i++)
|
{
|
bResult = bResult && WriteAddressValue(nAddress+i, pArrayData[i]);
|
}
|
return bResult;
|
}
|
|
BOOL CMotorControl_PowerPmacCmd::ReadAddressValue(long nAddress, float *pArrayData, int nArrayCount)
|
{
|
BOOL bResult = TRUE;
|
for (int i=0; i<nArrayCount; i++)
|
{
|
bResult = bResult && ReadAddressValue(nAddress+i, pArrayData[i]);
|
}
|
return bResult;
|
}
|
|
BOOL CMotorControl_PowerPmacCmd::WriteAddressValue(long nAddress, float *pArrayData, int nArrayCount)
|
{
|
BOOL bResult = TRUE;
|
for (int i=0; i<nArrayCount; i++)
|
{
|
bResult = bResult && WriteAddressValue(nAddress+i, pArrayData[i]);
|
}
|
return bResult;
|
}
|
|
int CMotorControl_PowerPmacCmd::GetStatus( int& nStatusCode, CString& strStatusMessage )
|
{
|
if (GetConnected()==FALSE)
|
{
|
nStatusCode = MotorStatus_NotConnected;
|
strStatusMessage = _T("Not_Connected");
|
return 0;
|
}
|
|
nStatusCode = MotorStatus_Connected;
|
strStatusMessage = _T("Connected");
|
|
return 1;
|
}
|
