#include "StdAfx.h"
|
#include "CHMotorControls/MotorControl_Umac.h"
|
|
#include "Runtime.h"
|
|
CMotorControl_Umac::CMotorControl_Umac(int nIndex, DWORD dwPeriod, int nThreadCount) : CMotorControl(nIndex, dwPeriod, nThreadCount)
|
{
|
m_uDeviceID = 0;
|
}
|
|
CMotorControl_Umac::~CMotorControl_Umac(void)
|
{
|
//AfxMessageBox(_T("CMotorControl_Umac"));
|
|
Disconnect();
|
}
|
|
|
// CMotorControl
|
int CMotorControl_Umac::Connect(const CMotorControlInfo* pControlInfo)
|
{
|
if (pControlInfo==NULL) return 0;
|
|
// set motor control info
|
m_ControlInfo = *pControlInfo;
|
|
OpenRuntimeLink();
|
|
m_uDeviceID = _ttoi(m_ControlInfo.GetConnectionPort());
|
|
// Power Umac Open
|
m_bConnected = DeviceOpen(m_uDeviceID);
|
|
// 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_Umac::Disconnect()
|
{
|
if (m_bConnected==FALSE) return;
|
|
DeviceClose(m_uDeviceID);
|
|
m_bConnected = FALSE;
|
}
|
|
BOOL CMotorControl_Umac::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)
|
{
|
if (WriteAddressValue(nGoPosAddr, vectorPos[i])==FALSE)
|
{
|
continue;
|
}
|
}
|
}
|
nBitData = nBitData << 1;
|
}
|
Sleep(700);
|
BOOL bResult = WriteAddressValue(nManualAddr, (long)5); // 3d review x=axis0, y=axis2 (1+4=5)
|
|
return bResult;
|
}
|
|
BOOL CMotorControl_Umac::ReadAddressValue(int nAddress, long &nValue)
|
{
|
if (!m_bConnected) return FALSE;
|
|
INT nRet;
|
TCHAR szResponse[255];
|
CString strBuffer=_T(""), strResponse=_T("");
|
|
strBuffer.Format(_T("P%d"), nAddress);
|
nRet = DeviceGetResponseEx(m_uDeviceID, szResponse, 255, CT2A(strBuffer));
|
|
strResponse = CA2T(szResponse);
|
nValue = _ttoi(strResponse);
|
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Umac::WriteAddressValue(int nAddress, long nValue)
|
{
|
if (!m_bConnected) return FALSE;
|
|
INT nRet;
|
CHAR szResponse[255];
|
CString strBuffer=_T(""), strResponse=_T("");
|
|
strBuffer.Format(_T("P%d=%d"), nAddress, nValue);
|
nRet = DeviceGetResponseEx(m_uDeviceID, szResponse, 255, CT2A(strBuffer));
|
|
strResponse = CA2T(szResponse);
|
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Umac::ReadAddressValue(int nAddress, float &fValue)
|
{
|
if (!m_bConnected) return FALSE;
|
|
INT nRet;
|
CHAR szResponse[255];
|
CString strBuffer=_T(""), strResponse=_T("");
|
|
strBuffer.Format(_T("P%d"), nAddress);
|
nRet = DeviceGetResponseEx(m_uDeviceID, szResponse, 255, CT2A(strBuffer));
|
|
strResponse = CA2T(szResponse);
|
fValue = float(_ttof(strResponse));
|
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Umac::WriteAddressValue(int nAddress, float fValue)
|
{
|
if (!m_bConnected) return FALSE;
|
|
INT nRet;
|
CHAR szResponse[255];
|
CString strBuffer=_T(""), strResponse=_T("");
|
|
strBuffer.Format(_T("P%d=%f"), nAddress, fValue);
|
nRet = DeviceGetResponseEx(m_uDeviceID, szResponse, 255, CT2A(strBuffer));
|
|
strResponse = CA2T(szResponse);
|
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Umac::ReadAddressValue(int nAddress, double &dValue)
|
{
|
if (!m_bConnected) return FALSE;
|
|
INT nRet;
|
CHAR szResponse[255];
|
CString strBuffer=_T(""), strResponse=_T("");
|
|
strBuffer.Format(_T("P%d"), nAddress);
|
nRet = DeviceGetResponseEx(m_uDeviceID, szResponse, 255, CT2A(strBuffer));
|
|
strResponse = CA2T(szResponse);
|
dValue = _ttof(strResponse);
|
|
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Umac::WriteAddressValue(int nAddress, double dValue)
|
{
|
if (!m_bConnected) return FALSE;
|
|
INT nRet;
|
CHAR szResponse[255];
|
CString strBuffer=_T(""), strResponse=_T("");
|
|
strBuffer.Format(_T("P%d=%f"), nAddress, dValue);
|
nRet = DeviceGetResponseEx(m_uDeviceID, szResponse, 255, CT2A(strBuffer));
|
|
strResponse = CA2T(szResponse);
|
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Umac::ReadAxisPosition(float *pAxisPos, int nAxisCount)
|
{
|
BOOL bRet = FALSE;
|
|
// read x position
|
bRet = ReadAddressValue(m_pAxisPositionAddr[0], pAxisPos[0]);
|
|
bRet = bRet | ReadAddressValue(m_pAxisPositionAddr[1], pAxisPos[1]);
|
|
return bRet;
|
}
|
|
BOOL CMotorControl_Umac::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_Umac::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_Umac::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_Umac::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_Umac::GetStatus( int& nStatusCode, CString& strStatusMessage )
|
{
|
if (DeviceReadReady(m_uDeviceID)!=FALSE) // È®ÀÎ ÇÊ¿ä
|
{
|
nStatusCode = MotorStatus_NotConnected;
|
strStatusMessage = _T("Not_Connected");
|
return 0;
|
}
|
|
nStatusCode = MotorStatus_Connected;
|
strStatusMessage = _T("Connected");
|
|
return 1;
|
}
|
