#include "stdafx.h"
|
#include "CHMotorControls/MotorControl.h"
|
|
CMotorControl::CMotorControl(int nIndex, DWORD dwPeriod,int nThreadCount) :
|
m_nIndex(nIndex),
|
m_dwPeriod(dwPeriod),
|
CTimerThreadPools(dwPeriod, 1)
|
{
|
m_pMC2P = NULL;
|
m_pMotionThread = NULL;
|
m_ControlInfo.Reset();
|
m_bConnected = FALSE;
|
|
// var
|
m_nAxisCount = 0;
|
m_pAxisStatus = NULL;
|
m_pAxisPosition = NULL;
|
m_pAxisStatusPrev = NULL;
|
m_pAxisPositionPrev = NULL;
|
m_pAxisStatusAddr = NULL;
|
m_pAxisPositionAddr = NULL;
|
|
// review enable
|
m_nAutoEnable = 0;
|
m_nAutoEnablePrev = 0;
|
|
/*< LYW 20220704 - #4220 ADD Start >*/
|
m_nMotorAlive = 0;
|
m_nMotorAlivePrev = 0;
|
dwLastAliveTick = GetTickCount();
|
/*< LYW 20220704 - #4220 ADD End >*/
|
|
// wsi signal
|
m_nWsiMotionEndPrev = 0;
|
|
m_axisThetaStatus = 0;
|
m_axisThetaStatusPrev = 0;
|
m_axisThetaPosition = 0.f;
|
m_axisThetaPositionPrev = -999.0;
|
|
m_bUseWsi = FALSE;
|
|
m_pMotionThread = new CMotionThread(static_cast<IMotionThread2Parent*>(this), nThreadCount);
|
}
|
|
CMotorControl::~CMotorControl(void)
|
{
|
if (m_pMotionThread)
|
{
|
delete m_pMotionThread;
|
}
|
m_pMotionThread = NULL;
|
|
|
m_nAxisCount = 0;
|
|
if (m_pAxisStatus)
|
{
|
delete [] m_pAxisStatus;
|
m_pAxisStatus = NULL;
|
}
|
|
if (m_pAxisPosition)
|
{
|
delete [] m_pAxisPosition;
|
m_pAxisPosition = NULL;
|
}
|
|
if (m_pAxisStatusPrev)
|
{
|
delete [] m_pAxisStatusPrev;
|
m_pAxisStatusPrev = NULL;
|
}
|
|
if (m_pAxisPositionPrev)
|
{
|
delete [] m_pAxisPositionPrev;
|
m_pAxisPositionPrev = NULL;
|
}
|
|
if (m_pAxisStatusAddr)
|
{
|
delete [] m_pAxisStatusAddr;
|
m_pAxisStatusAddr = NULL;
|
}
|
|
if (m_pAxisPositionAddr)
|
{
|
delete [] m_pAxisPositionAddr;
|
m_pAxisPositionAddr = NULL;
|
}
|
|
m_ControlInfo.Reset();
|
|
}
|
|
void CMotorControl::TimerThreadProcess(PVOID pParameter)
|
{
|
if (!m_bConnected) return;
|
|
if (m_pMC2P==NULL) return;
|
|
if (m_nAxisCount<1) return;
|
|
//Alive
|
if (ReadAddressValue(50, m_nMotorAlive))
|
{
|
if (m_nMotorAlivePrev != m_nMotorAlive)
|
{
|
dwLastAliveTick = GetTickCount();
|
}
|
m_nAutoEnablePrev = m_nAutoEnable;
|
|
if (GetTickCount() - dwLastAliveTick >15000 && dwLastAliveTick != 0)
|
{
|
m_pMC2P->IMC2P_SendMotorAliveErrorAlram();
|
m_pMC2P->IMC2P_DisplayMessage(m_nIndex, _T("Motor Alive Error!"));
|
dwLastAliveTick = GetTickCount();
|
}
|
}
|
else
|
{
|
m_pMC2P->IMC2P_SendMotorAliveErrorAlram();
|
m_pMC2P->IMC2P_DisplayMessage(m_nIndex, _T("Motor Alive Read Fail!"));
|
dwLastAliveTick = GetTickCount();
|
}
|
|
// auto enable
|
const CMotorCommonAddr *pAddr = m_ControlInfo.GetMotorCommonAddr();
|
if (pAddr && pAddr->m_nAllAutoEnableAddr!=MOTOR_ADDRESS_NONE)
|
{
|
if (ReadAddressValue(pAddr->m_nAllAutoEnableAddr, m_nAutoEnable))
|
{
|
if (m_nAutoEnablePrev != m_nAutoEnable)
|
{
|
m_pMC2P->IMC2P_UpdateAutoEnable(m_nIndex, m_nAutoEnable);
|
}
|
m_nAutoEnablePrev = m_nAutoEnable;
|
}
|
}
|
|
// axis position+-3
|
static int nAxisMultiValue = 1000;
|
|
if (ReadAxisPosition(m_pAxisPosition, m_nAxisCount))
|
{
|
for (int i=0; i<m_nAxisCount; i++)
|
{
|
if (int(m_pAxisPositionPrev[i]*nAxisMultiValue)!=int(m_pAxisPosition[i]*nAxisMultiValue))
|
{
|
m_pMC2P->IMC2P_UpdatePosition(m_nIndex, i, m_pAxisPosition[i]);
|
}
|
m_pAxisPositionPrev[i] = m_pAxisPosition[i];
|
}
|
}
|
|
// axis status
|
if (ReadAxisStatus(m_pAxisStatus, m_nAxisCount))
|
{
|
for (int i=0; i<m_nAxisCount; i++)
|
{
|
if (m_pAxisStatusPrev[i]!=m_pAxisStatus[i])
|
{
|
m_pMC2P->IMC2P_UpdateStatus(m_nIndex, i, m_pAxisStatus[i]);
|
}
|
m_pAxisStatusPrev[i] = m_pAxisStatus[i];
|
}
|
}
|
|
// wsi motion end signal
|
if(GetUseWsi())
|
{
|
for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
int nAddr = GetGantryWsiMotionEndAddr(nG_Idx);
|
if (nAddr==MOTOR_ADDRESS_NONE) continue;
|
|
long nMotionEnd = 0;
|
|
if (ReadAddressValue(nAddr, nMotionEnd))
|
{
|
if (nG_Idx == 0)
|
{
|
if ((m_nWsiMotionEndPrev != nMotionEnd) && nMotionEnd == 1)
|
{
|
m_pMC2P->IMC2P_UpdateWsiMotionEnd(m_nIndex, nG_Idx);
|
|
}
|
m_nWsiMotionEndPrev = nMotionEnd;
|
}
|
else
|
{
|
if ((m_nWsiMotionEndPrev2 != nMotionEnd) && nMotionEnd == 1)
|
{
|
m_pMC2P->IMC2P_UpdateWsiMotionEnd(m_nIndex, nG_Idx);
|
|
}
|
m_nWsiMotionEndPrev2 = nMotionEnd;
|
}
|
|
}
|
}
|
}
|
|
// theta axis
|
if (m_ControlInfo.GetUseThetaMotor())
|
{
|
static int nThetaAxisMultiValue = 1000000;
|
|
// theta position
|
if (ReadThetaAxisPosition(m_axisThetaPosition))
|
{
|
if (int(m_axisThetaPositionPrev*nThetaAxisMultiValue)!=int(m_axisThetaPosition*nThetaAxisMultiValue))
|
{
|
m_pMC2P->IMC2P_UpdateThetaPosition(m_nIndex, m_axisThetaPosition);
|
}
|
m_axisThetaPositionPrev = m_axisThetaPosition;
|
}
|
|
// theta status
|
if (ReadThetaAxisStatus(m_axisThetaStatus))
|
{
|
if (m_axisThetaStatusPrev!=m_axisThetaStatus)
|
{
|
m_pMC2P->IMC2P_UpdateThetaStatus(m_nIndex, m_axisThetaStatus);
|
}
|
m_axisThetaStatusPrev = m_axisThetaStatus;
|
}
|
}
|
}
|
|
BOOL CMotorControl::ReadAxisPosition( float *pAxisPos, int nAxisCount )
|
{
|
return ReadAddressValue(m_pAxisPositionAddr[0], pAxisPos, nAxisCount);
|
}
|
|
BOOL CMotorControl::ReadAxisStatus( long *pAxisStatus, int nAxisCount )
|
{
|
return ReadAddressValue(m_pAxisStatusAddr[0], pAxisStatus, nAxisCount);
|
}
|
|
BOOL CMotorControl::ReadThetaAxisPosition( float &fAxisPos)
|
{
|
const CMotorThetaAxisAddr* pAddr = m_ControlInfo.GetThetaMotorAxisAddr();
|
if (pAddr==NULL) return FALSE;
|
return ReadAddressValue(pAddr->m_nPositionAddr, fAxisPos);
|
}
|
|
BOOL CMotorControl::ReadThetaAxisStatus( long &nAxisStatus )
|
{
|
const CMotorThetaAxisAddr* pAddr = m_ControlInfo.GetThetaMotorAxisAddr();
|
if (pAddr==NULL) return FALSE;
|
return ReadAddressValue(pAddr->m_nStatusAddr, nAxisStatus);
|
}
|
|
void CMotorControl::IMT2P_RunThreadProcess(const CMotionData& motionData)
|
{
|
switch(motionData.nType)
|
{
|
case ThreadDataType_AxisMotionEnd:
|
Process_AxisMotionEnd(motionData);
|
break;
|
|
case ThreadDataType_GantryMotionEnd:
|
Process_GantryMotionEnd(motionData);
|
break;
|
case ThreadDataType_ThetaMotionEnd:
|
Process_ThetaMotionEnd(motionData);
|
break;
|
}
|
}
|
|
BOOL CMotorControl::TransformIP_CStringToDWORD(const CString& strServerIP, DWORD& dwServerIP)
|
{
|
BOOL bReturn = TRUE;
|
int nStartIdx = 0, nFieldIdx = 0;
|
int nIPField0, nIPField1, nIPField2, nIPField3;
|
CString strToken, strIPField0, strIPField1, strIPField2, strIPField3;
|
|
if(FALSE == strServerIP.IsEmpty())
|
{
|
do
|
{
|
strToken = strServerIP.Tokenize(_T("."), nStartIdx);
|
|
switch(nFieldIdx)
|
{
|
case 0:
|
strIPField0 = strToken;
|
break;
|
case 1:
|
strIPField1 = strToken;
|
break;
|
case 2:
|
strIPField2 = strToken;
|
break;
|
case 3:
|
strIPField3 = strToken;
|
break;
|
default:
|
break;
|
}
|
|
nFieldIdx++;
|
} while (strToken != _T(""));
|
|
nIPField0 = _ttoi(strIPField0);
|
nIPField1 = _ttoi(strIPField1);
|
nIPField2 = _ttoi(strIPField2);
|
nIPField3 = _ttoi(strIPField3);
|
dwServerIP = (nIPField0 << 24) | (nIPField1 << 16) | (nIPField2 << 8) | (nIPField3);
|
}
|
else
|
{
|
bReturn = FALSE;
|
}
|
|
return bReturn;
|
}
|
|
|
|
|
void CMotorControl::Process_AxisMotionEnd(const CMotionData& motionData)
|
{
|
if (m_pMC2P==NULL) return;
|
|
// 0: delay time, 1: wait time
|
|
// delay time
|
::Sleep(motionData.uData[0].dwValue);
|
|
BOOL bResult = FALSE;
|
DWORD nStartTick = GetTickCount();
|
|
do
|
{
|
bResult = IsAxisMotionEnd(motionData.nIndex);
|
::Sleep(10);
|
|
// wait time
|
if ( (GetTickCount()-nStartTick) > motionData.uData[1].dwValue)
|
{
|
break;
|
}
|
} while (!bResult);
|
|
m_pMC2P->IMC2P_AxisMotionEnd(m_nIndex, motionData.nIndex, bResult);
|
|
return;
|
}
|
|
void CMotorControl::Process_GantryMotionEnd(const CMotionData& motionData)
|
{
|
if (m_pMC2P==NULL) return;
|
|
// 0: delay time, 1: wait time
|
|
// delay time
|
::Sleep(motionData.uData[0].dwValue);
|
|
BOOL bResult = FALSE;
|
DWORD nStartTick = GetTickCount();
|
|
do
|
{
|
bResult = IsGantryMotionEnd(motionData.nIndex);
|
::Sleep(10);
|
|
if ( (GetTickCount()-nStartTick) >motionData.uData[1].dwValue)
|
{
|
break;
|
}
|
} while (!bResult);
|
|
m_pMC2P->IMC2P_GantryMotionEnd(m_nIndex, motionData.nIndex, bResult);
|
|
return;
|
}
|
|
void CMotorControl::Process_ThetaMotionEnd(const CMotionData& motionData)
|
{
|
if (m_pMC2P==NULL) return;
|
|
// 0: delay time, 1: wait time
|
|
// delay time
|
::Sleep(motionData.uData[0].dwValue);
|
|
BOOL bResult = FALSE;
|
DWORD nStartTick = GetTickCount();
|
|
do
|
{
|
bResult = IsAxisThetaMotionEnd();
|
::Sleep(10);
|
|
// wait time
|
if ( (GetTickCount()-nStartTick) > motionData.uData[1].dwValue)
|
{
|
break;
|
}
|
} while (!bResult);
|
|
m_pMC2P->IMC2P_AxisMotionEnd(m_nIndex, motionData.nIndex, bResult);
|
|
return;
|
}
|
|
int CMotorControl::GetAxisGantryIndex(int nAxisIdx)
|
{
|
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;
|
|
if (pANode->m_nAxisIdx==nAxisIdx)
|
{
|
return pANode->m_nGantryIdx;
|
}
|
}
|
}
|
return -1;
|
}
|
|
int CMotorControl::GetAxisPositionAddr(int nAxisIdx)
|
{
|
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;
|
|
if (pANode->m_nAxisIdx==nAxisIdx)
|
{
|
return pANode->m_nPositionAddr;
|
}
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetAxisStatusAddr(int nAxisIdx)
|
{
|
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;
|
|
if (pANode->m_nAxisIdx==nAxisIdx)
|
{
|
return pANode->m_nStatusAddr;
|
}
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetAxisGoPositionAddr(int nAxisIdx)
|
{
|
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;
|
|
if (pANode->m_nAxisIdx==nAxisIdx)
|
{
|
return pANode->m_nMovePositionAddr;
|
}
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetAxisGoPositionCommonAddr( int nAxisIdx )
|
{
|
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;
|
|
if (pANode->m_nAxisIdx==nAxisIdx)
|
{
|
return pANode->m_nMovePositionCommonAddr;
|
}
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetAxisJogCommandAddr(int nAxisIdx)
|
{
|
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;
|
|
if (pANode->m_nAxisIdx==nAxisIdx)
|
{
|
return pANode->m_nJogCommandAddr;
|
}
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetAxisManualGoSpeedAddr(int nAxisIdx)
|
{
|
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;
|
|
if (pANode->m_nAxisIdx==nAxisIdx)
|
{
|
return pANode->m_nMoveSpeedAddr;
|
}
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetAxisManualGoAccelAddr(int nAxisIdx)
|
{
|
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;
|
|
if (pANode->m_nAxisIdx==nAxisIdx)
|
{
|
return pANode->m_nAccelTimeAddr;
|
}
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetAxisJogSpeedAddr(int nAxisIdx)
|
{
|
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;
|
|
if (pANode->m_nAxisIdx==nAxisIdx)
|
{
|
return pANode->m_nJogSpeedAddr;
|
}
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetAxisThetaPositionAddr()
|
{
|
if (m_ControlInfo.GetUseThetaMotor()==FALSE) return MOTOR_ADDRESS_NONE;
|
return m_ControlInfo.GetThetaMotorAxisAddr()->m_nPositionAddr;
|
}
|
|
int CMotorControl::GetAxisThetaStatusAddr()
|
{
|
if (m_ControlInfo.GetUseThetaMotor()==FALSE) return MOTOR_ADDRESS_NONE;
|
return m_ControlInfo.GetThetaMotorAxisAddr()->m_nStatusAddr;
|
}
|
int CMotorControl::GetAxisThetaGoPositionAddr()
|
{
|
if (m_ControlInfo.GetUseThetaMotor()==FALSE) return MOTOR_ADDRESS_NONE;
|
return m_ControlInfo.GetThetaMotorAxisAddr()->m_nMovePositionAddr;
|
}
|
|
int CMotorControl::GetAxisThetaJogCommandAddr()
|
{
|
if (m_ControlInfo.GetUseThetaMotor()==FALSE) return MOTOR_ADDRESS_NONE;
|
return m_ControlInfo.GetThetaMotorAxisAddr()->m_nJogCommandAddr;
|
}
|
|
int CMotorControl::GetAxisThetaManualGoAddr()
|
{
|
if (m_ControlInfo.GetUseThetaMotor()==FALSE) return MOTOR_ADDRESS_NONE;
|
return m_ControlInfo.GetThetaMotorAxisAddr()->m_nManualGoAddr;
|
}
|
|
int CMotorControl::GetAxisThetaManualGoAckAddr()
|
{
|
if (m_ControlInfo.GetUseThetaMotor()==FALSE) return MOTOR_ADDRESS_NONE;
|
return m_ControlInfo.GetThetaMotorAxisAddr()->m_nManualGoAckAddr;
|
}
|
|
int CMotorControl::GetAxisThetaManualGoSpeedAddr()
|
{
|
if (m_ControlInfo.GetUseThetaMotor()==FALSE) return MOTOR_ADDRESS_NONE;
|
return m_ControlInfo.GetThetaMotorAxisAddr()->m_nMoveSpeedAddr;
|
}
|
|
int CMotorControl::GetAxisThetaManualGoAccelAddr()
|
{
|
if (m_ControlInfo.GetUseThetaMotor()==FALSE) return MOTOR_ADDRESS_NONE;
|
return m_ControlInfo.GetThetaMotorAxisAddr()->m_nAccelTimeAddr;
|
}
|
|
int CMotorControl::GetAxisThetaJogSpeedAddr()
|
{
|
if (m_ControlInfo.GetUseThetaMotor()==FALSE) return MOTOR_ADDRESS_NONE;
|
return m_ControlInfo.GetThetaMotorAxisAddr()->m_nJogSpeedAddr;
|
}
|
|
int CMotorControl::GetGantryAxisCount(int nGantryIdx)
|
{
|
for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode==NULL) continue;
|
|
if (pGNode->m_nGantryIdx==nGantryIdx)
|
{
|
return pGNode->GetMotorAxisAddrCount();
|
}
|
}
|
return 0;
|
}
|
|
int CMotorControl::GetGantryManualGoAddr(int nGantryIdx)
|
{
|
for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode==NULL) continue;
|
|
if (pGNode->m_nGantryIdx==nGantryIdx)
|
{
|
return pGNode->m_nManualGoAddr;
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetGantryManualGoAckAddr( int nGantryIdx )
|
{
|
for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode==NULL) continue;
|
|
if (pGNode->m_nGantryIdx==nGantryIdx)
|
{
|
return pGNode->m_nManualGoAckAddr;
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetGantryCollisionPositionXAddr(int nGantryIdx)
|
{
|
for (int nG_Idx = 0; nG_Idx < m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode == NULL) continue;
|
|
if (pGNode->m_nGantryIdx == nGantryIdx)
|
{
|
return pGNode->m_nCollisionXAddr;
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetGantryCollisionPositionYAddr(int nGantryIdx)
|
{
|
for (int nG_Idx = 0; nG_Idx < m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode == NULL) continue;
|
|
if (pGNode->m_nGantryIdx == nGantryIdx)
|
{
|
return pGNode->m_nCollisionYAddr;
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetGantryAutoGoAddr(int nGantryIdx)
|
{
|
for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode==NULL) continue;
|
|
if (pGNode->m_nGantryIdx==nGantryIdx)
|
{
|
return pGNode->m_nAutoReviewGoAddr;
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetGantryAutoGoAckAddr( int nGantryIdx )
|
{
|
for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode==NULL) continue;
|
|
if (pGNode->m_nGantryIdx==nGantryIdx)
|
{
|
return pGNode->m_nAutoReviewGoAckAddr;
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetGantryMeasureAutoGoAddr(int nGantryIdx)
|
{
|
for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode==NULL) continue;
|
|
if (pGNode->m_nGantryIdx==nGantryIdx)
|
{
|
return pGNode->m_nAutoMeasureGoAddr;
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetGantryMeasureAutoGoAckAddr( int nGantryIdx )
|
{
|
for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode==NULL) continue;
|
|
if (pGNode->m_nGantryIdx==nGantryIdx)
|
{
|
return pGNode->m_nAutoMeasureGoAckAddr;
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetGantryWsiAutoGoAddr(int nGantryIdx)
|
{
|
for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode==NULL) continue;
|
|
if (pGNode->m_nGantryIdx==nGantryIdx)
|
{
|
return pGNode->m_nAutoWsiGoAddr;
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetGantryWsiAutoGoAckAddr( int nGantryIdx )
|
{
|
for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode==NULL) continue;
|
|
if (pGNode->m_nGantryIdx==nGantryIdx)
|
{
|
return pGNode->m_nAutoWsiGoAckAddr;
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetGantryMoveCountAddr(int nGantryIdx)
|
{
|
for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode==NULL) continue;
|
|
if (pGNode->m_nGantryIdx==nGantryIdx)
|
{
|
return pGNode->m_nReviewMoveCountAddr;
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetGantryMeasureMoveCountAddr(int nGantryIdx)
|
{
|
for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode==NULL) continue;
|
|
if (pGNode->m_nGantryIdx==nGantryIdx)
|
{
|
return pGNode->m_nMeasureMoveCountAddr;
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetGantryWsiMoveCountAddr(int nGantryIdx)
|
{
|
for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode==NULL) continue;
|
|
if (pGNode->m_nGantryIdx==nGantryIdx)
|
{
|
return pGNode->m_nWsiMoveCountAddr;
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetGantryTriggerCountAddr(int nGantryIdx)
|
{
|
for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode==NULL) continue;
|
|
if (pGNode->m_nGantryIdx==nGantryIdx)
|
{
|
return pGNode->m_nReviewTriggerCountAddr;
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetGantryMeasureTriggerCountAddr(int nGantryIdx)
|
{
|
for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode==NULL) continue;
|
|
if (pGNode->m_nGantryIdx==nGantryIdx)
|
{
|
return pGNode->m_nMeasureTriggerCountAddr;
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetGantryWsiTriggerCountAddr(int nGantryIdx)
|
{
|
for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode==NULL) continue;
|
|
if (pGNode->m_nGantryIdx==nGantryIdx)
|
{
|
return pGNode->m_nWsiTriggerCountAddr;
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetGantryWsiMotionEndAddr( int nGantryIdx )
|
{
|
for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode==NULL) continue;
|
|
if (pGNode->m_nGantryIdx==nGantryIdx)
|
{
|
//¸ðÅÍ ¸ð¼Ç END ÁÖ¼Ò °¡Á®¿À´Â ºÎºÐ
|
//return 332;//pGNode->m_nWsiMotionEndAddr;
|
if (nGantryIdx == 0)
|
{
|
return 452;//pGNode->m_nWsiMotionEndAddr;
|
}
|
else
|
{
|
return 453;
|
}
|
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetGantryWsiNextPosMoveAddr( int nGantryIdx )
|
{
|
for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode==NULL) continue;
|
|
if (pGNode->m_nGantryIdx==nGantryIdx)
|
{
|
if (nGantryIdx == 0)
|
{
|
//C-PJT
|
return 472;
|
}
|
else
|
{
|
return 473;
|
}
|
//return 352;//pGNode->m_nWsiNextPosMoveAddr;
|
}
|
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetSoftwareTriggerAddr( int nGantryIdx )
|
{
|
for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode==NULL) continue;
|
|
if (pGNode->m_nGantryIdx==nGantryIdx)
|
{
|
return pGNode->m_nSoftwareTriggerAddr;
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetSoftwareTriggerCmdAddr( int nGantryIdx )
|
{
|
for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode==NULL) continue;
|
|
if (pGNode->m_nGantryIdx==nGantryIdx)
|
{
|
return pGNode->m_nSoftwareTriggerCmdAdr;
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
int CMotorControl::GetImageSnapCompleteAddr( int nGantryIdx )
|
{
|
for (int nG_Idx=0; nG_Idx<m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode==NULL) continue;
|
|
if (pGNode->m_nGantryIdx==nGantryIdx)
|
{
|
return pGNode->m_nImageSnapCompleteAddr;
|
}
|
}
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
BOOL CMotorControl::GetAxisLimitPosition(int nAxisIdx, double& dPlusLimit, double& dMinusLimit)
|
{
|
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;
|
|
if (pANode->m_nAxisIdx==nAxisIdx)
|
{
|
dPlusLimit = pANode->m_dPlusLimitPos;
|
dMinusLimit = pANode->m_dMinusLimitPos;
|
return TRUE;
|
}
|
}
|
}
|
return FALSE;
|
}
|
|
|
// axis
|
BOOL CMotorControl::AxisManualGo(int nAxisIndex, double dPosition)
|
{
|
int nGantryIdx = GetAxisGantryIndex(nAxisIndex);
|
|
if (nGantryIdx<0) return FALSE;
|
|
UINT nManualGoAddr = GetGantryManualGoAddr(nGantryIdx);
|
if (nManualGoAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
UINT nGoPosAddr = GetAxisGoPositionAddr(nAxisIndex);
|
if (nGoPosAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
if (WriteAddressValue(nGoPosAddr, dPosition)==FALSE)
|
{
|
return FALSE;
|
}
|
|
int nAxisCount = 0;
|
for (int i=0; i<nGantryIdx; i++)
|
{
|
nAxisCount += GetGantryAxisCount(i);
|
}
|
|
// ÇØ´ç ºñÆ®¸¸ set
|
long nValue = 1 << (nAxisIndex-nAxisCount);
|
|
return WriteAddressValue(nManualGoAddr, nValue);
|
}
|
|
|
BOOL CMotorControl::AxisManualGoEnd(int nAxisIdx, double dPosition)
|
{
|
if (AxisManualGo(nAxisIdx, dPosition)==FALSE) return FALSE;
|
|
if (m_pMotionThread==NULL) return FALSE;
|
|
CMotionData motionData;
|
motionData.nType = ThreadDataType_AxisMotionEnd;
|
motionData.nIndex = nAxisIdx;
|
motionData.uData[0].dwValue = 500; // delay time
|
motionData.uData[1].dwValue = 5000; // wait time
|
|
return m_pMotionThread->AddThreadData(motionData);
|
}
|
|
BOOL CMotorControl::AxisManualGo(const VectorInteger& vectorAxis, const VectorDouble& vectorPosition)
|
{
|
if (vectorAxis.size()<1 || vectorPosition.size()<1) return FALSE;
|
|
if (vectorAxis.size() != vectorPosition.size()) return FALSE;
|
|
// ¸ðµç ÃàÀÇ °ÕÆ®¸® À妽º°¡ °°Àº°¡ È®ÀÎ
|
int nGantryIdx = GetAxisGantryIndex(vectorAxis[0]);
|
if (nGantryIdx<0) return FALSE;
|
for (int i=1; i<(int)vectorAxis.size(); i++)
|
{
|
if (nGantryIdx!=GetAxisGantryIndex(vectorAxis[i]))
|
{
|
return FALSE;
|
}
|
}
|
|
// °ÕÆ®¸® ¼öµ¿À̵¿ ÁÖ¼Ò È®ÀÎ
|
UINT nManualGoAddr = GetGantryManualGoAddr(nGantryIdx);
|
if (nManualGoAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
// Ãະ ¼öµ¿À̵¿ ÁÂÇ¥ ¾²±â
|
for (int i=0; i<(int)vectorAxis.size(); i++)
|
{
|
UINT nGoPosAddr = GetAxisGoPositionAddr(vectorAxis[i]);
|
if (nGoPosAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
if (WriteAddressValue(nGoPosAddr, vectorPosition[i])==FALSE)
|
{
|
return FALSE;
|
}
|
}
|
|
int nAxisCount = 0;
|
for (int i=0; i<nGantryIdx; i++)
|
{
|
nAxisCount += GetGantryAxisCount(i);
|
}
|
|
// ÇØ´ç ÃàÀÇ ºñÆ®¸¸ set
|
long nValue = 0;
|
for (int i=0; i<(int)vectorAxis.size(); i++)
|
{
|
long nTemp = 1 << (vectorAxis[i]-nAxisCount);
|
nValue = nValue | nTemp;
|
}
|
|
return WriteAddressValue(nManualGoAddr, nValue);
|
}
|
|
BOOL CMotorControl::AxisJogPlus(int nAxisIdx)
|
{
|
int nJogAddr = (int)GetAxisJogCommandAddr(nAxisIdx);
|
if (nJogAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(nJogAddr, (long)1);
|
}
|
|
BOOL CMotorControl::AxisJogMinus(int nAxisIdx)
|
{
|
int nJogAddr = (int)GetAxisJogCommandAddr(nAxisIdx);
|
if (nJogAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(nJogAddr, (long)2);
|
}
|
|
BOOL CMotorControl::AxisJogStop(int nAxisIdx)
|
{
|
int nJogAddr = (int)GetAxisJogCommandAddr(nAxisIdx);
|
if (nJogAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(nJogAddr, (long)0);
|
}
|
|
BOOL CMotorControl::AxisManualGoSpeed(int nAxisIdx, double dSpeed)
|
{
|
int nAddr = GetAxisManualGoSpeedAddr(nAxisIdx);
|
|
if (nAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(nAddr, dSpeed);
|
}
|
|
BOOL CMotorControl::AxisJogSpeed(int nAxisIdx, double dSpeed)
|
{
|
int nAddr = GetAxisJogSpeedAddr(nAxisIdx);
|
|
if (nAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(nAddr, dSpeed);
|
}
|
|
BOOL CMotorControl::AxisManualGoAccel(int nAxisIdx, int nAccel)
|
{
|
int nAddr = GetAxisManualGoAccelAddr(nAxisIdx);
|
|
if (nAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(nAddr, (long)nAccel);
|
}
|
|
BOOL CMotorControl::AxisJogStop()
|
{
|
int nResultCode = 1;
|
for (int nAxisIdx=0; nAxisIdx<m_nAxisCount; nAxisIdx++)
|
{
|
nResultCode = nResultCode & AxisJogStop(nAxisIdx);
|
}
|
return nResultCode;
|
}
|
|
BOOL CMotorControl::AxisManualGoSpeed(double dSpeed)
|
{
|
int nResultCode = 1;
|
for (int nAxisIdx=0; nAxisIdx<m_nAxisCount; nAxisIdx++)
|
{
|
nResultCode = nResultCode & AxisManualGoSpeed(nAxisIdx, dSpeed);
|
}
|
return nResultCode;
|
}
|
|
BOOL CMotorControl::AxisJogSpeed(double dSpeed)
|
{
|
int nResultCode = 1;
|
for (int nAxisIdx=0; nAxisIdx<m_nAxisCount; nAxisIdx++)
|
{
|
nResultCode = nResultCode & AxisJogSpeed(nAxisIdx, dSpeed);
|
}
|
return nResultCode;
|
}
|
|
BOOL CMotorControl::AxisManualGoAccel(int nAccel)
|
{
|
int nResultCode = 1;
|
for (int nAxisIdx=0; nAxisIdx<m_nAxisCount; nAxisIdx++)
|
{
|
nResultCode = nResultCode & AxisManualGoAccel(nAxisIdx, (long)nAccel);
|
}
|
return nResultCode;
|
}
|
|
BOOL CMotorControl::IsAxisMotionEnd()
|
{
|
int nResultCode = 1;
|
for (int nAxisIdx=0; nAxisIdx<m_nAxisCount; nAxisIdx++)
|
{
|
nResultCode = nResultCode && IsAxisMotionEnd(nAxisIdx);
|
}
|
return nResultCode;
|
}
|
|
// axis theta
|
BOOL CMotorControl::AxisThetaManualGo(double dPosition)
|
{
|
if (m_ControlInfo.GetUseThetaMotor()!=1) return FALSE;
|
|
UINT nManualGoAddr = GetAxisThetaManualGoAddr();
|
if (nManualGoAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
UINT nGoPosAddr = GetAxisThetaGoPositionAddr();
|
if (nGoPosAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
if (WriteAddressValue(nGoPosAddr, dPosition)==FALSE)
|
{
|
return FALSE;
|
}
|
return WriteAddressValue(nManualGoAddr, (long)1);
|
}
|
|
|
BOOL CMotorControl::AxisThetaManualGoAck( long &lAck )
|
{
|
if (m_ControlInfo.GetUseThetaMotor()!=1) return FALSE;
|
|
UINT nManualGoAckAddr = GetAxisThetaManualGoAckAddr();
|
if (nManualGoAckAddr == MOTOR_ADDRESS_NONE) return FALSE;
|
|
return ReadAddressValue(nManualGoAckAddr, lAck);
|
}
|
|
BOOL CMotorControl::AxisThetaManualGoAck( long &lAck, DWORD nWaitTime )
|
{
|
// delay time
|
int nDelayTime = 10;
|
|
BOOL bResult = FALSE;
|
DWORD nStartTick = GetTickCount();
|
|
do
|
{
|
::Sleep(nDelayTime);
|
AxisThetaManualGoAck(lAck);
|
|
// Loop Stop Á¶°Ç. ( Ack °ª¿¡ µû¶ó¼... )
|
if (lAck == TRUE)
|
bResult = TRUE;
|
|
// wait time
|
if ( (GetTickCount()-nStartTick) > nWaitTime)
|
{
|
break;
|
}
|
} while (!bResult);
|
|
return TRUE;
|
}
|
|
BOOL CMotorControl::AxisThetaManualGoEnd(double dPosition)
|
{
|
if (AxisThetaManualGo(dPosition)==FALSE) return FALSE;
|
|
if (m_pMotionThread==NULL) return FALSE;
|
|
CMotionData motionData;
|
motionData.nType = ThreadDataType_ThetaMotionEnd;
|
motionData.nIndex = THETAMOTOR_AXIS_INDEX;
|
motionData.uData[0].dwValue = 500; // delay time
|
motionData.uData[1].dwValue = 5000; // wait time
|
|
return m_pMotionThread->AddThreadData(motionData);
|
}
|
|
BOOL CMotorControl::AxisThetaJogPlus()
|
{
|
int nJogAddr = (int)GetAxisThetaJogCommandAddr();
|
if (nJogAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(nJogAddr, (long)1);
|
}
|
|
BOOL CMotorControl::AxisThetaJogMinus()
|
{
|
int nJogAddr = (int)GetAxisThetaJogCommandAddr();
|
if (nJogAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(nJogAddr, (long)2);
|
}
|
|
BOOL CMotorControl::AxisThetaJogStop()
|
{
|
int nJogAddr = (int)GetAxisThetaJogCommandAddr();
|
if (nJogAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(nJogAddr, (long)0);
|
}
|
|
BOOL CMotorControl::AxisThetaManualGoSpeed(double dSpeed)
|
{
|
int nAddr = GetAxisThetaManualGoSpeedAddr();
|
|
if (nAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(nAddr, dSpeed);
|
}
|
|
BOOL CMotorControl::AxisThetaJogSpeed(double dSpeed)
|
{
|
int nAddr = GetAxisThetaJogSpeedAddr();
|
|
if (nAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(nAddr, dSpeed);
|
}
|
|
BOOL CMotorControl::AxisThetaManualGoAccel(int nAccel)
|
{
|
int nAddr = GetAxisThetaManualGoAccelAddr();
|
|
if (nAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(nAddr, (long)nAccel);
|
}
|
|
// gantry
|
BOOL CMotorControl::GantryManualGo(int nGantryIdx, const VectorDouble& vectorAxisPos)
|
{
|
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)vectorAxisPos.size(); i++)
|
{
|
UINT nGoPosAddr = GetAxisGoPositionAddr(nAxisCount+i);
|
if (nGoPosAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
if (WriteAddressValue(nGoPosAddr, vectorAxisPos[i])==FALSE)
|
{
|
return FALSE;
|
}
|
|
nValue = nValue | (1 << i);
|
}
|
|
return WriteAddressValue(nManualAddr, nValue);
|
}
|
|
BOOL CMotorControl::GantryManualGo(int nGantryIdx, const VectorDouble& vectorAxisPos, 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, vectorAxisPos[i])==FALSE)
|
{
|
continue;
|
}
|
}
|
}
|
nBitData = nBitData << 1;
|
}
|
Sleep(700);
|
BOOL bResult = WriteAddressValue(nManualAddr, (long) nMoveType);
|
|
//return WriteAddressValue(nManualAddr, (long) nMoveType);
|
return bResult;
|
}
|
|
BOOL CMotorControl::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);
|
//UINT nYGoPosAddr = GetAxisGoPositionAddr(nAxisCount+2); // lmk motor y2
|
if (nYGoPosAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
if (WriteAddressValue(nXGoPosAddr, dPosX)==FALSE)
|
{
|
return FALSE;
|
}
|
|
if (WriteAddressValue(nYGoPosAddr, dPosY)==FALSE)
|
{
|
return FALSE;
|
}
|
|
return WriteAddressValue(nManualAddr, (long)3);
|
//return WriteAddressValue(nManualAddr, (long)5); // lmk motor y2
|
}
|
|
BOOL CMotorControl::GantryManualGoAck( int nGantryIdx , long &lAck)
|
{
|
UINT nGantryManualGoAckAddr = GetGantryManualGoAckAddr(nGantryIdx);
|
if (nGantryManualGoAckAddr == MOTOR_ADDRESS_NONE) return FALSE;
|
|
return ReadAddressValue(nGantryManualGoAckAddr,lAck );
|
}
|
|
BOOL CMotorControl::GantryManualGoAck( int nGantryIdx, long &lAck, DWORD nWaitTime )
|
{
|
// delay time
|
int nDelayTime = 10;
|
|
BOOL bResult = FALSE;
|
DWORD nStartTick = GetTickCount();
|
|
do
|
{
|
::Sleep(nDelayTime);
|
GantryManualGoAck( nGantryIdx , lAck);
|
|
// Loop Stop Á¶°Ç. ( Ack °ª¿¡ µû¶ó¼... )
|
if (lAck == TRUE)
|
bResult = TRUE;
|
|
// wait time
|
if ( (GetTickCount()-nStartTick) > nWaitTime)
|
{
|
break;
|
}
|
} while (!bResult);
|
|
return TRUE;
|
}
|
|
|
BOOL CMotorControl::GantryManualGoEnd(int nGantryIdx, const VectorDouble& vectorPos)
|
{
|
if (GantryManualGo(nGantryIdx, vectorPos)==FALSE) return FALSE;
|
|
if (m_pMotionThread==NULL) return FALSE;
|
|
CMotionData motionData;
|
motionData.nType = ThreadDataType_GantryMotionEnd;
|
motionData.nIndex = nGantryIdx;
|
motionData.uData[0].dwValue = 500; // delay time
|
motionData.uData[1].dwValue = 5000; // wait time
|
|
return m_pMotionThread->AddThreadData(motionData);
|
}
|
|
BOOL CMotorControl::GantryManualGoEnd(int nGantryIdx, const VectorDouble& vectorPos, int nMoveType)
|
{
|
if (GantryManualGo(nGantryIdx, vectorPos, nMoveType)==FALSE) return FALSE;
|
|
if (m_pMotionThread==NULL) return FALSE;
|
|
CMotionData motionData;
|
motionData.nType = ThreadDataType_GantryMotionEnd;
|
motionData.nIndex = nGantryIdx;
|
motionData.uData[0].dwValue = 500; // delay time
|
motionData.uData[1].dwValue = 5000; // wait time
|
|
return m_pMotionThread->AddThreadData(motionData);
|
}
|
|
BOOL CMotorControl::GantryManualGoEnd(int nGantryIdx, double dPosX, double dPosY)
|
{
|
if (GantryManualGo(nGantryIdx, dPosX, dPosY)==FALSE) return FALSE;
|
|
if (m_pMotionThread==NULL) return FALSE;
|
|
CMotionData motionData;
|
motionData.nType = ThreadDataType_GantryMotionEnd;
|
motionData.nIndex = nGantryIdx;
|
motionData.uData[0].dwValue = 500; // delay time
|
motionData.uData[1].dwValue = 5000; // wait time
|
|
return m_pMotionThread->AddThreadData(motionData);
|
}
|
|
BOOL CMotorControl::GantrySetCollisionPosition(int nGantryIdx, double dPosX, double dPosY,int nFirstIn)
|
{
|
BOOL bRet = FALSE;
|
|
int nAddr = (int)GetGantryCollisionPositionXAddr(nGantryIdx);
|
if (nAddr != MOTOR_ADDRESS_NONE)
|
bRet = WriteAddressValue(nAddr, dPosX);
|
|
int nAddr2 = 400;
|
if (nAddr2 != MOTOR_ADDRESS_NONE)
|
bRet = WriteAddressValue(nAddr2,(long)nFirstIn);
|
|
|
//nAddr = (int)GetGantryCollisionPositionYAddr(nGantryIdx);
|
//if (nAddr != MOTOR_ADDRESS_NONE)
|
// bRet |= WriteAddressValue(nAddr, dPosY);
|
|
return bRet;
|
}
|
|
const int CMotorControl::GetGantryCollisionXAddr(int nGantryIdx)
|
{
|
for (int nG_Idx = 0; nG_Idx < m_ControlInfo.GetMotorGantryAddrCount(); nG_Idx++)
|
{
|
CMotorGantryAddr *pGNode = m_ControlInfo.GetMotorGantryAddr(nG_Idx);
|
if (pGNode == NULL)
|
continue;
|
|
if (pGNode->m_nGantryIdx == nGantryIdx)
|
return pGNode->m_nCollisionXAddr;
|
}
|
|
return MOTOR_ADDRESS_NONE;
|
}
|
|
BOOL CMotorControl::GantrySetTwoGantrySyncModeSend(bool bUse)
|
{
|
BOOL bRet = FALSE;
|
//WSI ÁøÇà½Ã Á¿ìÁøÇàÈÄ ´ÙÀ½ Æ÷ÀÎÆ® ÁøÇà 1-> ON
|
int nAddr3 = 410;
|
if (nAddr3 != MOTOR_ADDRESS_NONE)
|
bRet = WriteAddressValue(nAddr3, (long)bUse);
|
return bRet;
|
}
|
|
BOOL CMotorControl::IsAxisMotionEnd(int nAxisIdx)
|
{
|
// axis
|
if (nAxisIdx<0 || nAxisIdx>=m_nAxisCount) return TRUE;
|
|
if( (m_pAxisStatus[nAxisIdx] & 0x01) == 0)
|
{
|
return FALSE;
|
}
|
return TRUE;
|
}
|
|
BOOL CMotorControl::IsAxisThetaMotionEnd()
|
{
|
if (m_ControlInfo.GetUseThetaMotor()==FALSE) return TRUE;
|
|
// axis theta
|
if( (m_axisThetaStatus & 0x01) == 0)
|
{
|
return FALSE;
|
}
|
|
return TRUE;
|
}
|
|
|
BOOL CMotorControl::IsGantryMotionEnd(int nGantryIdx)
|
{
|
int nAxisStart = 0;
|
for (int i=0; i<nGantryIdx; i++)
|
{
|
nAxisStart += GetGantryAxisCount(i);
|
}
|
|
int nAxisEnd = nAxisStart + GetGantryAxisCount(nGantryIdx);
|
|
for (int i=nAxisStart; i<nAxisEnd; i++)
|
{
|
if( (m_pAxisStatus[i] & 0x01) == 0)
|
{
|
return FALSE;
|
}
|
}
|
|
return TRUE;
|
}
|
|
|
BOOL CMotorControl::GantryAutoGo(int nGantryIdx, const VectorDouble& vectorPosX, const VectorDouble& vectorPosY, BOOL bCommand)
|
{
|
double dPlusLimitX, dMinusLimitX, dPlusLimitY, dMinusLimitY;
|
int nPointCount = (int)vectorPosX.size();
|
|
if (nPointCount > m_ControlInfo.GetMaxAxisMoveCount()) return FALSE;
|
|
if (nPointCount<1 || vectorPosY.size()<1) return FALSE;
|
|
if (nPointCount!=(int)vectorPosY.size()) return FALSE;
|
|
int nStartAxisIdx = 0;
|
for (int i=0; i<nGantryIdx; i++)
|
{
|
nStartAxisIdx += GetGantryAxisCount(i);
|
}
|
|
int nGoPosAddrX = GetAxisGoPositionAddr(nStartAxisIdx);
|
if (nGoPosAddrX==MOTOR_ADDRESS_NONE) return FALSE;
|
|
int nGoPosAddrY = GetAxisGoPositionAddr(nStartAxisIdx+1);
|
if (nGoPosAddrY==MOTOR_ADDRESS_NONE) return FALSE;
|
|
GetAxisLimitPosition(nStartAxisIdx, dPlusLimitX, dMinusLimitX);
|
GetAxisLimitPosition(nStartAxisIdx+1, dPlusLimitY, dMinusLimitY);
|
|
BOOL bSuccess = TRUE;
|
int nMaxMotorMoveCount = m_ControlInfo.GetMaxAxisMoveCount();
|
float *fXPos=NULL, *fYPos=NULL;
|
fXPos = new float[nMaxMotorMoveCount];
|
fYPos = new float[nMaxMotorMoveCount];
|
ZeroMemory(fXPos, sizeof(float)*nMaxMotorMoveCount);
|
ZeroMemory(fYPos, sizeof(float)*nMaxMotorMoveCount);
|
|
for (int i=0; i<nPointCount; i++)
|
{
|
if(vectorPosX[i] > dMinusLimitX && vectorPosX[i] < dPlusLimitX)
|
{
|
fXPos[i] = float(vectorPosX[i]);
|
}
|
else
|
{
|
fXPos[i] = 0.0f;
|
if (m_pMC2P)
|
{
|
m_pMC2P->IMC2P_DisplayMessage(m_nIndex, _T("X MotorPosition Idx[%d] Position[%.03lf] Limit"), i, vectorPosX[i]);
|
}
|
}
|
|
if(vectorPosY[i] > dMinusLimitY && vectorPosY[i] < dPlusLimitY)
|
{
|
fYPos[i] = float(vectorPosY[i]);
|
}
|
else
|
{
|
fYPos[i] = 0.0f;
|
if (m_pMC2P)
|
{
|
m_pMC2P->IMC2P_DisplayMessage(m_nIndex, _T("Y MotorPosition Idx[%d] Position[%.03lf] Limit"), i, vectorPosY[i]);
|
}
|
}
|
}
|
|
// write x pos
|
if (WriteAddressValue(nGoPosAddrX, fXPos, nPointCount) == FALSE)
|
{
|
delete fXPos;
|
delete fYPos;
|
|
return FALSE;
|
}
|
|
// write y pos
|
if (WriteAddressValue(nGoPosAddrY, fYPos, nPointCount)==FALSE)
|
{
|
delete fXPos;
|
delete fYPos;
|
|
return FALSE;
|
}
|
|
delete fXPos;
|
delete fYPos;
|
|
// count
|
int nMoveCountAddr = (int)GetGantryMoveCountAddr(nGantryIdx);
|
if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
if (WriteAddressValue(nMoveCountAddr, (long)nPointCount)==FALSE)
|
{
|
return FALSE;
|
}
|
|
if (bCommand==FALSE)
|
{
|
return TRUE;
|
}
|
|
// command
|
int nMoveCmdAddr = (int)GetGantryAutoGoAddr(nGantryIdx);
|
|
return WriteAddressValue(nMoveCmdAddr, (long)1);
|
}
|
|
BOOL CMotorControl::GantryAutoGo(int nGantryIdx)
|
{
|
int nGoCmdAddr = (int)GetGantryAutoGoAddr(nGantryIdx);
|
|
if (nGoCmdAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(nGoCmdAddr, (long)1);
|
}
|
|
BOOL CMotorControl::GantryAutoGoAck( int nGantryIdx, long& lAck )
|
{
|
int nGantryReviewAutoGoAck = GetGantryManualGoAckAddr(nGantryIdx);
|
if (nGantryReviewAutoGoAck == FALSE) return FALSE;
|
|
return ReadAddressValue(nGantryReviewAutoGoAck,lAck);
|
}
|
|
BOOL CMotorControl::GantryAutoGoAck( int nGantryIdx, long& lAck, DWORD nWaitTime )
|
{
|
// delay time
|
int nDelayTime = 10;
|
|
BOOL bResult = FALSE;
|
DWORD nStartTick = GetTickCount();
|
|
do
|
{
|
::Sleep(nDelayTime);
|
GantryAutoGoAck( nGantryIdx, lAck );
|
|
// Loop Stop Á¶°Ç. ( Ack °ª¿¡ µû¶ó¼... )
|
if (lAck == TRUE)
|
bResult = TRUE;
|
|
// wait time
|
if ( (GetTickCount()-nStartTick) > nWaitTime)
|
{
|
break;
|
}
|
} while (!bResult);
|
|
return TRUE;
|
}
|
|
BOOL CMotorControl::GantrySetMoveCount(int nGantryIdx, long nCount)
|
{
|
UINT nMoveCountAddr = GetGantryMoveCountAddr(nGantryIdx);
|
if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(nMoveCountAddr, nCount);
|
}
|
|
BOOL CMotorControl::GantryGetMoveCount(int nGantryIdx, long& nCount)
|
{
|
UINT nCountAddr = GetGantryMoveCountAddr(nGantryIdx);
|
if (nCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return ReadAddressValue(nCountAddr, nCount);
|
}
|
|
BOOL CMotorControl::GantryGetTriggerCount(int nGantryIdx, long& nCount)
|
{
|
UINT nCountAddr = GetGantryTriggerCountAddr(nGantryIdx);
|
if (nCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return ReadAddressValue(nCountAddr, nCount);
|
}
|
|
BOOL CMotorControl::GantryMeasureAutoGo(int nGantryIdx, const VectorDouble& vectorPosData, BOOL bCommand)
|
{
|
int nGantryAxisCount = GetGantryAxisCount(nGantryIdx);
|
|
int nTotalPointCount = (int)vectorPosData.size();
|
|
if ( (nTotalPointCount%nGantryAxisCount)!=0 ) return FALSE;
|
|
int nPointCount = nTotalPointCount / nGantryAxisCount;
|
|
if (nPointCount > m_ControlInfo.GetMaxAxisMoveCount()) return FALSE;
|
|
int nStartAxisIdx = 0;
|
for (int i=0; i<nGantryIdx; i++)
|
{
|
nStartAxisIdx += GetGantryAxisCount(i);
|
}
|
|
BOOL bSuccess = TRUE;
|
double dPosData = 0.0;
|
double dPlusLimit, dMinusLimit;
|
int nMaxMotorMoveCount = m_ControlInfo.GetMaxAxisMoveCount();
|
float *fPosData = new float[nMaxMotorMoveCount];
|
memset(fPosData, 0, sizeof(float)*nMaxMotorMoveCount);
|
|
for (int nAxisIdx=0; nAxisIdx<nGantryAxisCount; nAxisIdx++)
|
{
|
int nGoPosAddr = GetAxisGoPositionAddr(nStartAxisIdx+nAxisIdx);
|
if (nGoPosAddr==MOTOR_ADDRESS_NONE)
|
{
|
delete fPosData;
|
|
return FALSE;
|
}
|
|
GetAxisLimitPosition(nStartAxisIdx+nAxisIdx, dPlusLimit, dMinusLimit);
|
|
for (int nPosIdx=0; nPosIdx<nPointCount; nPosIdx++)
|
{
|
dPosData = vectorPosData[(nPosIdx*nGantryAxisCount)+nAxisIdx];
|
|
if(dPosData > dMinusLimit && dPosData < dPlusLimit)
|
{
|
fPosData[nPosIdx] = float(dPosData);
|
}
|
else
|
{
|
fPosData[nPosIdx] = 0.0;
|
if (m_pMC2P)
|
{
|
m_pMC2P->IMC2P_DisplayMessage(m_nIndex, _T("MotorPosition Idx[%d] Position[%.03lf] Limit"), nAxisIdx, dPosData);
|
}
|
}
|
}
|
|
// write y pos
|
if (WriteAddressValue(nGoPosAddr, fPosData, nPointCount)==FALSE)
|
{
|
delete fPosData;
|
|
return FALSE;
|
}
|
}
|
|
delete fPosData;
|
|
// count
|
int nMoveCountAddr = (int)GetGantryMeasureMoveCountAddr(nGantryIdx);
|
if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
if (WriteAddressValue(nMoveCountAddr, (long)nPointCount)==FALSE)
|
{
|
return FALSE;
|
}
|
|
if (bCommand==FALSE)
|
{
|
return TRUE;
|
}
|
|
// command
|
int nMoveCmdAddr = (int)GetGantryMeasureAutoGoAddr(nGantryIdx);
|
|
return WriteAddressValue(nMoveCmdAddr, (long)1);
|
}
|
|
BOOL CMotorControl::GantryMeasureAutoGo(int nGantryIdx)
|
{
|
int nGoCmdAddr = (int)GetGantryMeasureAutoGoAddr(nGantryIdx);
|
|
if (nGoCmdAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(nGoCmdAddr, (long)1);
|
}
|
|
BOOL CMotorControl::GantryMeasureAutoGoAck( int nGantryIdx, long &lAck )
|
{
|
int nGantryMeasureAutoGoAckAddr = GetGantryMeasureAutoGoAckAddr(nGantryIdx);
|
if (nGantryMeasureAutoGoAckAddr == MOTOR_ADDRESS_NONE) return FALSE;
|
|
return ReadAddressValue(nGantryMeasureAutoGoAckAddr, lAck);
|
}
|
|
BOOL CMotorControl::GantryMeasureAutoGoAck( int nGantryIdx, long &lAck, DWORD nWaitTime )
|
{
|
// delay time
|
int nDelayTime = 10;
|
|
BOOL bResult = FALSE;
|
DWORD nStartTick = GetTickCount();
|
|
do
|
{
|
::Sleep(nDelayTime);
|
GantryMeasureAutoGoAck( nGantryIdx, lAck );
|
|
// Loop Stop Á¶°Ç. ( Ack °ª¿¡ µû¶ó¼... )
|
if (lAck == TRUE)
|
bResult = TRUE;
|
|
// wait time
|
if ( (GetTickCount()-nStartTick) > nWaitTime)
|
{
|
break;
|
}
|
} while (!bResult);
|
|
return TRUE;
|
}
|
|
BOOL CMotorControl::GantrySetMeasureMoveCount(int nGantryIdx, long nCount)
|
{
|
UINT nMoveCountAddr = GetGantryMeasureMoveCountAddr(nGantryIdx);
|
if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(nMoveCountAddr, nCount);
|
}
|
|
BOOL CMotorControl::GantryGetMeasureMoveCount(int nGantryIdx, long& nCount)
|
{
|
UINT nCountAddr = GetGantryMeasureMoveCountAddr(nGantryIdx);
|
if (nCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return ReadAddressValue(nCountAddr, nCount);
|
}
|
|
BOOL CMotorControl::GantryGetMeasureTriggerCount(int nGantryIdx, long& nCount)
|
{
|
UINT nCountAddr = GetGantryMeasureTriggerCountAddr(nGantryIdx);
|
if (nCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return ReadAddressValue(nCountAddr, nCount);
|
}
|
|
BOOL CMotorControl::GantryWsiAutoGo(int nGantryIdx, const VectorDouble& vectorPosX, const VectorDouble& vectorPosY, BOOL bCommand)
|
{
|
double dPlusLimitX, dMinusLimitX, dPlusLimitY, dMinusLimitY;
|
int nPointCount = (int)vectorPosX.size();
|
|
if (nPointCount > m_ControlInfo.GetMaxAxisMoveCount()) return FALSE;
|
|
if (nPointCount<1 || vectorPosY.size()<1) return FALSE;
|
|
if (nPointCount!=(int)vectorPosY.size()) return FALSE;
|
|
int nStartAxisIdx = 0;
|
for (int i=0; i<nGantryIdx; i++)
|
{
|
nStartAxisIdx += GetGantryAxisCount(i);
|
}
|
|
int nGoPosAddrX = GetAxisGoPositionAddr(nStartAxisIdx);
|
if (nGoPosAddrX==MOTOR_ADDRESS_NONE) return FALSE;
|
|
int nGoPosAddrY = GetAxisGoPositionAddr(nStartAxisIdx+1);
|
if (nGoPosAddrY==MOTOR_ADDRESS_NONE) return FALSE;
|
|
GetAxisLimitPosition(nStartAxisIdx, dPlusLimitX, dMinusLimitX);
|
GetAxisLimitPosition(nStartAxisIdx+1, dPlusLimitY, dMinusLimitY);
|
|
BOOL bSuccess = TRUE;
|
int nMaxMotorMoveCount = m_ControlInfo.GetMaxAxisMoveCount();
|
float *fXPos=NULL, *fYPos=NULL;
|
fXPos = new float[nMaxMotorMoveCount];
|
fYPos = new float[nMaxMotorMoveCount];
|
ZeroMemory(fXPos, sizeof(float)*nMaxMotorMoveCount);
|
ZeroMemory(fYPos, sizeof(float)*nMaxMotorMoveCount);
|
|
for (int i=0; i<nPointCount; i++)
|
{
|
if(vectorPosX[i] > dMinusLimitX && vectorPosX[i] < dPlusLimitX)
|
{
|
fXPos[i] = float(vectorPosX[i]);
|
}
|
else
|
{
|
fXPos[i] = 0.0f;
|
if (m_pMC2P)
|
{
|
m_pMC2P->IMC2P_DisplayMessage(m_nIndex, _T("X MotorPosition Idx[%d] Position[%.03lf] Limit"), i, vectorPosX[i]);
|
}
|
}
|
|
if(vectorPosY[i] > dMinusLimitY && vectorPosY[i] < dPlusLimitY)
|
{
|
fYPos[i] = float(vectorPosY[i]);
|
}
|
else
|
{
|
fYPos[i] = 0.0f;
|
if (m_pMC2P)
|
{
|
m_pMC2P->IMC2P_DisplayMessage(m_nIndex, _T("Y MotorPosition Idx[%d] Position[%.03lf] Limit"), i, vectorPosY[i]);
|
}
|
}
|
}
|
|
// write x pos
|
if (WriteAddressValue(nGoPosAddrX, fXPos, nPointCount)==FALSE)
|
{
|
delete fXPos;
|
delete fYPos;
|
|
return FALSE;
|
}
|
|
// write y pos
|
if (WriteAddressValue(nGoPosAddrY, fYPos, nPointCount)==FALSE)
|
{
|
delete fXPos;
|
delete fYPos;
|
|
return FALSE;
|
}
|
|
delete fXPos;
|
delete fYPos;
|
|
if (bCommand==FALSE) return TRUE;
|
|
|
int nMoveCountAddr = (int)GetGantryWsiMoveCountAddr(nGantryIdx);
|
if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
if (WriteAddressValue(nMoveCountAddr, (long)nPointCount)==FALSE)
|
{
|
return FALSE;
|
}
|
|
if (bCommand==FALSE)
|
{
|
return TRUE;
|
}
|
|
|
int nGoCmdAddr = (int)GetGantryWsiAutoGoAddr(nGantryIdx);
|
//int nGoCmdAddr = (int)GetGantryWsiAutoGoAddr((long)1);
|
if (nGoCmdAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(nGoCmdAddr, (long)1);
|
}
|
|
BOOL CMotorControl::GantryWsiAutoGo(int nGantryIdx)
|
{
|
int nGoCmdAddr = (int)GetGantryWsiAutoGoAddr(nGantryIdx);
|
|
if (nGoCmdAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(nGoCmdAddr, (long)1);
|
}
|
|
BOOL CMotorControl::GantryWsiAutoGoAck( int nGantryIdx, long &lAck )
|
{
|
int nGantryWsiAutoGoAckAddr = GetGantryWsiAutoGoAckAddr(nGantryIdx);
|
if(nGantryWsiAutoGoAckAddr == MOTOR_ADDRESS_NONE) return FALSE;
|
|
return ReadAddressValue(nGantryWsiAutoGoAckAddr,lAck);
|
}
|
|
BOOL CMotorControl::GantryWsiAutoGoAck( int nGantryIdx, long &lAck, DWORD nWaitTime )
|
{
|
// delay time
|
int nDelayTime = 10;
|
|
BOOL bResult = FALSE;
|
DWORD nStartTick = GetTickCount();
|
|
do
|
{
|
::Sleep(nDelayTime);
|
GantryWsiAutoGoAck( nGantryIdx, lAck );
|
|
// Loop Stop Á¶°Ç. ( Ack °ª¿¡ µû¶ó¼... )
|
if (lAck == TRUE)
|
bResult = TRUE;
|
|
// wait time
|
if ( (GetTickCount()-nStartTick) > nWaitTime)
|
{
|
break;
|
}
|
} while (!bResult);
|
|
return TRUE;
|
}
|
|
BOOL CMotorControl::GantrySetWsiMoveCount(int nGantryIdx, long nCount)
|
{
|
UINT nMoveCountAddr = GetGantryWsiMoveCountAddr(nGantryIdx);
|
if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(nMoveCountAddr, nCount);
|
}
|
|
BOOL CMotorControl::GantryGetWsiMoveCount(int nGantryIdx, long& nCount)
|
{
|
UINT nCountAddr = GetGantryWsiMoveCountAddr(nGantryIdx);
|
if (nCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return ReadAddressValue(nCountAddr, nCount);
|
}
|
|
BOOL CMotorControl::GantryGetWsiTriggerCount(int nGantryIdx, long& nCount)
|
{
|
UINT nCountAddr = GetGantryWsiTriggerCountAddr(nGantryIdx);
|
if (nCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return ReadAddressValue(nCountAddr, nCount);
|
}
|
|
BOOL CMotorControl::GantryWsiNextPosMove( int nGantryIdx, long nNextIdx )
|
{
|
if (nNextIdx<0 || nNextIdx>m_ControlInfo.GetMaxAxisMoveCount()) return FALSE;
|
|
UINT nAddr = GetGantryWsiNextPosMoveAddr(nGantryIdx);
|
if (nAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
//¸ðÅÍ¿¡°Ô ÇöÀç À妽º ¿Í ´ÙÀ½ ÃøÁ¤ Æ÷ÀÎÆ® À̵¿ ¸í·É
|
|
// set next index
|
if (!WriteAddressValue(nAddr+2, nNextIdx)) return FALSE;
|
|
// set next position move signal
|
return WriteAddressValue(nAddr, long(1));
|
}
|
|
// Software
|
BOOL CMotorControl::SoftWareTriggerCmd( int nGantryIdx, long lCmd )
|
{
|
int nSoftwareTriggerCmdAddr = GetSoftwareTriggerCmdAddr(nGantryIdx);
|
if (nSoftwareTriggerCmdAddr == MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(nSoftwareTriggerCmdAddr, lCmd);
|
}
|
|
// common
|
BOOL CMotorControl::CommonSetAfDelayTime(long nTime)
|
{
|
if (!m_bConnected) return FALSE;
|
|
const CMotorCommonAddr *pAddr = m_ControlInfo.GetMotorCommonAddr();
|
if (pAddr==NULL) return FALSE;
|
|
if (pAddr->m_nAfDelayTimeAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
if(nTime < 1)
|
{
|
return WriteAddressValue(pAddr->m_nAfDelayTimeAddr, (long)m_ControlInfo.GetAfDelayTime());
|
}
|
|
return WriteAddressValue(pAddr->m_nAfDelayTimeAddr, (float)nTime);
|
}
|
|
BOOL CMotorControl::CommonSetImageDelayTime(long nTime)
|
{
|
if (!m_bConnected) return FALSE;
|
|
const CMotorCommonAddr *pAddr = m_ControlInfo.GetMotorCommonAddr();
|
if (pAddr==NULL) return FALSE;
|
|
if (pAddr->m_nAfDelayTimeAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
if(nTime < 1)
|
{
|
return WriteAddressValue(pAddr->m_nImageDelayTimeAddr, (long)m_ControlInfo.GetImageDelayTime());
|
}
|
|
return WriteAddressValue(pAddr->m_nImageDelayTimeAddr, (float)nTime);
|
}
|
|
|
BOOL CMotorControl::CommonSetWsiDelayTime(long nTime)
|
{
|
if (!m_bConnected) return FALSE;
|
|
const CMotorCommonAddr *pAddr = m_ControlInfo.GetMotorCommonAddr();
|
if (pAddr==NULL) return FALSE;
|
|
if (pAddr->m_nWsiDelayTimeAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
if(nTime < 1)
|
{
|
return WriteAddressValue(pAddr->m_nWsiDelayTimeAddr, (long)m_ControlInfo.GetWsiDelayTime());
|
}
|
|
return WriteAddressValue(pAddr->m_nWsiDelayTimeAddr, (float)nTime);
|
}
|
|
BOOL CMotorControl::CommonSetAutoStop()
|
{
|
if (!m_bConnected) return FALSE;
|
|
const CMotorCommonAddr *pAddr = m_ControlInfo.GetMotorCommonAddr();
|
if (pAddr==NULL) return FALSE;
|
|
if (pAddr->m_nAllAutoStopAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(pAddr->m_nAllAutoStopAddr, (long)1);
|
}
|
|
BOOL CMotorControl::CommonSetAutoGoAll()
|
{
|
if (!m_bConnected) return FALSE;
|
|
const CMotorCommonAddr *pAddr = m_ControlInfo.GetMotorCommonAddr();
|
if (pAddr==NULL) return FALSE;
|
|
if (pAddr->m_nAllAutoReviewGoAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(pAddr->m_nAllAutoReviewGoAddr, (long)1);
|
}
|
|
BOOL CMotorControl::CommonSetAutoMeasureGoAll()
|
{
|
if (!m_bConnected) return FALSE;
|
|
const CMotorCommonAddr *pAddr = m_ControlInfo.GetMotorCommonAddr();
|
if (pAddr==NULL) return FALSE;
|
|
if (pAddr->m_nAllAutoMeasureGoAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(pAddr->m_nAllAutoMeasureGoAddr, (long)1);
|
}
|
|
BOOL CMotorControl::CommonSetAutoWsiGoAll()
|
{
|
const CMotorCommonAddr *pAddr = m_ControlInfo.GetMotorCommonAddr();
|
if (pAddr==NULL) return FALSE;
|
|
if (pAddr->m_nAllAutoWsiGoAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
|
return WriteAddressValue(pAddr->m_nAllAutoWsiGoAddr, (long)1);
|
}
|
|
|
|
BOOL CMotorControl::RemoveAllAutoGoCount(int nGantryIdx)
|
{
|
if (m_bConnected==FALSE) return FALSE;
|
|
// count
|
int nMoveCountAddr = (int)GetGantryMeasureMoveCountAddr(nGantryIdx);
|
if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
if (WriteAddressValue(nMoveCountAddr, (long)0)==FALSE)
|
{
|
return FALSE;
|
}
|
|
nMoveCountAddr = (int)GetGantryMoveCountAddr(nGantryIdx);
|
if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
if (WriteAddressValue(nMoveCountAddr, (long)0)==FALSE)
|
{
|
return FALSE;
|
}
|
|
nMoveCountAddr = (int)GetGantryWsiMoveCountAddr(nGantryIdx);
|
if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
if (WriteAddressValue(nMoveCountAddr, (long)0)==FALSE)
|
{
|
return FALSE;
|
}
|
|
//Trigger
|
nMoveCountAddr = (int)GetGantryMeasureTriggerCountAddr(nGantryIdx);
|
if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
if (WriteAddressValue(nMoveCountAddr, (long)0)==FALSE)
|
{
|
return FALSE;
|
}
|
|
nMoveCountAddr = (int)GetGantryTriggerCountAddr(nGantryIdx);
|
if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
if (WriteAddressValue(nMoveCountAddr, (long)0)==FALSE)
|
{
|
return FALSE;
|
}
|
|
nMoveCountAddr = (int)GetGantryWsiTriggerCountAddr(nGantryIdx);
|
if (nMoveCountAddr==MOTOR_ADDRESS_NONE) return FALSE;
|
if (WriteAddressValue(nMoveCountAddr, (long)0)==FALSE)
|
{
|
return FALSE;
|
}
|
|
return TRUE;
|
}
|
|
BOOL CMotorControl::StartThread()
|
{
|
return CTimerThreadPools::StartThread();
|
}
|
|
void CMotorControl::StopThread()
|
{
|
CTimerThreadPools::StopThread();
|
}
|
|
int CMotorControl::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;
|
|
}
|
