#include "StdAfx.h"
|
#include "MotionInfo_Axis.h"
|
#include <math.h>
|
|
CMotionInfo_Axis::CMotionInfo_Axis(int nIndex)
|
{
|
memset(this, 0, sizeof(CMotionInfo_Axis));
|
|
m_nCurStatus = 1;
|
m_nIndex = nIndex;
|
}
|
|
CMotionInfo_Axis::~CMotionInfo_Axis()
|
{
|
|
}
|
|
// getter
|
int CMotionInfo_Axis::GetIndex() const
|
{
|
return m_nIndex;
|
}
|
|
double CMotionInfo_Axis::GetMoveVelocity() const
|
{
|
return m_dMoveVelocity;
|
}
|
|
double CMotionInfo_Axis::GetMoveAccelTime() const
|
{
|
return m_dMoveAccelTime;
|
}
|
|
double CMotionInfo_Axis::GetMoveDecelTime() const
|
{
|
return m_dMoveDecelTime;
|
}
|
|
double CMotionInfo_Axis::GetMoveStartPos() const
|
{
|
return m_dMoveStartPos;
|
}
|
|
double CMotionInfo_Axis::GetMoveEndPos() const
|
{
|
return m_dMoveEndPos;
|
}
|
|
double CMotionInfo_Axis::GetJogVelocity() const
|
{
|
return m_dJogVelocity;
|
}
|
|
double CMotionInfo_Axis::GetJogAccelTime() const
|
{
|
return m_dJogAccelTime;
|
}
|
|
double CMotionInfo_Axis::GetJogDecelTime() const
|
{
|
return m_dJogDecelTime;
|
}
|
|
double CMotionInfo_Axis::GetJogStartPos() const
|
{
|
return m_dJogStartPos;
|
}
|
|
int CMotionInfo_Axis::GetMoveDirection() const
|
{
|
return m_nMoveDirection;
|
}
|
|
double CMotionInfo_Axis::GetCurVelocity() const
|
{
|
return (m_nMoveDirection==0) ? 0: m_dCurVelocity;
|
}
|
|
double CMotionInfo_Axis::GetCurAccelTime() const
|
{
|
return (m_nMoveDirection==0) ? 0: m_dCurAccelTime;
|
}
|
|
double CMotionInfo_Axis::GetCurDecelTime() const
|
{
|
return (m_nMoveDirection==0) ? 0: m_dCurDecelTime;
|
}
|
|
double CMotionInfo_Axis::GetCurFixTime() const
|
{
|
return (m_nMoveDirection==0) ? 0: m_dCurFixTime;
|
}
|
|
double CMotionInfo_Axis::GetCurTotalTime() const
|
{
|
return (m_nMoveDirection==0) ? 0: m_dCurTotalTime;
|
}
|
|
double CMotionInfo_Axis::GetCurAccelDist() const
|
{
|
return (m_nMoveDirection==0) ? 0: m_dCurAccelDist;
|
}
|
|
double CMotionInfo_Axis::GetCurDecelDist() const
|
{
|
return (m_nMoveDirection==0) ? 0: m_dCurDecelDist;
|
}
|
|
double CMotionInfo_Axis::GetCurFixDist() const
|
{
|
return (m_nMoveDirection==0) ? 0: m_dCurFixDist;
|
}
|
|
double CMotionInfo_Axis::GetCurTotalDist() const
|
{
|
return (m_nMoveDirection==0) ? 0: m_dCurTotalDist;
|
}
|
|
// setter
|
void CMotionInfo_Axis::SetMoveVelocity(double dVelocity)
|
{
|
if (dVelocity!=m_dMoveVelocity)
|
{
|
m_dMoveVelocity = dVelocity;
|
CalculateMoveValue();
|
}
|
}
|
|
void CMotionInfo_Axis::SetMoveAccelTime(double dAccelTime)
|
{
|
if (dAccelTime!=m_dMoveAccelTime)
|
{
|
m_dMoveAccelTime = dAccelTime;
|
CalculateMoveValue();
|
}
|
}
|
|
void CMotionInfo_Axis::SetMoveDecelTime(double dDecelTime)
|
{
|
if (dDecelTime!=m_dMoveDecelTime)
|
{
|
m_dMoveDecelTime = dDecelTime;
|
CalculateMoveValue();
|
}
|
}
|
|
void CMotionInfo_Axis::SetMoveStartPos(double dStartPos)
|
{
|
if (dStartPos!=m_dMoveStartPos)
|
{
|
m_dMoveStartPos = dStartPos;
|
CalculateMoveValue();
|
}
|
}
|
|
void CMotionInfo_Axis::SetMoveEndPos(double dEndPos)
|
{
|
if (dEndPos!=m_dMoveEndPos)
|
{
|
m_dMoveEndPos = dEndPos;
|
CalculateMoveValue();
|
}
|
}
|
|
void CMotionInfo_Axis::SetMoveData(double dVelocity, double dAccelTime, double dDecelTime, double dStartPos, double dEndPos)
|
{
|
if (dVelocity!=m_dMoveVelocity)
|
{
|
m_dMoveVelocity = dVelocity;
|
m_nMoveDirection = 0;
|
}
|
|
if (dAccelTime!=m_dMoveAccelTime)
|
{
|
m_dMoveAccelTime = dAccelTime;
|
m_nMoveDirection = 0;
|
}
|
|
if (dDecelTime!=m_dMoveDecelTime)
|
{
|
m_dMoveDecelTime = dDecelTime;
|
m_nMoveDirection = 0;
|
}
|
|
if (dStartPos!=m_dMoveStartPos)
|
{
|
m_dMoveStartPos = dStartPos;
|
m_nMoveDirection = 0;
|
}
|
|
if (dEndPos!=m_dMoveEndPos)
|
{
|
m_dMoveEndPos = dEndPos;
|
m_nMoveDirection = 0;
|
}
|
|
if (m_nMoveDirection==0) CalculateMoveValue();
|
}
|
|
void CMotionInfo_Axis::SetIndex( int nIndex )
|
{
|
m_nIndex = nIndex;
|
}
|
|
void CMotionInfo_Axis::SetJogVelocity( double dVelocity )
|
{
|
if (dVelocity!=m_dJogVelocity)
|
{
|
m_dJogVelocity = dVelocity;
|
CalculateJogValue();
|
}
|
}
|
|
void CMotionInfo_Axis::SetJogAccelTime( double dAccelTime )
|
{
|
if (dAccelTime!=m_dJogAccelTime)
|
{
|
m_dJogAccelTime = dAccelTime;
|
CalculateJogValue();
|
}
|
}
|
|
void CMotionInfo_Axis::SetJogDecelTime( double dDecelTime )
|
{
|
if (dDecelTime!=m_dJogDecelTime)
|
{
|
m_dJogDecelTime = dDecelTime;
|
CalculateJogValue();
|
}
|
}
|
|
void CMotionInfo_Axis::SetJogStartPos( double dStartPos )
|
{
|
if (dStartPos!=m_dJogStartPos)
|
{
|
m_dJogStartPos = dStartPos;
|
CalculateJogValue();
|
}
|
}
|
|
void CMotionInfo_Axis::SetJogData( double dVelocity, double dAccelTime, double dDecelTime, double dStartPos)
|
{
|
bool bChange = false;
|
if (dVelocity!=m_dJogVelocity)
|
{
|
m_dJogVelocity = dVelocity;
|
bChange = true;
|
}
|
|
if (dAccelTime!=m_dJogAccelTime)
|
{
|
m_dJogAccelTime = dAccelTime;
|
bChange = true;
|
}
|
|
if (dDecelTime!=m_dJogDecelTime)
|
{
|
m_dJogDecelTime = dDecelTime;
|
bChange = true;
|
}
|
|
if (dStartPos!=m_dJogStartPos)
|
{
|
m_dJogStartPos = dStartPos;
|
bChange = true;
|
}
|
|
CalculateJogValue();
|
}
|
|
|
int CMotionInfo_Axis::CalculateMoveMotion(double dTime, double& dPos, double& dVelocity, UINT& nStatus)
|
{
|
// return : 1 °è»ê¼º°ø, 0 °è»ê½ÇÆÐ, -1 ½Ã°£¿À¹ö
|
// [in] dTime : ¼Ò¿ä½Ã°£ (sec)
|
// [out] dPos : ÇöÀçÀ§Ä¡ (mm)
|
// [out] dSpeed : ÇöÀç ¼Óµµ (mm/sec)
|
// [out] bEnd : ¿Ï·áÀ¯¹«
|
if (m_nIndex==-1) return 0;
|
|
if (m_nMoveDirection==0) // is not calculated
|
{
|
if (CalculateMoveValue()==0)
|
{
|
m_dCurPosition = m_dMoveStartPos;
|
m_nCurStatus = 1;
|
return 0;
|
}
|
}
|
|
nStatus = 0;
|
if (m_dCurTotalDist==0.0) // À̵¿°Å¸®°¡ '0' À̸é
|
{
|
dPos = m_dCurPosition = m_dMoveStartPos;
|
nStatus = m_nCurStatus = 1;
|
dVelocity = 0.0;
|
return 1;
|
}
|
|
// 1. ±¸°£º° °è»ê
|
if (dTime < m_dCurAccelTime) // [°¡¼Ó±¸°£]
|
{
|
double dA = (m_dCurVelocity / m_dCurAccelTime);
|
dVelocity = (dA * dTime); // ¼Óµµ
|
dPos = (dVelocity * dTime) / 2.0; // À§Ä¡
|
}
|
else if (dTime < (m_dCurAccelTime + m_dCurFixTime)) // [Á¤¼Ó±¸°£]
|
{
|
dTime = dTime - m_dCurAccelTime;
|
dVelocity = m_dCurVelocity; // ¼Óµµ
|
dPos = m_dCurAccelDist + (dTime * dVelocity); // À§Ä¡
|
}
|
else if (dTime < m_dCurTotalTime) // [°¨¼Ó±¸°£]
|
{
|
double dA = (m_dCurVelocity / m_dCurDecelTime);
|
dTime = dTime - (m_dCurAccelTime + m_dCurFixTime);
|
dVelocity = m_dCurVelocity - (dA * dTime); // ¼Óµµ
|
double dV = (m_dCurVelocity-dVelocity);
|
dPos = m_dCurAccelDist + m_dCurFixDist + ((dVelocity*dTime) + (dV*dTime)/2.0); // À§Ä¡
|
|
}
|
else // [À̵¿¿Ï·á]
|
{
|
dPos = m_dCurPosition =m_dMoveEndPos;
|
nStatus = m_nCurStatus = 1;
|
dVelocity = 0.0;
|
return 1;
|
}
|
|
// set direction
|
dPos = m_dCurPosition = m_dMoveStartPos + (dPos * m_nMoveDirection);
|
nStatus = m_nCurStatus = 0;
|
|
return 1;
|
}
|
|
int CMotionInfo_Axis::CalculateMoveValue()
|
{
|
m_nMoveDirection = 0;
|
|
if (m_dMoveVelocity<0.0) return 0;
|
|
m_dCurTotalDist = (m_dMoveEndPos-m_dMoveStartPos); // ÃÑÀ̵¿°Å¸®
|
|
if (m_dCurTotalDist==0.0) // À̵¿°Å¸®°¡ 0 À̸é
|
{
|
return 1;
|
}
|
|
// 0. °ü·Ã º¯¼ö °è»ê
|
int nMoveDirection = (m_dCurTotalDist>0.0) ? 1: -1; // À̵¿¹æÇâ
|
|
// 1.ÇöÀç °¡¼Ó ½Ã°£ ¹× °Å¸®
|
m_dCurAccelTime = m_dMoveAccelTime;
|
m_dCurAccelDist = 0.0; // °¡¼Ó À̵¿°Å¸®
|
if (m_dCurAccelTime!=0.0) // °¡¼Ó ½Ã°£ÀÌ 0 ÀÌ ¾Æ´Ï¸é
|
{
|
m_dCurAccelDist = (m_dCurAccelTime * m_dMoveVelocity) / 2.0; // °¡¼Ó½Ã À̵¿°Å¸®
|
}
|
|
// 2.ÇöÀç °¨¼Ó ½Ã°£ ¹× °Å¸®
|
m_dCurDecelTime = m_dMoveDecelTime;
|
m_dCurDecelDist = 0.0; // °¨¼Ó À̵¿°Å¸®
|
if (m_dCurDecelTime!=0.0) // °¨¼Ó ½Ã°£ÀÌ 0 ÀÌ ¾Æ´Ï¸é
|
{
|
m_dCurDecelDist = (m_dCurDecelTime * m_dMoveVelocity) / 2.0; // °¨¼Ó À̵¿°Å¸®
|
}
|
|
// ÇöÀç °¡°¨¼Ó ½Ã°£ ¹× °Å¸® Àç°è»ê
|
m_dCurVelocity = m_dMoveVelocity;
|
if ( (nMoveDirection*m_dCurTotalDist) < (m_dCurAccelDist+m_dCurDecelDist) ) // °¡°¨¼Ó½Ã À̵¿°Å¸®°¡ ÃÑ À̵¿°Å¸®º¸´Ù Ŭ¶§
|
{
|
double dA = (m_dMoveVelocity / m_dCurAccelTime);
|
|
m_dCurAccelDist = m_dCurDecelDist = ((nMoveDirection*m_dCurTotalDist) / 2.0);
|
|
m_dCurAccelTime = sqrt( (2.0*m_dCurAccelDist) / dA );
|
m_dCurDecelTime = sqrt( (2.0*m_dCurDecelDist) / dA );
|
|
m_dCurVelocity = (2.0*m_dCurAccelDist) / m_dCurAccelTime;
|
}
|
|
// 3.ÇöÀç µî¼Ó ½Ã°£ ¹× °Å¸®
|
m_dCurFixTime = 0.0;
|
m_dCurFixDist = (nMoveDirection*m_dCurTotalDist) - (m_dCurAccelDist+m_dCurDecelDist); // Á¤¼Ó½Ã À̵¿°Å¸® °è»ê
|
if (m_dCurFixDist!=0.0) // Á¤¼Ó½Ã À̵¿°Å¸®°¡ 0 ÀÌ ¾Æ´Ò¶§
|
{
|
m_dCurFixTime = m_dCurFixDist / m_dMoveVelocity; // Á¤¼Ó½Ã À̵¿½Ã°£
|
}
|
|
// 4. ÃÑ À̵¿ ½Ã°£.
|
m_dCurTotalTime = m_dCurAccelTime + m_dCurFixTime + m_dCurDecelTime; // ÃÑ À̵¿½Ã°£
|
|
m_nMoveDirection = nMoveDirection;
|
|
return 1;
|
}
|
|
int CMotionInfo_Axis::CalculateJogMotion( double dTime, int nJogDir, double& dPos, double& dVelocity, UINT& nStatus )
|
{
|
// return : 1 °è»ê¼º°ø, 0 °è»ê½ÇÆÐ, -1 ½Ã°£¿À¹ö
|
// [in] dTime : ¼Ò¿ä½Ã°£ (sec)
|
// [out] dPos : ÇöÀçÀ§Ä¡ (mm)
|
// [out] dSpeed : ÇöÀç ¼Óµµ (mm/sec)
|
// [out] bEnd : ¿Ï·áÀ¯¹«
|
if (m_nIndex==-1) return 0;
|
|
nStatus = 0;
|
if (nJogDir==0)
|
{
|
dPos = m_dCurPosition;
|
nStatus = m_nCurStatus = 1;
|
dVelocity = 0.0;
|
return 1;
|
}
|
|
// 1. ±¸°£º° °è»ê
|
if (dTime < m_dCurAccelTime) // [°¡¼Ó±¸°£]
|
{
|
double dA = (m_dCurVelocity / m_dCurAccelTime);
|
dVelocity = (dA * dTime); // ¼Óµµ
|
dPos = (dVelocity * dTime) / 2.0; // À§Ä¡
|
}
|
else // [Á¤¼Ó±¸°£]
|
{
|
dTime = dTime - m_dCurAccelTime;
|
dVelocity = m_dCurVelocity; // ¼Óµµ
|
dPos = m_dCurAccelDist + (dTime * dVelocity); // À§Ä¡
|
}
|
|
dPos = m_dCurPosition = m_dJogStartPos + (dPos * nJogDir); // set direction
|
nStatus = m_nCurStatus = 0;
|
|
return 1;
|
}
|
|
int CMotionInfo_Axis::CalculateJogValue()
|
{
|
if (m_dJogVelocity<0.0) return 0;
|
|
// 1.ÇöÀç °¡¼Ó ½Ã°£ ¹× °Å¸®
|
m_dCurAccelTime = m_dJogAccelTime;
|
m_dCurAccelDist = 0.0; // °¡¼Ó À̵¿°Å¸®
|
if (m_dCurAccelTime!=0.0) // °¡¼Ó ½Ã°£ÀÌ 0 ÀÌ ¾Æ´Ï¸é
|
{
|
m_dCurAccelDist = (m_dCurAccelTime * m_dJogVelocity) / 2.0; // °¡¼Ó½Ã À̵¿°Å¸®
|
}
|
|
// 2.ÇöÀç °¨¼Ó ½Ã°£ ¹× °Å¸®
|
m_dCurDecelTime = m_dJogDecelTime;
|
m_dCurDecelDist = 0.0; // °¨¼Ó À̵¿°Å¸®
|
if (m_dCurDecelTime!=0.0) // °¨¼Ó ½Ã°£ÀÌ 0 ÀÌ ¾Æ´Ï¸é
|
{
|
m_dCurDecelDist = (m_dCurDecelTime * m_dJogVelocity) / 2.0; // °¨¼Ó À̵¿°Å¸®
|
}
|
|
// ÇöÀç °¡°¨¼Ó ½Ã°£ ¹× °Å¸® Àç°è»ê
|
m_dCurVelocity = m_dJogVelocity;
|
|
return 1;
|
}
|
|
UINT CMotionInfo_Axis::GetCurStatus() const
|
{
|
return m_nCurStatus;
|
}
|
|
double CMotionInfo_Axis::GetCurPosition() const
|
{
|
return m_dCurPosition;
|
}
|
|
int CMotionInfo_Axis::GetEmergencyStop() const
|
{
|
return m_nEmergencyStop;
|
}
|
|
void CMotionInfo_Axis::SetEmergencyStop(int nValue)
|
{
|
m_nEmergencyStop = nValue;
|
}
|
