#include "StdAfx.h"
|
#include "MotionInfo_Axis.h"
|
#include <math.h>
|
|
CMotionInfo_Axis::CMotionInfo_Axis(int nIndex)
|
{
|
memset(this, 0, sizeof(CMotionInfo_Axis));
|
m_nIndex = nIndex;
|
}
|
|
CMotionInfo_Axis::~CMotionInfo_Axis()
|
{
|
|
}
|
|
// getter
|
int CMotionInfo_Axis::GetIndex() const
|
{
|
return m_nIndex;
|
}
|
|
double CMotionInfo_Axis::GetVelocity() const
|
{
|
return m_dVelocity;
|
}
|
|
double CMotionInfo_Axis::GetAccelTime() const
|
{
|
return m_dAccelTime;
|
}
|
|
double CMotionInfo_Axis::GetDecelTime() const
|
{
|
return m_dDecelTime;
|
}
|
|
double CMotionInfo_Axis::GetStartPos() const
|
{
|
return m_dStartPos;
|
}
|
|
double CMotionInfo_Axis::GetEndPos() const
|
{
|
return m_dEndPos;
|
}
|
|
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::SetVelocity(double dVelocity)
|
{
|
if (dVelocity!=m_dVelocity)
|
{
|
m_dVelocity = dVelocity;
|
CalculateValue();
|
}
|
}
|
|
void CMotionInfo_Axis::SetAccelTime(double dAccelTime)
|
{
|
if (dAccelTime!=m_dVelocity)
|
{
|
m_dAccelTime = dAccelTime;
|
CalculateValue();
|
}
|
}
|
|
void CMotionInfo_Axis::SetDecelTime(double dDecelTime)
|
{
|
if (dDecelTime!=m_dVelocity)
|
{
|
m_dDecelTime = dDecelTime;
|
CalculateValue();
|
}
|
}
|
|
void CMotionInfo_Axis::SetStartPos(double dStartPos)
|
{
|
if (dStartPos!=m_dVelocity)
|
{
|
m_dStartPos = dStartPos;
|
CalculateValue();
|
}
|
}
|
|
void CMotionInfo_Axis::SetEndPos(double dEndPos)
|
{
|
if (dEndPos!=m_dVelocity)
|
{
|
m_dEndPos = dEndPos;
|
CalculateValue();
|
}
|
}
|
|
void CMotionInfo_Axis::SetData(double dVelocity, double dAccelTime, double dDecelTime, double dStartPos, double dEndPos)
|
{
|
if (dVelocity!=m_dVelocity)
|
{
|
m_dVelocity = dVelocity;
|
m_nMoveDirection = 0;
|
}
|
|
if (dAccelTime!=m_dVelocity)
|
{
|
m_dAccelTime = dAccelTime;
|
m_nMoveDirection = 0;
|
}
|
|
if (dDecelTime!=m_dVelocity)
|
{
|
m_dDecelTime = dDecelTime;
|
m_nMoveDirection = 0;
|
}
|
|
if (dStartPos!=m_dVelocity)
|
{
|
m_dStartPos = dStartPos;
|
m_nMoveDirection = 0;
|
}
|
|
if (dEndPos!=m_dVelocity)
|
{
|
m_dEndPos = dEndPos;
|
m_nMoveDirection = 0;
|
}
|
|
if (m_nMoveDirection==0) CalculateValue();
|
}
|
|
int CMotionInfo_Axis::CalculateMotion(double dTime, double& dPos, double& dVelocity, BOOL& bEnd)
|
{
|
// 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 (CalculateValue()==0)
|
{
|
return 0;
|
}
|
}
|
|
if (m_dCurTotalDist==0.0) // À̵¿°Å¸®°¡ '0' À̸é
|
{
|
dPos = m_dStartPos;
|
dVelocity = 0.0;
|
bEnd = TRUE;
|
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_dEndPos;
|
dVelocity = 0.0;
|
bEnd = TRUE;
|
return 1;
|
}
|
|
dPos *= m_nMoveDirection; // set direction
|
|
return 1;
|
}
|
|
int CMotionInfo_Axis::CalculateValue()
|
{
|
m_nMoveDirection = 0;
|
|
if (m_dVelocity<0.0) return 0;
|
|
m_dCurTotalDist = (m_dEndPos-m_dStartPos); // ÃÑÀ̵¿°Å¸®
|
|
if (m_dCurTotalDist==0.0) // À̵¿°Å¸®°¡ 0 À̸é
|
{
|
return 1;
|
}
|
|
// 0. °ü·Ã º¯¼ö °è»ê
|
int nMoveDirection = (m_dCurTotalDist>0.0) ? 1: -1; // À̵¿¹æÇâ
|
|
// 1.ÇöÀç °¡¼Ó ½Ã°£ ¹× °Å¸®
|
m_dCurAccelTime = m_dAccelTime;
|
m_dCurAccelDist = 0.0; // °¡¼Ó À̵¿°Å¸®
|
if (m_dCurAccelTime!=0.0) // °¡¼Ó ½Ã°£ÀÌ 0 ÀÌ ¾Æ´Ï¸é
|
{
|
m_dCurAccelDist = (m_dCurAccelTime * m_dVelocity) / 2.0; // °¡¼Ó½Ã À̵¿°Å¸®
|
}
|
|
// 2.ÇöÀç °¨¼Ó ½Ã°£ ¹× °Å¸®
|
m_dCurDecelTime = m_dDecelTime;
|
m_dCurDecelDist = 0.0; // °¨¼Ó À̵¿°Å¸®
|
if (m_dCurDecelTime!=0.0) // °¨¼Ó ½Ã°£ÀÌ 0 ÀÌ ¾Æ´Ï¸é
|
{
|
m_dCurDecelDist = (m_dCurDecelTime * m_dVelocity) / 2.0; // °¨¼Ó À̵¿°Å¸®
|
}
|
|
// ÇöÀç °¡°¨¼Ó ½Ã°£ ¹× °Å¸® Àç°è»ê
|
m_dCurVelocity = m_dVelocity;
|
if ( (nMoveDirection*m_dCurTotalDist) < (m_dCurAccelDist+m_dCurDecelDist) ) // °¡°¨¼Ó½Ã À̵¿°Å¸®°¡ ÃÑ À̵¿°Å¸®º¸´Ù Ŭ¶§
|
{
|
double dA = (m_dVelocity / 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_dVelocity; // Á¤¼Ó½Ã À̵¿½Ã°£
|
}
|
|
// 4. ÃÑ À̵¿ ½Ã°£.
|
m_dCurTotalTime = m_dCurAccelTime + m_dCurFixTime + m_dCurDecelTime; // ÃÑ À̵¿½Ã°£
|
|
m_nMoveDirection = nMoveDirection;
|
|
return 1;
|
}
|
