#include "StdAfx.h"
|
#include "PathScheduler_Dual_Flying.h"
|
#include "MotionInfo_Axis.h"
|
#include <algorithm>
|
|
CPathScheduler_Dual_Flying::CPathScheduler_Dual_Flying(void)
|
{
|
}
|
|
|
CPathScheduler_Dual_Flying::~CPathScheduler_Dual_Flying(void)
|
{
|
}
|
|
inline bool ComparePosAscend(SPathData& a, SPathData& b)
|
{
|
return (a.nPosX < b.nPosX);
|
}
|
|
inline bool ComparePosDescend(SPathData& a, SPathData& b)
|
{
|
return (a.nPosX > b.nPosX);
|
}
|
|
int CPathScheduler_Dual_Flying::CalculatePath(int nModuleIdx, int nScanIdx, const SPathData& startPath, const VectorPathData& vecTotalPathData, const SRangeData& rtRange, CPathSchedulerResult& scheduleResult )
|
{
|
scheduleResult.Reset();
|
|
CRect rtTemp(rtRange.nLeft, rtRange.nTop, rtRange.nRight, rtRange.nBottom);
|
|
// get path data
|
VectorPathData vecPathData;
|
for (constVectorPathDataIt it=vecTotalPathData.begin(); it!=vecTotalPathData.end(); it++)
|
{
|
if ( rtTemp.PtInRect( CPoint(it->nPosX,it->nPosY )) )
|
{
|
vecPathData.push_back(*it);
|
}
|
}
|
|
// sort for scan direction
|
if (rtRange.nScanDir==0)
|
{
|
std::sort(vecPathData.begin(), vecPathData.end(), ComparePosAscend);
|
}
|
else
|
{
|
std::sort(vecPathData.begin(), vecPathData.end(), ComparePosDescend);
|
}
|
|
const SMotionInfo* pAxisX = m_PathSchedulerParam.GetScanMotionInfo();
|
const SMotionInfo* pAxisY = m_PathSchedulerParam.GetModuleMotionInfo();
|
if (pAxisX==NULL || pAxisY==NULL)
|
{
|
return 0;
|
}
|
|
// ½ºÄÉÁ층
|
CMotionInfo_Axis axisX(0);
|
CMotionInfo_Axis axisY(1);
|
|
axisX.SetVelocity(pAxisX->dMoveSpeed);
|
axisX.SetAccelTime(pAxisX->dAccelTime);
|
axisX.SetDecelTime(pAxisX->dDecelTime);
|
|
axisY.SetVelocity(pAxisY->dMoveSpeed);
|
axisY.SetAccelTime(pAxisY->dAccelTime);
|
axisY.SetDecelTime(pAxisY->dDecelTime);
|
|
// set result count
|
scheduleResult.SetScheduleResultCount((int)vecPathData.size()+1);
|
|
FILE *fp = NULL;
|
CString strFilename = _T("");
|
strFilename.Format(_T("c:\\temp\\%d_%d.csv"), nModuleIdx, nScanIdx);
|
_tfopen_s(&fp, strFilename, _T("w"));
|
if (fp)
|
{
|
_ftprintf_s(fp, _T("[Left], [Right], [Top], [Bottom], [ScanStart], [ScanEnd], [ScanDir]\n"));
|
_ftprintf_s(fp, _T("%d, %d, %d, %d, %d, %d, %d\n"),
|
rtRange.nLeft,
|
rtRange.nRight,
|
rtRange.nTop,
|
rtRange.nBottom,
|
rtRange.nScanStartX,
|
rtRange.nScanEndX,
|
rtRange.nScanDir);
|
_ftprintf_s(fp, _T("[Index], [PrevX], [PrevY], [NextX], [NextY]\n"));
|
}
|
|
|
// add start pos
|
int nSetCount = 0;
|
|
// calculate path
|
SPathData prevPos = startPath;
|
for (int nIdx=0; nIdx<(int)vecPathData.size(); nIdx++)
|
{
|
axisX.SetStartPos(prevPos.nPosX/1000.0);
|
axisX.SetEndPos(vecPathData[nIdx].nPosX/1000.0);
|
|
axisY.SetStartPos(prevPos.nPosY/1000.0);
|
axisY.SetEndPos(vecPathData[nIdx].nPosY/1000.0);
|
|
double x_time = axisX.GetCurTotalTime();
|
double y_time = axisY.GetCurTotalTime();
|
if (x_time > (y_time+pAxisY->dInpositionTime)) // check motion time
|
{
|
if (fp)
|
_ftprintf_s(fp, _T("%d, %d, %d, %d, %d\n"), nSetCount, prevPos.nPosX, prevPos.nPosY, vecPathData[nIdx].nPosX, vecPathData[nIdx].nPosY);
|
|
prevPos = vecPathData[nIdx];
|
SSchedulerResult sResult;
|
sResult.nTotalIndex = prevPos.nIndex;
|
sResult.dPositionX = double(prevPos.nPosX) / 1000.0;
|
sResult.dPositionY = double(prevPos.nPosY) / 1000.0;
|
|
memcpy(sResult.pDataType, prevPos.pDataType, sizeof(int)*PATH_DATA_TYPE_COUNT);
|
|
scheduleResult.SetScheduleResult(nSetCount, sResult);
|
nSetCount++;
|
}
|
}
|
|
if (fp)
|
fclose(fp);
|
|
// resize result count;
|
scheduleResult.ResizeScheduleResult(nSetCount);
|
|
return scheduleResult.GetPathSchedulerResultCount();
|
}
|
|
int CPathScheduler_Dual_Flying::GetDirectionX(int nDir1, int nDir2)
|
{
|
//SchedulerDir_LeftTop=0, SchedulerDir_RightTop, SchedulerDir_LeftBottom, SchedulerDir_RightBottom
|
|
// Dir1 == Left
|
if (nDir1==SchedulerDir_LeftTop || nDir1==SchedulerDir_LeftBottom)
|
{
|
if (nDir2==SchedulerDir_LeftTop || nDir2==SchedulerDir_LeftBottom)
|
{
|
return 1;
|
}
|
else
|
{
|
return -1;
|
}
|
}
|
|
// Dir1 == Right
|
if (nDir2==SchedulerDir_RightTop || nDir2==SchedulerDir_RightBottom)
|
{
|
return 1;
|
}
|
|
return -1;
|
}
|
|
int CPathScheduler_Dual_Flying::GetDirectionY(int nDir1, int nDir2)
|
{
|
//SchedulerDir_LeftTop=0, SchedulerDir_RightTop, SchedulerDir_LeftBottom, SchedulerDir_RightBottom
|
|
// Dir1 == Top
|
if (nDir1==SchedulerDir_LeftTop || nDir1==SchedulerDir_RightTop)
|
{
|
if (nDir2==SchedulerDir_LeftTop || nDir2==SchedulerDir_RightTop)
|
{
|
return 1;
|
}
|
else
|
{
|
return -1;
|
}
|
}
|
|
// Dir1 == Bottom
|
if (nDir2==SchedulerDir_LeftBottom || nDir2==SchedulerDir_RightBottom)
|
{
|
return 1;
|
}
|
|
return -1;
|
}
|
|
|
int CPathScheduler_Dual_Flying::IsChangeModuleIndex(int nDir1, int nDir2)
|
{
|
return (nDir1!=nDir2);
|
}
|
|
int CPathScheduler_Dual_Flying::IsChangeScanIndex(int nDir1, int nDir2)
|
{
|
if (GetDirectionY(nDir1, nDir2)==-1) return 1;
|
return 0;
|
}
|
|
int CPathScheduler_Dual_Flying::IsChangeScanDir(int nDir1, int nDir2)
|
{
|
if (GetDirectionX(nDir1, nDir2)==-1) return 1;
|
return 0;
|
}
|
|
int CPathScheduler_Dual_Flying::GetChangeModuleIndex(int nModuleIndex, int nOriginDir, int nBaseDir, int nGantryModuleCount, int nTotalModuleCount)
|
{
|
int nTmpValue = nModuleIndex;
|
|
if (GetDirectionX(nOriginDir, nBaseDir)==-1) // x ¿ª¹æÇâ
|
{
|
if (nTmpValue<nGantryModuleCount)
|
{
|
nTmpValue = nTmpValue + nGantryModuleCount;
|
}
|
else
|
{
|
nTmpValue = nTmpValue - nGantryModuleCount;
|
}
|
}
|
|
if (GetDirectionY(nOriginDir, nBaseDir)==-1) // y ¿ª¹æÇâ
|
{
|
if (nTmpValue<nGantryModuleCount)
|
{
|
nTmpValue = (nGantryModuleCount - 1) - nTmpValue;
|
}
|
else
|
{
|
nTmpValue = nTotalModuleCount + (nGantryModuleCount - 1) - nTmpValue;
|
}
|
}
|
|
return nTmpValue;
|
}
|
|
int CPathScheduler_Dual_Flying::GetChangeScanIndex(int nScanIndex, int nTotalScanCount)
|
{
|
return (nTotalScanCount - 1) - nScanIndex;
|
}
|
|
int CPathScheduler_Dual_Flying::GetChangeScanDir(int nScanDir)
|
{
|
return !nScanDir;
|
}
|
|
int CPathScheduler_Dual_Flying::CalculateRangeRect(int nGantryCount, int nModuleCount, int nScanCount, int nGlassSizeX, int nGlassSizeY, int nScanMargin, int nOriginDir, int nBaseDir, VectorRangeData& vecRangeData)
|
{
|
int nGlassDivX = nGlassSizeX / nGantryCount;
|
int nGantryModuleCount = nModuleCount / nGantryCount;
|
int nGlassDivModuleY = nGlassSizeY / nGantryModuleCount;
|
int nGlassDivScanY = nGlassDivModuleY / nScanCount;
|
|
SRangeData tmpRange;
|
|
// x axis
|
int nTotalModuleIdx = 0;
|
for (int nGantryIdx=0; nGantryIdx<nGantryCount; nGantryIdx++)
|
{
|
// scan x range
|
tmpRange.nLeft = nGantryIdx * nGlassDivX;
|
tmpRange.nRight = tmpRange.nLeft + nGlassDivX;
|
|
for (int nModuleIdx=0; nModuleIdx<nGantryModuleCount; nModuleIdx++)
|
{
|
// module y range
|
int nModuleStart = nModuleIdx * nGlassDivModuleY;
|
|
for (int nScanIdx = 0; nScanIdx<nScanCount; nScanIdx++)
|
{
|
// scan y range
|
tmpRange.nTop = nModuleStart + (nScanIdx*nGlassDivScanY);
|
tmpRange.nBottom = tmpRange.nTop + nGlassDivScanY;
|
|
// ½ºÄµ y ½ÃÀÛÀ§Ä¡ ±âº»°ªÀº ¿µ¿ªÀÇ Áß¾ÓÀÓ.
|
// tmpRange.nScanStartY = tmpRange.nTop + ((tmpRange.nBottom - tmpRange.nTop) / 2);
|
tmpRange.nModuleIdx = nTotalModuleIdx;
|
tmpRange.nScanIdx = nScanIdx;
|
tmpRange.nScanDir = (nScanIdx % 2);
|
|
if (tmpRange.nScanDir==0) // Á¤¹æÇâ ½ºÄµ
|
{
|
tmpRange.nScanStartX = tmpRange.nLeft - nScanMargin; // ½ºÄµ ½ÃÀÛÀ§Ä¡
|
tmpRange.nScanEndX = tmpRange.nRight + nScanMargin; // ½ºÄµ Á¾·áÀ§Ä¡
|
}
|
else // ¿ª¹æÇâ ½ºÄµ
|
{
|
tmpRange.nScanStartX = tmpRange.nRight + nScanMargin; // ½ºÄµ ½ÃÀÛÀ§Ä¡
|
tmpRange.nScanEndX = tmpRange.nLeft - nScanMargin; // ½ºÄµ Á¾·áÀ§Ä¡
|
}
|
|
vecRangeData.push_back(tmpRange);
|
}
|
|
nTotalModuleIdx++;
|
}
|
}
|
|
// change range data (origin_dir / base_dir)
|
|
// int nModuleIdx; // module index
|
// int nScanIdx; // scan index
|
// int nScanDir; // scan dir 0/1
|
//
|
// int nScanStartX; // scan start x
|
// int nScanStartY; // scan start y
|
// int nScanEndX; // scan end x
|
//
|
// int nLeft; // range left
|
// int nTop; // range top
|
// int nRight; // range right
|
// int nBottom; // range bottom
|
|
VectorRangeData vecResult;
|
|
for (int nModuleIdx=0; nModuleIdx<nModuleCount; nModuleIdx++)
|
{
|
for (int nScanIdx=0; nScanIdx<nScanCount; nScanIdx++)
|
{
|
SRangeData *pData = GetScanRangeData(nModuleIdx, nScanIdx);
|
if (pData==NULL) continue;
|
|
SRangeData newData = *pData;
|
|
if (IsChangeModuleIndex(nOriginDir, nBaseDir))
|
{
|
int nTmpValue= GetChangeModuleIndex(nModuleIdx, nOriginDir, nBaseDir, nGantryModuleCount, nModuleCount);
|
newData.nModuleIdx = nTmpValue;
|
}
|
|
int bScanIdx = IsChangeScanIndex(nOriginDir, nBaseDir);
|
int bScanDir = IsChangeScanDir(nOriginDir, nBaseDir);
|
|
if (bScanDir==1 && bScanIdx==1) // 11
|
{
|
// change scan index
|
int nTmpValue = GetChangeScanIndex(nScanIdx, nScanCount);
|
newData.nScanIdx = nTmpValue;
|
}
|
else if (bScanDir==1 && bScanIdx==0) // 10
|
{
|
// change scan dir
|
int nTmpValue = GetChangeScanDir(newData.nScanDir);
|
newData.nScanDir = nTmpValue;
|
|
// change scan pos
|
swap_value(newData.nScanStartX, newData.nScanEndX);
|
}
|
else if (bScanDir==0 && bScanIdx==1) // 01
|
{
|
// change scan index
|
int nTmpValue = GetChangeScanIndex(nScanIdx, nScanCount);
|
newData.nScanIdx = nTmpValue;
|
|
// change scan dir
|
nTmpValue = GetChangeScanDir(newData.nScanDir);
|
newData.nScanDir = nTmpValue;
|
|
// change scan pos
|
swap_value(newData.nScanStartX, newData.nScanEndX);
|
}
|
// 00
|
|
vecResult.push_back(newData);
|
}
|
}
|
|
// set member data
|
vecRangeData = vecResult;
|
|
return (int)vecRangeData.size();
|
}
|
|
|
int CPathScheduler_Dual_Flying::PathScheduling( const VectorPathData& vecPathData, const VectorPathData& vecStartPath )
|
{
|
m_vecRangeRect.clear();
|
m_vecScanRangeData.clear();
|
m_vecPathSchedulerResult.clear();
|
|
int nGantryCount = 2;
|
|
int nRangeCount = CalculateRangeRect(
|
nGantryCount,
|
m_PathSchedulerParam.GetModuleCount(),
|
m_PathSchedulerParam.GetScanCount(),
|
m_PathSchedulerParam.GetGlassSizeX(),
|
m_PathSchedulerParam.GetGlassSizeY(),
|
m_PathSchedulerParam.GetScanMargin(),
|
m_PathSchedulerParam.GetOriginDir(),
|
m_PathSchedulerParam.GetBaseModuleDir(),
|
m_vecScanRangeData);
|
|
if (nRangeCount<1) return 0;
|
|
int nTotalCount = 0;
|
SPathData sStartPath; // ¸ðµâº° ½ºÄµ ½ÃÀÛÀ§Ä¡ ÀúÀå.
|
for (int nModuleIdx=0; nModuleIdx<m_PathSchedulerParam.GetModuleCount(); nModuleIdx++)
|
{
|
if (int(vecStartPath.size()) <= nModuleIdx) continue;
|
|
for (int nScanIdx = 0; nScanIdx<m_PathSchedulerParam.GetScanCount(); nScanIdx++)
|
{
|
SRangeData* pRangeData = GetScanRangeData(nModuleIdx, nScanIdx);
|
|
if (pRangeData==NULL) continue;
|
|
// add range rect
|
m_vecRangeRect.push_back(CRect(pRangeData->nLeft, pRangeData->nTop, pRangeData->nRight, pRangeData->nBottom));
|
|
if (nScanIdx==0) // first scan ÀÇ ½ÃÀÛÀ§Ä¡´Â ÇöÀç ¸ðµâÀÇ À§Ä¡ÀÌ´Ù.
|
{
|
sStartPath = vecStartPath[nModuleIdx];
|
}
|
|
// ¿µ¿ªº° ½ºÄÉÁ층 ¼öÇà.
|
CPathSchedulerResult schedulerResult(nModuleIdx, nScanIdx);
|
int nCount = CalculatePath(nModuleIdx, nScanIdx, sStartPath, vecPathData, *pRangeData, schedulerResult);
|
nTotalCount += nCount;
|
if (nCount>1)
|
{
|
// ´ÙÀ½ ½ºÄµÀÇ ½ÃÀÛÀ§Ä¡´Â ÀÌÀü ½ºÄµÀÇ Á¾·á À§Ä¡ÀÌ´Ù.
|
const SSchedulerResult* pNode = schedulerResult.GetPathSchedulerResult(schedulerResult.GetPathSchedulerResultCount() - 1);
|
if (pNode)
|
{
|
sStartPath.nPosX = (pRangeData->nScanEndX);
|
sStartPath.nPosY = int(pNode->dPositionY * 1000);
|
}
|
m_vecPathSchedulerResult.push_back(schedulerResult);
|
}
|
else // ´ë»ó °áÇÔÀÌ ¾øÀ»¶§ xÃà À§Ä¡¸¸ º¯°æÇÑ´Ù.
|
{
|
sStartPath.nPosX = (pRangeData->nScanEndX); //
|
}
|
}
|
}
|
|
return int(m_vecPathSchedulerResult.size());
|
}
|
|
SRangeData* CPathScheduler_Dual_Flying::GetScanRangeData( int nModuleIdx, int nScanIdx )
|
{
|
for (VectorRangeDataIt it=m_vecScanRangeData.begin(); it!=m_vecScanRangeData.end(); it++)
|
{
|
if (it->nModuleIdx==nModuleIdx && it->nScanIdx==nScanIdx)
|
{
|
return &(*it);
|
}
|
}
|
return NULL;
|
}
|
|
const SRangeData* CPathScheduler_Dual_Flying::GetScanRangeData( int nModuleIdx, int nScanIdx ) const
|
{
|
for (constVectorRangeDataIt it=m_vecScanRangeData.begin(); it!=m_vecScanRangeData.end(); it++)
|
{
|
if (it->nModuleIdx==nModuleIdx && it->nScanIdx==nScanIdx)
|
{
|
return &(*it);
|
}
|
}
|
return NULL;
|
}
|
