#include "StdAfx.h"
|
#include "CHMotorControls/MotorControl_PowerPmac.h"
|
#include "PPmacDprRuntime.h"
|
|
CMotorControl_PowerPmac::CMotorControl_PowerPmac(int nIndex, DWORD dwPeriod, int nThreadCount) : CMotorControl(nIndex, dwPeriod, nThreadCount)
|
{
|
m_uDeviceID = 0;
|
}
|
|
CMotorControl_PowerPmac::~CMotorControl_PowerPmac(void)
|
{
|
//AfxMessageBox(_T("CMotorControl_PowerPmac"));
|
|
Disconnect();
|
}
|
|
int CMotorControl_PowerPmac::Connect(const CMotorControlInfo* pControlInfo)
|
{
|
if (pControlInfo==NULL) return 0;
|
|
// set motor control info
|
m_ControlInfo = *pControlInfo;
|
|
ClosePPmacDprRuntimeLink();
|
|
if (OpenPPmacDprRuntimeLink()==NULL)
|
{
|
return 0;
|
}
|
|
PPmacDprDisconnect(m_uDeviceID);
|
PPmacDprClose(m_uDeviceID);
|
|
m_bConnected = FALSE;
|
|
|
DWORD dwipAddress = 0;
|
|
TransformIP_CStringToDWORD(m_ControlInfo.GetConnectionPort(), dwipAddress);
|
|
int nPortNo = 1025;
|
|
m_uDeviceID = PPmacDprOpen(dwipAddress, nPortNo);
|
if ( PPmacDprConnect(m_uDeviceID)!=PS_Ok )
|
{
|
m_bConnected = FALSE;
|
return -1;
|
}
|
|
m_bConnected = TRUE;
|
|
// 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_PowerPmac::Disconnect()
|
{
|
if (m_bConnected)
|
{
|
PPmacDprDisconnect(m_uDeviceID);
|
|
PPmacDprClose(m_uDeviceID);
|
|
m_bConnected = FALSE;
|
}
|
|
ClosePPmacDprRuntimeLink();
|
}
|
|
inline void CMotorControl_PowerPmac::SwapBytes(int nSizeType, void *pSource, int nSourceSize)
|
{
|
if (nSizeType==0 || (nSizeType%2)!=0) return;
|
|
unsigned char temp;
|
unsigned char* pSrcData = (unsigned char*)pSource;
|
unsigned char* pSrc = NULL;
|
unsigned char* pDst = NULL;
|
|
int nLoopCount = nSizeType / 2;
|
int nJumpSize = nSizeType - 1;
|
|
for (int i=0; i<nSourceSize; i++)
|
{
|
pSrc = pSrcData;
|
pDst = pSrc + nJumpSize;
|
for (int j=0; j<nLoopCount; j++)
|
{
|
// swap data
|
temp = *pSrc;
|
*pSrc = *pDst;
|
*pDst = temp;
|
|
pSrc++;
|
pDst--;
|
}
|
pSrcData += nSizeType;
|
}
|
}
|
|
BOOL CMotorControl_PowerPmac::ReadAddressValue(int nAddress, double &dValue)
|
{
|
if (!m_bConnected) return FALSE;
|
if (nAddress<0) return FALSE;
|
|
float fValue = 0.0f;
|
if (PPmacDprGetDPRMem(m_uDeviceID, (long)nAddress*4, sizeof(float), PVOID(&fValue))!=PS_Ok)
|
{
|
return FALSE;
|
}
|
|
// swap
|
SwapBytes(sizeof(float), &fValue, 1);
|
|
dValue = fValue;
|
|
return TRUE;
|
}
|
|
BOOL CMotorControl_PowerPmac::ReadAddressValue(int nAddress, long &lValue)
|
{
|
if (!m_bConnected) return FALSE;
|
if (nAddress<0) return FALSE;
|
|
lValue = 0;
|
if (PPmacDprGetDPRMem(m_uDeviceID, (long)nAddress*4, sizeof(long), PVOID(&lValue))!=PS_Ok)
|
{
|
return FALSE;
|
}
|
|
// swap
|
SwapBytes(sizeof(long), &lValue, 1);
|
|
return TRUE;
|
}
|
|
BOOL CMotorControl_PowerPmac::WriteAddressValue(int nAddress, double dValue)
|
{
|
if (!m_bConnected) return FALSE;
|
if (nAddress<0) return FALSE;
|
|
float fValue = float(dValue);
|
|
// swap
|
SwapBytes(sizeof(float), &fValue, 1);
|
|
if (PPmacDprSetDPRMem(m_uDeviceID, (long)nAddress*4, sizeof(float), PVOID(&fValue))!=PS_Ok)
|
{
|
return FALSE;
|
}
|
|
return TRUE;
|
}
|
|
BOOL CMotorControl_PowerPmac::WriteAddressValue(int nAddress, long lValue)
|
{
|
if (!m_bConnected) return FALSE;
|
if (nAddress<0) return FALSE;
|
|
// swap
|
SwapBytes(sizeof(long), &lValue, 1);
|
|
if (PPmacDprSetDPRMem(m_uDeviceID, (long)nAddress*4, sizeof(long), PVOID(&lValue))!=PS_Ok)
|
{
|
return FALSE;
|
}
|
|
return TRUE;
|
}
|
|
|
BOOL CMotorControl_PowerPmac::ReadAddressValue(int nAddress, float &fValue)
|
{
|
if (!m_bConnected) return FALSE;
|
if (nAddress<0) return FALSE;
|
|
if (PPmacDprGetDPRMem(m_uDeviceID, (long)nAddress*4, sizeof(float), PVOID(&fValue))!=PS_Ok)
|
{
|
return FALSE;
|
}
|
|
// swap
|
SwapBytes(sizeof(float), &fValue, 1);
|
|
return TRUE;
|
}
|
|
BOOL CMotorControl_PowerPmac::WriteAddressValue(int nAddress, float fValue)
|
{
|
if (!m_bConnected) return FALSE;
|
if (nAddress<0) return FALSE;
|
|
// swap
|
SwapBytes(sizeof(float), &fValue, 1);
|
|
if (PPmacDprSetDPRMem(m_uDeviceID, (long)nAddress*4, sizeof(float), PVOID(&fValue))!=PS_Ok)
|
{
|
return FALSE;
|
}
|
|
return TRUE;
|
}
|
|
|
BOOL CMotorControl_PowerPmac::ReadAddressValue(long nAddress, long *pArrayData, int nArrayCount)
|
{
|
if (!m_bConnected) return FALSE;
|
if (nAddress<0) return FALSE;
|
|
if (PPmacDprGetDPRMem(m_uDeviceID, (long)nAddress*4, sizeof(long)*nArrayCount, PVOID(pArrayData))!=PS_Ok)
|
{
|
return FALSE;
|
}
|
|
// swap
|
SwapBytes(sizeof(long), pArrayData, nArrayCount);
|
|
return TRUE;
|
}
|
|
BOOL CMotorControl_PowerPmac::WriteAddressValue(long nAddress, long *pArrayData, int nArrayCount)
|
{
|
if (!m_bConnected) return FALSE;
|
if (nAddress<0) return FALSE;
|
|
// swap
|
SwapBytes(sizeof(long), pArrayData, nArrayCount);
|
|
if (PPmacDprSetDPRMem(m_uDeviceID, (long)nAddress*4, sizeof(long)*nArrayCount, PVOID(pArrayData))!=PS_Ok)
|
{
|
return FALSE;
|
}
|
|
return TRUE;
|
}
|
|
BOOL CMotorControl_PowerPmac::ReadAddressValue(long nAddress, float *pArrayData, int nArrayCount)
|
{
|
if (!m_bConnected) return FALSE;
|
if (nAddress<0) return FALSE;
|
|
if (PPmacDprGetDPRMem(m_uDeviceID, (long)nAddress*4, sizeof(float)*nArrayCount, PVOID(pArrayData))!=PS_Ok)
|
{
|
return FALSE;
|
}
|
|
// swap
|
SwapBytes(sizeof(float), pArrayData, nArrayCount);
|
|
return TRUE;
|
}
|
|
BOOL CMotorControl_PowerPmac::WriteAddressValue(long nAddress, float *pArrayData, int nArrayCount)
|
{
|
if (!m_bConnected) return FALSE;
|
if (nAddress<0) return FALSE;
|
|
// swap
|
SwapBytes(sizeof(float), pArrayData, nArrayCount);
|
|
if (PPmacDprSetDPRMem(m_uDeviceID, (long)nAddress*4, sizeof(float)*nArrayCount, PVOID(pArrayData))!=PS_Ok)
|
{
|
return FALSE;
|
}
|
|
return TRUE;
|
}
|
|
int CMotorControl_PowerPmac::GetStatus( int& nStatusCode, CString& strStatusMessage )
|
{
|
if (PPmacDprIsConnected(m_uDeviceID)!=PS_Ok) // Å×½ºÆ® ÇÊ¿ä
|
{
|
nStatusCode = MotorStatus_NotConnected;
|
strStatusMessage = _T("Not_Connected");
|
return 0;
|
}
|
|
nStatusCode = MotorStatus_Connected;
|
strStatusMessage = _T("Connected");
|
|
return 1;
|
}
|
