#include "StdAfx.h"
|
#include "CHMotorControls/MotorControl_Simulation.h"
|
|
|
CMotorControl_Simulation::CMotorControl_Simulation( int nIndex, DWORD dwPeriod/*=100*/,int nThreadCount /*= 1*/ ) : CMotorControl(nIndex, dwPeriod, nThreadCount)
|
{
|
m_pMotionInfo_Axis = NULL;
|
m_pCurAxisPosition = NULL;
|
m_pCurAxisVelocity = NULL;
|
m_pCurAxisStatus = NULL;
|
|
m_MotionInfo_ThetaAxis.SetIndex(THETAMOTOR_AXIS_INDEX);
|
m_lCurThetaAxisStatus = 1;
|
m_fCurThetaAxisPosition = 0.;
|
m_fCurThetaAxisVelocity = 0.;
|
}
|
|
CMotorControl_Simulation::~CMotorControl_Simulation(void)
|
{
|
if (m_pCurAxisStatus)
|
{
|
delete [] m_pCurAxisStatus;
|
m_pCurAxisStatus = NULL;
|
}
|
|
if (m_pCurAxisVelocity)
|
{
|
delete [] m_pCurAxisVelocity;
|
m_pCurAxisVelocity = NULL;
|
}
|
|
if (m_pCurAxisPosition)
|
{
|
delete [] m_pCurAxisPosition;
|
m_pCurAxisPosition = NULL;
|
}
|
|
if (m_pMotionInfo_Axis)
|
{
|
delete [] m_pMotionInfo_Axis;
|
m_pMotionInfo_Axis = NULL;
|
}
|
}
|
|
int CMotorControl_Simulation::Connect( const CMotorControlInfo* pControlInfo )
|
{
|
if (pControlInfo==NULL) return 0;
|
|
// set motor control info
|
m_ControlInfo = *pControlInfo;
|
|
// 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;
|
|
// new
|
if (m_pMotionInfo_Axis) delete [] m_pMotionInfo_Axis;
|
m_pMotionInfo_Axis = new CMotionInfo_Axis[m_nAxisCount];
|
|
if (m_pCurAxisPosition) delete [] m_pCurAxisPosition;
|
m_pCurAxisPosition = new float[m_nAxisCount];
|
memset(m_pCurAxisPosition, 0, sizeof(float)*m_nAxisCount);
|
|
if (m_pCurAxisVelocity) delete [] m_pCurAxisVelocity;
|
m_pCurAxisVelocity = new float[m_nAxisCount];
|
memset(m_pCurAxisVelocity, 0, sizeof(float)*m_nAxisCount);
|
|
if (m_pCurAxisStatus) delete [] m_pCurAxisStatus;
|
m_pCurAxisStatus = new long[m_nAxisCount];
|
memset(m_pCurAxisStatus, 0, sizeof(long)*m_nAxisCount);
|
|
for (int nAxisIndex=0; nAxisIndex<m_nAxisCount; nAxisIndex++)
|
{
|
m_pMotionInfo_Axis[nAxisIndex].SetIndex(nAxisIndex);
|
m_pMotionInfo_Axis[nAxisIndex].SetMoveData(500, 0.1, 0.1, 0, 100);
|
m_pMotionInfo_Axis[nAxisIndex].SetJogData(10, 0.1, 0.1, 0);
|
|
m_pCurAxisStatus[nAxisIndex] = 1; // not moving
|
}
|
|
|
if(m_ControlInfo.GetUseThetaMotor() == TRUE)
|
{
|
m_MotionInfo_ThetaAxis.SetMoveData(1,0.1,0.1,0,360);
|
m_MotionInfo_ThetaAxis.SetJogData(0.1, 0.1, 0.1, 0);
|
m_lCurThetaAxisStatus = 1;
|
m_fCurThetaAxisPosition = 0;
|
m_fCurThetaAxisVelocity = 0;
|
}
|
|
m_bConnected = TRUE;
|
|
return 1;
|
}
|
|
void CMotorControl_Simulation::Disconnect()
|
{
|
if (m_bConnected)
|
{
|
m_bConnected = FALSE;
|
}
|
}
|
|
BOOL CMotorControl_Simulation::ReadAddressValue( int nAddress, long &nValue )
|
{
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Simulation::ReadAddressValue( int nAddress, float &fValue )
|
{
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Simulation::ReadAddressValue( int nAddress, double &dValue )
|
{
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Simulation::ReadAddressValue( long nAddress, long *pArrayData, int nArrayCount )
|
{
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Simulation::ReadAddressValue( long nAddress, float *pArrayData, int nArrayCount )
|
{
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Simulation::WriteAddressValue( int nAddress, long nValue )
|
{
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Simulation::WriteAddressValue( int nAddress, float fValue )
|
{
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Simulation::WriteAddressValue( int nAddress, double dValue )
|
{
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Simulation::WriteAddressValue( long nAddress, long *pArrayData, int nArrayCount )
|
{
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Simulation::WriteAddressValue( long nAddress, float *pArrayData, int nArrayCount )
|
{
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Simulation::CommonSetAutoStop()
|
{
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Simulation::AxisManualGo( int nAxisIdx, double dPosition )
|
{
|
if (nAxisIdx<0 || nAxisIdx>=m_nAxisCount) return FALSE;
|
|
if (m_pMotionThread==NULL) return FALSE;
|
|
CMotionData data;
|
data.nIndex = nAxisIdx;
|
data.nType = ThreadDataType_AxisManualGo;
|
data.uData[0].dValue = (double)m_pCurAxisPosition[nAxisIdx]; // current pos
|
data.uData[1].dValue = dPosition; // target pos
|
|
return m_pMotionThread->AddThreadData(data);
|
}
|
|
BOOL CMotorControl_Simulation::AxisJogPlus( int nAxisIdx )
|
{
|
if (nAxisIdx<0 || nAxisIdx>=m_nAxisCount) return FALSE;
|
|
if (m_pMotionThread==NULL) return FALSE;
|
|
CMotionData data;
|
data.nIndex = nAxisIdx;
|
data.nType = ThreadDataType_JogGo;
|
data.uData[0].dValue = (double)m_pCurAxisPosition[nAxisIdx]; // current pos
|
data.uData[1].nValue = 1; // jog plus
|
|
return m_pMotionThread->AddThreadData(data);
|
}
|
|
BOOL CMotorControl_Simulation::AxisJogMinus( int nAxisIdx )
|
{
|
if (nAxisIdx<0 || nAxisIdx>=m_nAxisCount) return FALSE;
|
|
if (m_pMotionThread==NULL) return FALSE;
|
|
CMotionData data;
|
data.nIndex = nAxisIdx;
|
data.nType = ThreadDataType_JogGo;
|
data.uData[0].dValue = (double)m_pCurAxisPosition[nAxisIdx]; // current pos
|
data.uData[1].nValue = -1; // jog minus
|
|
return m_pMotionThread->AddThreadData(data);
|
}
|
|
BOOL CMotorControl_Simulation::AxisJogStop( int nAxisIdx )
|
{
|
CMotionInfo_Axis *pMotionAxis = GetMotionInfo_Axis(nAxisIdx);
|
if (pMotionAxis==NULL) return FALSE;
|
|
if (pMotionAxis->GetCurStatus()==0) // moving?
|
{
|
pMotionAxis->SetEmergencyStop(1); // stop command
|
TRACE(_T("%d Axis Stop!\n"), nAxisIdx);
|
}
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Simulation::AxisManualGoSpeed( int nAxisIdx, double dSpeed )
|
{
|
CMotionInfo_Axis *pMotionAxis = GetMotionInfo_Axis(nAxisIdx);
|
if (pMotionAxis==NULL) return FALSE;
|
pMotionAxis->SetMoveVelocity(dSpeed);
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Simulation::AxisJogSpeed( int nAxisIdx, double dSpeed )
|
{
|
CMotionInfo_Axis *pMotionAxis = GetMotionInfo_Axis(nAxisIdx);
|
if (pMotionAxis==NULL) return FALSE;
|
pMotionAxis->SetJogVelocity(dSpeed);
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Simulation::AxisManualGoAccel( int nAxisIdx, int nAccel )
|
{
|
CMotionInfo_Axis *pMotionAxis = GetMotionInfo_Axis(nAxisIdx);
|
if (pMotionAxis==NULL) return FALSE;
|
pMotionAxis->SetMoveAccelTime(double(nAccel) / 1000.);
|
pMotionAxis->SetMoveDecelTime(double(nAccel) / 1000.);
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Simulation::GantryManualGo( int nGantryIdx, const VectorDouble& vectorPos )
|
{
|
if (m_pMotionThread==NULL) return FALSE;
|
|
// axis count check
|
int nAxisCount = GetGantryAxisCount(nGantryIdx);
|
if (nAxisCount < (int)vectorPos.size()) return FALSE;
|
|
// axis index check
|
int nAxisIdx = 0;
|
for (int i=0; i<nGantryIdx; i++)
|
{
|
nAxisIdx += GetGantryAxisCount(i);
|
}
|
|
CMotionData data;
|
data.nIndex = nGantryIdx;
|
data.nType = ThreadDataType_GantryGo;
|
|
int nTotalIdx = 0;
|
data.uData[nTotalIdx++].nValue = (int)vectorPos.size(); // axis count
|
|
for (int nIdx=0; nIdx<(int)vectorPos.size(); nIdx++)
|
{
|
// axis index
|
data.uData[nTotalIdx++].nValue = nAxisIdx;
|
|
// axis cur pos
|
data.uData[nTotalIdx++].dValue = m_pCurAxisPosition[nAxisIdx];
|
|
// axis target pos
|
data.uData[nTotalIdx++].dValue = vectorPos[nIdx];
|
|
nAxisIdx++;
|
}
|
|
return m_pMotionThread->AddThreadData(data);
|
}
|
|
BOOL CMotorControl_Simulation::GantryManualGo( int nGantryIdx, const VectorDouble& vectorPos, int nMoveType )
|
{
|
if (m_pMotionThread==NULL) return FALSE;
|
|
// axis count check
|
int nAxisCount = GetGantryAxisCount(nGantryIdx);
|
if (nAxisCount < (int)vectorPos.size()) return FALSE;
|
|
// axis index check
|
int nAxisIdx = 0;
|
for (int i=0; i<nGantryIdx; i++)
|
{
|
nAxisIdx += GetGantryAxisCount(i);
|
}
|
|
CMotionData data;
|
data.nIndex = nGantryIdx;
|
data.nType = ThreadDataType_GantryGo;
|
|
int nTotalIdx = 0;
|
data.uData[nTotalIdx++].nValue = (int)vectorPos.size(); // axis count
|
|
for (int nIdx=0; nIdx<(int)vectorPos.size(); nIdx++)
|
{
|
// axis index
|
data.uData[nTotalIdx++].nValue = nAxisIdx;
|
|
// axis cur pos
|
data.uData[nTotalIdx++].dValue = m_pCurAxisPosition[nAxisIdx];
|
|
// axis target pos
|
data.uData[nTotalIdx++].dValue = vectorPos[nIdx];
|
|
nAxisIdx++;
|
}
|
|
return m_pMotionThread->AddThreadData(data);
|
}
|
|
BOOL CMotorControl_Simulation::GantryManualGo( int nGantryIdx, double dPosX, double dPosY )
|
{
|
if (m_pMotionThread==NULL) return FALSE;
|
|
// axis count check
|
int nAxisCount = GetGantryAxisCount(nGantryIdx);
|
if (nAxisCount < 2) return FALSE;
|
|
// axis index check
|
int nAxisIdx = 0;
|
for (int i=0; i<nGantryIdx; i++)
|
{
|
nAxisIdx += GetGantryAxisCount(i);
|
}
|
|
CMotionData data;
|
data.nIndex = nGantryIdx;
|
data.nType = ThreadDataType_GantryGo;
|
|
int nTotalIdx = 0;
|
data.uData[nTotalIdx++].nValue = 2; // axis count
|
|
// axis index
|
data.uData[nTotalIdx++].nValue = nAxisIdx;
|
|
// axis cur pos
|
data.uData[nTotalIdx++].dValue = m_pCurAxisPosition[nAxisIdx];
|
|
// axis target pos
|
data.uData[nTotalIdx++].dValue = dPosX;
|
|
nAxisIdx++;
|
|
// axis index
|
data.uData[nTotalIdx++].nValue = nAxisIdx;
|
|
// axis cur pos
|
data.uData[nTotalIdx++].dValue = m_pCurAxisPosition[nAxisIdx];
|
|
// axis target pos
|
data.uData[nTotalIdx++].dValue = dPosY;
|
|
return m_pMotionThread->AddThreadData(data);
|
}
|
|
BOOL CMotorControl_Simulation::GantryAutoGo( int nGantryIdx, const VectorDouble& vectorPosX, const VectorDouble& vectorPosY, BOOL bCommand/*=FALSE*/ )
|
{
|
if (m_pMotionThread==NULL) return FALSE;
|
|
// axis count check
|
int nAxisCount = GetGantryAxisCount(nGantryIdx);
|
if (nAxisCount < 2) return FALSE;
|
|
// axis index check
|
int nAxisIdx = 0;
|
for (int i=0; i<nGantryIdx; i++)
|
{
|
nAxisIdx += GetGantryAxisCount(i);
|
}
|
|
CMotionData data;
|
data.nIndex = nGantryIdx;
|
data.nType = ThreadDataType_AutoGo;
|
|
int nTotalIdx = 0;
|
data.uData[nTotalIdx++].nValue = 2; // axis count
|
|
// axis index
|
data.uData[nTotalIdx++].nValue = nAxisIdx;
|
|
// axis cur pos
|
data.uData[nTotalIdx++].dValue = m_pCurAxisPosition[nAxisIdx];
|
|
// axis target pos
|
data.uData[nTotalIdx++].dValue = vectorPosX[0];
|
data.vecDoubleX = vectorPosX;
|
|
nAxisIdx++;
|
|
// axis index
|
data.uData[nTotalIdx++].nValue = nAxisIdx;
|
|
// axis cur pos
|
data.uData[nTotalIdx++].dValue = m_pCurAxisPosition[nAxisIdx];
|
|
// axis target pos
|
data.uData[nTotalIdx++].dValue = vectorPosY[0];
|
data.vecDoubleY = vectorPosY;
|
|
return m_pMotionThread->AddThreadData(data);
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
return TRUE;
|
}
|
|
void CMotorControl_Simulation::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;
|
|
case ThreadDataType_AxisManualGo:
|
Process_AxisManualGo(motionData);
|
break;
|
|
case ThreadDataType_JogGo :
|
Process_JogGo(motionData);
|
break;
|
|
case ThreadDataType_GantryGo :
|
Process_GantryGo(motionData);
|
break;
|
|
case ThreadDataType_AutoGo :
|
Process_AutoGo(motionData);
|
break;
|
}
|
}
|
|
void CMotorControl_Simulation::Process_AxisManualGo( const CMotionData& motionData )
|
{
|
CMotionInfo_Axis *pMotionAxis = GetMotionInfo_Axis(motionData.nIndex);
|
float *pCurPos = GetCurAxisPosition(motionData.nIndex);
|
float *pCurVel = GetCurAxisVelocity(motionData.nIndex);
|
long *pCurStatus = GetCurAxisStatus(motionData.nIndex);
|
|
if (pMotionAxis==NULL || pCurPos==NULL || pCurVel==NULL || pCurStatus==NULL) return;
|
|
pMotionAxis->SetMoveStartPos(motionData.uData[0].dValue); // start pos
|
pMotionAxis->SetMoveEndPos(motionData.uData[1].dValue); // end pos
|
DWORD dwPeriod = m_dwPeriod / 2;
|
double dTimeSec = 0.0;
|
double dPos, dVel;
|
UINT nStatus;
|
|
DWORD dwStartTick = GetTickCount();
|
do
|
{
|
::Sleep(dwPeriod);
|
|
// cal time
|
dTimeSec = double(GetTickCount() - dwStartTick) / 1000.0;
|
|
// cal motion
|
if (pMotionAxis->CalculateMoveMotion(dTimeSec, dPos, dVel, nStatus))
|
{
|
*pCurPos = (float)dPos;
|
*pCurVel = (float)dVel;
|
*pCurStatus = (long)nStatus;
|
}
|
|
// check emo
|
if (pMotionAxis->GetEmergencyStop())
|
{
|
pMotionAxis->SetEmergencyStop(0);
|
pMotionAxis->SetCurStatus(1);
|
//AfxMessageBox(_T("stop"));
|
break;
|
}
|
|
} while (nStatus!=1);
|
|
|
}
|
|
void CMotorControl_Simulation::Process_JogGo( const CMotionData& motionData )
|
{
|
CMotionInfo_Axis *pMotionAxis = GetMotionInfo_Axis(motionData.nIndex);
|
float *pCurPos = GetCurAxisPosition(motionData.nIndex);
|
float *pCurVel = GetCurAxisVelocity(motionData.nIndex);
|
long *pCurStatus = GetCurAxisStatus(motionData.nIndex);
|
|
if (pMotionAxis==NULL || pCurPos==NULL || pCurVel==NULL || pCurStatus==NULL) return;
|
|
pMotionAxis->SetJogStartPos(motionData.uData[0].dValue); // start pos
|
int nJogCmd = motionData.uData[1].nValue;
|
DWORD dwPeriod = m_dwPeriod / 2;
|
double dTimeSec = 0.0;
|
double dPos, dVel;
|
UINT nStatus;
|
|
DWORD dwStartTick = GetTickCount();
|
do
|
{
|
::Sleep(dwPeriod);
|
|
// cal time
|
dTimeSec = double(GetTickCount() - dwStartTick) / 1000.0;
|
|
// cal motion
|
if (pMotionAxis->CalculateJogMotion(dTimeSec, nJogCmd, dPos, dVel, nStatus))
|
{
|
*pCurPos = (float)dPos;
|
*pCurVel = (float)dVel;
|
*pCurStatus = (long)nStatus;
|
}
|
|
// check emo
|
if (pMotionAxis->GetEmergencyStop())
|
{
|
pMotionAxis->SetEmergencyStop(0);
|
pMotionAxis->SetCurStatus(1);
|
//AfxMessageBox(_T("stop"));
|
break;
|
}
|
|
} while (nStatus!=1);
|
|
}
|
|
void CMotorControl_Simulation::Process_GantryGo( const CMotionData& motionData )
|
{
|
int nTotalIdx = 0;
|
int nGantryIdx = motionData.nIndex;
|
int nAxisCount = motionData.uData[nTotalIdx++].nValue;
|
|
|
CMotionInfo_Axis **pMotionAxis = new CMotionInfo_Axis*[nAxisCount];
|
float **pCurPos = new float*[nAxisCount];
|
float **pCurVel = new float*[nAxisCount];
|
long **pCurStatus = new long*[nAxisCount];
|
|
for (int nIdx=0; nIdx<nAxisCount; nIdx++)
|
{
|
pMotionAxis[nIdx] = GetMotionInfo_Axis(motionData.uData[nTotalIdx].nValue);
|
pCurPos[nIdx] = GetCurAxisPosition(motionData.uData[nTotalIdx].nValue);
|
pCurVel[nIdx] = GetCurAxisVelocity(motionData.uData[nTotalIdx].nValue);
|
pCurStatus[nIdx] = GetCurAxisStatus(motionData.uData[nTotalIdx].nValue);
|
nTotalIdx++;
|
|
if (pMotionAxis[nIdx]==NULL || pCurPos[nIdx]==NULL || pCurVel[nIdx]==NULL || pCurStatus[nIdx]==NULL) continue;
|
|
pMotionAxis[nIdx]->SetMoveStartPos(motionData.uData[nTotalIdx++].dValue); // start pos
|
pMotionAxis[nIdx]->SetMoveEndPos(motionData.uData[nTotalIdx++].dValue); // end pos
|
}
|
|
DWORD dwPeriod = m_dwPeriod / 2;
|
double dTimeSec = 0.0;
|
double dPos, dVel;
|
UINT nStatus;
|
UINT nTotalStatus;
|
|
DWORD dwStartTick = GetTickCount();
|
do
|
{
|
::Sleep(dwPeriod);
|
|
// cal time
|
dTimeSec = double(GetTickCount() - dwStartTick) / 1000.0;
|
|
nTotalStatus = 1;
|
for (int nIdx=0; nIdx<nAxisCount; nIdx++)
|
{
|
// cal motion
|
if (pMotionAxis[nIdx]->CalculateMoveMotion(dTimeSec, dPos, dVel, nStatus))
|
{
|
*pCurPos[nIdx] = (float)dPos;
|
*pCurVel[nIdx] = (float)dVel;
|
*pCurStatus[nIdx] = (long)nStatus;
|
|
nTotalStatus = nTotalStatus * nStatus;
|
}
|
|
// check emo
|
if (pMotionAxis[nIdx]->GetEmergencyStop())
|
{
|
pMotionAxis[nIdx]->SetEmergencyStop(0);
|
pMotionAxis[nIdx]->SetCurStatus(1);
|
nTotalStatus = 1;
|
break;
|
}
|
}
|
|
} while (nTotalStatus!=1);
|
|
delete [] pMotionAxis;
|
delete [] pCurPos;
|
delete [] pCurVel;
|
delete [] pCurStatus;
|
|
}
|
|
void CMotorControl_Simulation::Process_AutoGo( const CMotionData& motionData )
|
{
|
|
// Auto Go Param
|
const CMotorControlInfo *pMotorContorlInfo = GetControlInfo();
|
if(pMotorContorlInfo == NULL) return;
|
|
DWORD dwAFTime = pMotorContorlInfo->GetAfDelayTime(); // AF Delay
|
DWORD dwSnapTime = pMotorContorlInfo->GetImageDelayTime(); // Snap Delay
|
|
// Gantry Go Param
|
int nTotalIdx = 0;
|
int nGantryIdx = motionData.nIndex;
|
int nAxisCount = motionData.uData[nTotalIdx++].nValue;
|
|
DWORD dwPeriod = m_dwPeriod / 2;
|
double dTimeSec = 0.0;
|
double dPos, dVel;
|
UINT nStatus;
|
UINT nTotalStatus;
|
|
CMotionInfo_Axis **pMotionAxis = new CMotionInfo_Axis*[nAxisCount];
|
float **pCurPos = new float*[nAxisCount];
|
float **pCurVel = new float*[nAxisCount];
|
long **pCurStatus = new long*[nAxisCount];
|
|
// Get Current Axis Info
|
for (int nIdx=0; nIdx<nAxisCount; nIdx++)
|
{
|
pMotionAxis[nIdx] = GetMotionInfo_Axis(motionData.uData[nTotalIdx].nValue);
|
pCurPos[nIdx] = GetCurAxisPosition(motionData.uData[nTotalIdx].nValue);
|
pCurVel[nIdx] = GetCurAxisVelocity(motionData.uData[nTotalIdx].nValue);
|
pCurStatus[nIdx] = GetCurAxisStatus(motionData.uData[nTotalIdx].nValue);
|
nTotalIdx+= 3;
|
}
|
|
for(int nPosIdx = 0; nPosIdx < (int)motionData.vecDoubleX.size(); nPosIdx++)
|
{
|
nTotalIdx = 1;
|
|
for (int nIdx=0; nIdx<nAxisCount; nIdx++)
|
{
|
nTotalIdx++;
|
if (pMotionAxis[nIdx]==NULL || pCurPos[nIdx]==NULL || pCurVel[nIdx]==NULL || pCurStatus[nIdx]==NULL) continue;
|
|
if(nPosIdx == 0) // 1st Position
|
{
|
pMotionAxis[nIdx]->SetMoveStartPos(motionData.uData[nTotalIdx++].dValue); // start pos
|
pMotionAxis[nIdx]->SetMoveEndPos(motionData.uData[nTotalIdx++].dValue); // end pos
|
}
|
else // N-th Position
|
{
|
if(nIdx == 0) // Set X Axis
|
{
|
pMotionAxis[nIdx]->SetMoveStartPos(motionData.vecDoubleX[nPosIdx-1]); // start pos
|
pMotionAxis[nIdx]->SetMoveEndPos(motionData.vecDoubleX[nPosIdx]); // end pos
|
}
|
else // Set Y Axis
|
{
|
pMotionAxis[nIdx]->SetMoveStartPos(motionData.vecDoubleY[nPosIdx-1]); // start pos
|
pMotionAxis[nIdx]->SetMoveEndPos(motionData.vecDoubleY[nPosIdx]); // end pos
|
}
|
}
|
}
|
|
dTimeSec = 0.0;
|
|
DWORD dwStartTick = GetTickCount();
|
do
|
{
|
::Sleep(dwPeriod);
|
|
// cal time
|
dTimeSec = double(GetTickCount() - dwStartTick) / 1000.0;
|
|
nTotalStatus = 1;
|
for (int nIdx=0; nIdx<nAxisCount; nIdx++)
|
{
|
// cal motion
|
if (pMotionAxis[nIdx]->CalculateMoveMotion(dTimeSec, dPos, dVel, nStatus))
|
{
|
*pCurPos[nIdx] = (float)dPos;
|
*pCurVel[nIdx] = (float)dVel;
|
*pCurStatus[nIdx] = (long)nStatus;
|
|
nTotalStatus = nTotalStatus * nStatus;
|
}
|
|
// check emo
|
if (pMotionAxis[nIdx]->GetEmergencyStop())
|
{
|
pMotionAxis[nIdx]->SetEmergencyStop(0);
|
pMotionAxis[nIdx]->SetCurStatus(1);
|
nTotalStatus = 1;
|
break;
|
}
|
}
|
|
} while (nTotalStatus!=1);
|
|
Sleep(dwAFTime); // AF Delay
|
|
m_pMC2P->IMC2P_GantrySoftWareTrigger(m_nIndex,nGantryIdx,nPosIdx); // Software Trigger
|
|
Sleep(dwSnapTime); // Snap Delay
|
|
}
|
|
delete [] pMotionAxis;
|
delete [] pCurPos;
|
delete [] pCurVel;
|
delete [] pCurStatus;
|
}
|
|
CMotionInfo_Axis* CMotorControl_Simulation::GetMotionInfo_Axis( int nAxisIndex )
|
{
|
if(nAxisIndex == THETAMOTOR_AXIS_INDEX)
|
{
|
return &m_MotionInfo_ThetaAxis;
|
}
|
|
if (nAxisIndex<0 || nAxisIndex>=m_nAxisCount) return NULL;
|
return &m_pMotionInfo_Axis[nAxisIndex];
|
}
|
|
float* CMotorControl_Simulation::GetCurAxisPosition( int nAxisIndex )
|
{
|
if(nAxisIndex == THETAMOTOR_AXIS_INDEX)
|
{
|
return &m_fCurThetaAxisPosition;
|
}
|
if (nAxisIndex<0 || nAxisIndex>=m_nAxisCount) return NULL;
|
return &m_pCurAxisPosition[nAxisIndex];
|
}
|
|
float* CMotorControl_Simulation::GetCurAxisVelocity( int nAxisIndex )
|
{
|
if(nAxisIndex == THETAMOTOR_AXIS_INDEX)
|
{
|
return &m_fCurThetaAxisVelocity;
|
}
|
if (nAxisIndex<0 || nAxisIndex>=m_nAxisCount) return NULL;
|
return &m_pCurAxisVelocity[nAxisIndex];
|
}
|
|
long* CMotorControl_Simulation::GetCurAxisStatus( int nAxisIndex )
|
{
|
if(nAxisIndex == THETAMOTOR_AXIS_INDEX)
|
{
|
return &m_lCurThetaAxisStatus;
|
}
|
|
if (nAxisIndex<0 || nAxisIndex>=m_nAxisCount) return NULL;
|
return &m_pCurAxisStatus[nAxisIndex];
|
}
|
|
BOOL CMotorControl_Simulation::ReadAxisPosition( float *pAxisPos, int nAxisCount )
|
{
|
if (m_pCurAxisPosition==NULL || pAxisPos==NULL || (m_nAxisCount<nAxisCount)) return FALSE;
|
memcpy(pAxisPos, m_pCurAxisPosition, sizeof(float)*nAxisCount);
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Simulation::ReadAxisStatus( long *pAxisStatus, int nAxisCount )
|
{
|
if (m_pCurAxisStatus==NULL || pAxisStatus==NULL || (m_nAxisCount<nAxisCount)) return FALSE;
|
memcpy(pAxisStatus, m_pCurAxisStatus, sizeof(long)*nAxisCount);
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Simulation::ReadThetaAxisPosition( float& fAxisPos )
|
{
|
fAxisPos = m_fCurThetaAxisPosition;
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Simulation::ReadThetaAxisStatus( long& nAxisStatus )
|
{
|
nAxisStatus = m_lCurThetaAxisStatus;
|
return TRUE;
|
}
|
|
BOOL CMotorControl_Simulation::AxisThetaJogPlus()
|
{
|
if (m_pMotionThread==NULL) return FALSE;
|
|
CMotionData data;
|
data.nIndex = THETAMOTOR_AXIS_INDEX;
|
data.nType = ThreadDataType_JogGo;
|
data.uData[0].dValue = (double)m_fCurThetaAxisPosition; // current pos
|
data.uData[1].nValue = 1; // jog plus
|
|
return m_pMotionThread->AddThreadData(data);
|
}
|
|
BOOL CMotorControl_Simulation::AxisThetaJogMinus()
|
{
|
if (m_pMotionThread==NULL) return FALSE;
|
|
CMotionData data;
|
data.nIndex = THETAMOTOR_AXIS_INDEX;
|
data.nType = ThreadDataType_JogGo;
|
data.uData[0].dValue = (double)m_fCurThetaAxisPosition; // current pos
|
data.uData[1].nValue = -1; // jog Minus
|
|
return m_pMotionThread->AddThreadData(data);
|
}
|
|
BOOL CMotorControl_Simulation::AxisThetaManualGo( double dPosition )
|
{
|
if (m_pMotionThread==NULL) return FALSE;
|
|
CMotionData data;
|
data.nIndex = THETAMOTOR_AXIS_INDEX;
|
data.nType = ThreadDataType_AxisManualGo;
|
data.uData[0].dValue = (double)m_fCurThetaAxisPosition; // current pos
|
data.uData[1].dValue = dPosition; // target pos
|
|
return m_pMotionThread->AddThreadData(data);
|
}
|
|
BOOL CMotorControl_Simulation::AxisThetaJogStop()
|
{
|
CMotionInfo_Axis *pMotionAxis = &m_MotionInfo_ThetaAxis;
|
if (pMotionAxis==NULL) return FALSE;
|
|
if (pMotionAxis->GetCurStatus()==0) // moving?
|
{
|
pMotionAxis->SetEmergencyStop(1); // stop command
|
TRACE(_T("Theta Axis Stop!\n"));
|
|
}
|
return TRUE;
|
}
|
