#include "StdAfx.h"
|
#include "MotionInfo_Gantry.h"
|
|
CMotionInfo_Gantry::CMotionInfo_Gantry( int nIndex ) : m_nIndex(nIndex)
|
{
|
m_vecMotionInfo.resize(AxisType_Count);
|
}
|
|
CMotionInfo_Gantry::~CMotionInfo_Gantry(void)
|
{
|
}
|
|
const CMotionInfo_Axis* CMotionInfo_Gantry::GetMotionInfo_Axis( int nAxisType ) const
|
{
|
if (nAxisType<0 || nAxisType>=(int)m_vecMotionInfo.size()) return NULL;
|
return &(m_vecMotionInfo[nAxisType]);
|
}
|
|
CMotionInfo_Axis* CMotionInfo_Gantry::GetMotionInfo_Axis( int nAxisType )
|
{
|
if (nAxisType<0 || nAxisType>=(int)m_vecMotionInfo.size()) return NULL;
|
return &(m_vecMotionInfo[nAxisType]);
|
}
|
|
int CMotionInfo_Gantry::GetMotionInfo_AxisCount() const
|
{
|
return (int)m_vecMotionInfo.size();
|
}
|
|
int CMotionInfo_Gantry::GetIndex() const
|
{
|
return m_nIndex;
|
}
|
|
double CMotionInfo_Gantry::GetVelocity( int nAxisType ) const
|
{
|
const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
|
if (pNode==NULL) return 0.0;
|
return pNode->GetVelocity();
|
}
|
|
double CMotionInfo_Gantry::GetAccelTime( int nAxisType ) const
|
{
|
const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
|
if (pNode==NULL) return 0.0;
|
return pNode->GetAccelTime();
|
}
|
|
double CMotionInfo_Gantry::GetDecelTime( int nAxisType ) const
|
{
|
const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
|
if (pNode==NULL) return 0.0;
|
return pNode->GetDecelTime();
|
}
|
|
double CMotionInfo_Gantry::GetStartPos( int nAxisType ) const
|
{
|
const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
|
if (pNode==NULL) return 0.0;
|
return pNode->GetStartPos();
|
}
|
|
double CMotionInfo_Gantry::GetEndPos( int nAxisType ) const
|
{
|
const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
|
if (pNode==NULL) return 0.0;
|
return pNode->GetEndPos();
|
}
|
|
double CMotionInfo_Gantry::GetCurVelocity( int& nAxisType ) const
|
{
|
const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
|
if (pNode==NULL)
|
{
|
return GetCurVelocity_Min(nAxisType);
|
}
|
return pNode->GetVelocity();
|
}
|
|
double CMotionInfo_Gantry::GetCurAccelTime( int& nAxisType ) const
|
{
|
const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
|
if (pNode==NULL)
|
{
|
return GetCurAccelTime_Max(nAxisType);
|
}
|
return pNode->GetCurAccelTime();
|
}
|
|
double CMotionInfo_Gantry::GetCurDecelTime( int& nAxisType ) const
|
{
|
const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
|
if (pNode==NULL)
|
{
|
return GetCurDecelTime_Max(nAxisType);
|
}
|
return pNode->GetCurDecelTime();
|
}
|
|
double CMotionInfo_Gantry::GetCurFixTime( int& nAxisType ) const
|
{
|
const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
|
if (pNode==NULL)
|
{
|
return GetCurFixTime_Max(nAxisType);
|
}
|
return pNode->GetCurFixTime();
|
}
|
|
double CMotionInfo_Gantry::GetCurTotalTime( int& nAxisType ) const
|
{
|
const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
|
if (pNode==NULL)
|
{
|
return GetCurTotalTime_Max(nAxisType);
|
}
|
return pNode->GetCurTotalTime();
|
}
|
|
double CMotionInfo_Gantry::GetCurAccelDist( int& nAxisType ) const
|
{
|
const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
|
if (pNode==NULL)
|
{
|
return GetCurAccelDist_Max(nAxisType);
|
}
|
return pNode->GetCurAccelDist();
|
}
|
|
double CMotionInfo_Gantry::GetCurDecelDist( int& nAxisType ) const
|
{
|
const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
|
if (pNode==NULL)
|
{
|
return GetCurDecelDist_Max(nAxisType);
|
}
|
return pNode->GetCurDecelDist();
|
}
|
|
double CMotionInfo_Gantry::GetCurFixDist( int& nAxisType ) const
|
{
|
const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
|
if (pNode==NULL)
|
{
|
return GetCurFixDist_Max(nAxisType);
|
}
|
return pNode->GetCurFixDist();
|
}
|
|
double CMotionInfo_Gantry::GetCurTotalDist( int& nAxisType ) const
|
{
|
const CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
|
if (pNode==NULL)
|
{
|
return GetCurTotalDist_Max(nAxisType);
|
}
|
return pNode->GetCurTotalDist();
|
}
|
|
double CMotionInfo_Gantry::GetCurVelocity_Min( int& nAxisType ) const
|
{
|
nAxisType = -1;
|
double dTempValue = DBL_MAX;
|
for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
|
{
|
if (dTempValue > it->GetCurVelocity())
|
{
|
dTempValue = it->GetCurVelocity();
|
nAxisType = it->GetIndex();
|
}
|
}
|
return dTempValue;
|
}
|
|
double CMotionInfo_Gantry::GetCurAccelTime_Min( int& nAxisType ) const
|
{
|
nAxisType = -1;
|
double dTempValue = DBL_MAX;
|
for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
|
{
|
if (dTempValue > it->GetCurAccelTime())
|
{
|
dTempValue = it->GetCurAccelTime();
|
nAxisType = it->GetIndex();
|
}
|
}
|
return dTempValue;
|
}
|
|
double CMotionInfo_Gantry::GetCurDecelTime_Min( int& nAxisType ) const
|
{
|
nAxisType = -1;
|
double dTempValue = DBL_MAX;
|
for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
|
{
|
if (dTempValue > it->GetCurDecelTime())
|
{
|
dTempValue = it->GetCurDecelTime();
|
nAxisType = it->GetIndex();
|
}
|
}
|
return dTempValue;
|
}
|
|
double CMotionInfo_Gantry::GetCurFixTime_Min( int& nAxisType ) const
|
{
|
nAxisType = -1;
|
double dTempValue = DBL_MAX;
|
for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
|
{
|
if (dTempValue > it->GetCurFixTime())
|
{
|
dTempValue = it->GetCurFixTime();
|
nAxisType = it->GetIndex();
|
}
|
}
|
return dTempValue;
|
}
|
|
double CMotionInfo_Gantry::GetCurTotalTime_Min( int& nAxisType ) const
|
{
|
nAxisType = -1;
|
double dTempValue = DBL_MAX;
|
for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
|
{
|
if (dTempValue > it->GetCurTotalTime())
|
{
|
dTempValue = it->GetCurTotalTime();
|
nAxisType = it->GetIndex();
|
}
|
}
|
return dTempValue;
|
}
|
|
double CMotionInfo_Gantry::GetCurAccelDist_Min( int& nAxisType ) const
|
{
|
nAxisType = -1;
|
double dTempValue = DBL_MAX;
|
for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
|
{
|
if (dTempValue > it->GetCurAccelDist())
|
{
|
dTempValue = it->GetCurAccelDist();
|
nAxisType = it->GetIndex();
|
}
|
}
|
return dTempValue;
|
}
|
|
double CMotionInfo_Gantry::GetCurDecelDist_Min( int& nAxisType ) const
|
{
|
nAxisType = -1;
|
double dTempValue = DBL_MAX;
|
for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
|
{
|
if (dTempValue > it->GetCurDecelDist())
|
{
|
dTempValue = it->GetCurDecelDist();
|
nAxisType = it->GetIndex();
|
}
|
}
|
return dTempValue;
|
}
|
|
double CMotionInfo_Gantry::GetCurFixDist_Min( int& nAxisType ) const
|
{
|
nAxisType = -1;
|
double dTempValue = DBL_MAX;
|
for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
|
{
|
if (dTempValue > it->GetCurFixDist())
|
{
|
dTempValue = it->GetCurFixDist();
|
nAxisType = it->GetIndex();
|
}
|
}
|
return dTempValue;
|
}
|
|
double CMotionInfo_Gantry::GetCurTotalDist_Min( int& nAxisType ) const
|
{
|
nAxisType = -1;
|
double dTempValue = DBL_MAX;
|
for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
|
{
|
if (dTempValue > it->GetCurTotalDist())
|
{
|
dTempValue = it->GetCurTotalDist();
|
nAxisType = it->GetIndex();
|
}
|
}
|
return dTempValue;
|
}
|
|
double CMotionInfo_Gantry::GetCurVelocity_Max( int& nAxisType ) const
|
{
|
nAxisType = -1;
|
double dTempValue = DBL_MIN;
|
for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
|
{
|
if (dTempValue < it->GetCurVelocity())
|
{
|
dTempValue = it->GetCurVelocity();
|
nAxisType = it->GetIndex();
|
}
|
}
|
return dTempValue;
|
}
|
|
double CMotionInfo_Gantry::GetCurAccelTime_Max( int& nAxisType ) const
|
{
|
nAxisType = -1;
|
double dTempValue = DBL_MIN;
|
for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
|
{
|
if (dTempValue < it->GetCurAccelTime())
|
{
|
dTempValue = it->GetCurAccelTime();
|
nAxisType = it->GetIndex();
|
}
|
}
|
return dTempValue;
|
}
|
|
double CMotionInfo_Gantry::GetCurDecelTime_Max( int& nAxisType ) const
|
{
|
nAxisType = -1;
|
double dTempValue = DBL_MIN;
|
for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
|
{
|
if (dTempValue < it->GetCurDecelTime())
|
{
|
dTempValue = it->GetCurDecelTime();
|
nAxisType = it->GetIndex();
|
}
|
}
|
return dTempValue;
|
}
|
|
double CMotionInfo_Gantry::GetCurFixTime_Max( int& nAxisType ) const
|
{
|
nAxisType = -1;
|
double dTempValue = DBL_MIN;
|
for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
|
{
|
if (dTempValue < it->GetCurFixTime())
|
{
|
dTempValue = it->GetCurFixTime();
|
nAxisType = it->GetIndex();
|
}
|
}
|
return dTempValue;
|
}
|
|
double CMotionInfo_Gantry::GetCurTotalTime_Max( int& nAxisType ) const
|
{
|
nAxisType = -1;
|
double dTempValue = DBL_MIN;
|
for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
|
{
|
if (dTempValue < it->GetCurTotalTime())
|
{
|
dTempValue = it->GetCurTotalTime();
|
nAxisType = it->GetIndex();
|
}
|
}
|
return dTempValue;
|
}
|
|
double CMotionInfo_Gantry::GetCurAccelDist_Max( int& nAxisType ) const
|
{
|
nAxisType = -1;
|
double dTempValue = DBL_MIN;
|
for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
|
{
|
if (dTempValue < it->GetCurAccelDist())
|
{
|
dTempValue = it->GetCurAccelDist();
|
nAxisType = it->GetIndex();
|
}
|
}
|
return dTempValue;
|
}
|
|
double CMotionInfo_Gantry::GetCurDecelDist_Max( int& nAxisType ) const
|
{
|
nAxisType = -1;
|
double dTempValue = DBL_MIN;
|
for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
|
{
|
if (dTempValue < it->GetCurDecelDist())
|
{
|
dTempValue = it->GetCurDecelDist();
|
nAxisType = it->GetIndex();
|
}
|
}
|
return dTempValue;
|
}
|
|
double CMotionInfo_Gantry::GetCurFixDist_Max( int& nAxisType ) const
|
{
|
nAxisType = -1;
|
double dTempValue = DBL_MIN;
|
for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
|
{
|
if (dTempValue < it->GetCurFixDist())
|
{
|
dTempValue = it->GetCurFixDist();
|
nAxisType = it->GetIndex();
|
}
|
}
|
return dTempValue;
|
}
|
|
double CMotionInfo_Gantry::GetCurTotalDist_Max( int& nAxisType ) const
|
{
|
nAxisType = -1;
|
double dTempValue = DBL_MIN;
|
for (constVectorMotionInfo_AxisIt it=m_vecMotionInfo.begin(); it!=m_vecMotionInfo.end(); it++)
|
{
|
if (dTempValue < it->GetCurTotalDist())
|
{
|
dTempValue = it->GetCurTotalDist();
|
nAxisType = it->GetIndex();
|
}
|
}
|
return dTempValue;
|
}
|
|
void CMotionInfo_Gantry::SetData( int nAxisType, double dVelocity, double dAccelTime, double dDecelTime, double dStartPos, double dEndPos )
|
{
|
CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
|
if (pNode==NULL) return;
|
pNode->SetData(dVelocity, dAccelTime, dDecelTime, dStartPos, dEndPos);
|
}
|
|
void CMotionInfo_Gantry::SetVelocity( int nAxisType, double dVelocity )
|
{
|
CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
|
if (pNode==NULL) return;
|
pNode->SetVelocity(dVelocity);
|
}
|
|
void CMotionInfo_Gantry::SetAccelTime( int nAxisType, double dAccelTime )
|
{
|
CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
|
if (pNode==NULL) return;
|
pNode->SetAccelTime(dAccelTime);
|
}
|
|
void CMotionInfo_Gantry::SetDecelTime( int nAxisType, double dDecelTime )
|
{
|
CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
|
if (pNode==NULL) return;
|
pNode->SetDecelTime(dDecelTime);
|
}
|
|
void CMotionInfo_Gantry::SetStartPos( int nAxisType, double dStartPos )
|
{
|
CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
|
if (pNode==NULL) return;
|
pNode->SetStartPos(dStartPos);
|
}
|
|
void CMotionInfo_Gantry::SetEndPos( int nAxisType, double dEndPos )
|
{
|
CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
|
if (pNode==NULL) return;
|
pNode->SetEndPos(dEndPos);
|
}
|
|
int CMotionInfo_Gantry::CalculateMotion( int nAxisType, double dTime, double& dPos, double& dVelocity, BOOL& bEnd )
|
{
|
CMotionInfo_Axis* pNode = GetMotionInfo_Axis(nAxisType);
|
if (pNode==NULL) return 0;
|
return pNode->CalculateMotion(dTime, dPos, dVelocity, bEnd);
|
}
|
|
int CMotionInfo_Gantry::CalculateMotion( double dTime, VectorDouble& vecPos, VectorDouble& vecVelocity, VectorBOOL& vecEnd )
|
{
|
double dPos, dVelocity; BOOL bEnd;
|
|
vecPos.clear(); vecVelocity.clear(); vecEnd.clear();
|
|
int nReturn = 1;
|
for (int nAxisIdx=0; nAxisIdx<GetMotionInfo_AxisCount(); nAxisIdx++)
|
{
|
bEnd = FALSE;
|
dPos = dVelocity = 0.0;
|
|
nReturn = nReturn * CalculateMotion(nAxisIdx, dTime, dPos, dVelocity, bEnd);
|
|
vecPos.push_back(dPos);
|
vecVelocity.push_back(dVelocity);
|
vecEnd.push_back(bEnd);
|
}
|
|
return nReturn;
|
}
|
