/// <summary>
/// 현재 위치와 목표위치의 위치차이 Tolerance check
/// </summary>
/// <param name="sPos"> 목표 위치값</param>
/// <param name="bResult"></param>
/// <param name="bCheck_TAxis"></param>
/// <param name="bCheck_ZAxis"></param>
/// <param name="bSkipError">위치가 틀릴경우 에러 보고할지 여부</param>
/// <returns></returns>
public int CompareElevatorPos(CPos_XYTZ sPos,out bool bResult,
bool bCheck_XAxis,bool bCheck_YAxis,bool bCheck_TAxis,bool bSkipError = true)
{
int iResult = SUCCESS;
bResult = false;
// trans to array
double[] dPos;
sPos.TransToArray(out dPos);
bool[] bJudge = new bool[DEF_MAX_COORDINATE];
iResult = m_RefComp.AxElevator.ComparePosition(dPos,out bJudge,DEF_ALL_COORDINATE);
if (iResult != SUCCESS)
{
return(iResult);
}
// skip axis
if (bCheck_XAxis == false)
{
bJudge[DEF_X] = true;
}
if (bCheck_YAxis == false)
{
bJudge[DEF_Y] = true;
}
if (bCheck_TAxis == false)
{
bJudge[DEF_T] = true;
}
// error check
bResult = true;
foreach (bool bTemp in bJudge)
{
if (bTemp == false)
{
bResult = false;
}
}
// skip error?
if (bSkipError == false && bResult == false)
{
string str = $"Stage의 위치비교 결과 미일치합니다. Target Pos : {sPos.ToString()}";
WriteLog(str,ELogType.Debug,ELogWType.Error);
return(GenerateErrorCode(ERR_Elevator_NOT_SAME_POSITION));
}
return(SUCCESS);
}
/// <summary>
/// sPos으로 이동하고, PosInfo를 iPos으로 셋팅한다. Backlash는 일단 차후로.
/// </summary>
/// <param name="sPos"></param>
/// <param name="iPos"></param>
/// <param name="bMoveFlag"></param>
/// <param name="bUseBacklash"></param>
/// <returns></returns>
public int MoveElevatorPos(CPos_XYTZ sPos,int iPos,bool[] bMoveFlag = null,bool bUseBacklash = false,
bool bUsePriority = false,int[] movePriority = null)
{
int iResult = SUCCESS;
// safety check
iResult = CheckForElevatorAxisMove();
if (iResult != SUCCESS)
{
return(iResult);
}
// assume move Z axis if bMoveFlag is null
if (bMoveFlag == null)
{
bMoveFlag = new bool[DEF_MAX_COORDINATE] {
false,false,false,true
};
}
// Bottom Position으로 가는 것이면 Align Offset을 초기화해야 한다.
if (iPos == (int)EElevatorPos.BOTTOM)
{
AxElevatorInfo.InitAlignOffset();
}
// trans to array
double[] dTargetPos;
sPos.TransToArray(out dTargetPos);
// backlash
if (bUseBacklash)
{
// 나중에 작업
}
// 1. move X, Y, T
if (bMoveFlag[DEF_X] == true || bMoveFlag[DEF_Y] == true || bMoveFlag[DEF_T] == true)
{
// set priority
if (bUsePriority == true && movePriority != null)
{
m_RefComp.AxElevator.SetAxesMovePriority(movePriority);
}
// move
bMoveFlag[DEF_Z] = false;
iResult = m_RefComp.AxElevator.Move(DEF_ALL_COORDINATE,bMoveFlag,dTargetPos,bUsePriority);
if (iResult != SUCCESS)
{
WriteLog("fail : move Elevator x y t axis",ELogType.Debug,ELogWType.Error);
return(iResult);
}
}
// 2. move Z Axis
if (bMoveFlag[DEF_Z] == true)
{
bool[] bTempFlag = new bool[DEF_MAX_COORDINATE] {
false,false,false,true
};
iResult = m_RefComp.AxElevator.Move(DEF_ALL_COORDINATE,bTempFlag,dTargetPos);
if (iResult != SUCCESS)
{
WriteLog("fail : move Elevator z axis",ELogType.Debug,ELogWType.Error);
return(iResult);
}
}
// set working pos
if (iPos > (int)EElevatorPos.NONE)
{
AxElevatorInfo.PosInfo = iPos;
}
string str = $"success : move Elevator to pos:{iPos} {sPos.ToString()}";
WriteLog(str,ELogType.Debug,ELogWType.Normal);
return(SUCCESS);
}
/// <summary>
/// sPos으로 이동하고, PosInfo를 iPos으로 셋팅한다. Backlash는 일단 차후로.
/// </summary>
/// <param name="sPos"></param>
/// <param name="iPos"></param>
/// <param name="bMoveFlag"></param>
/// <param name="bUseBacklash"></param>
/// <returns></returns>
public int MoveHandlerPos(CPos_XYTZ sPos, int iPos, bool[] bMoveFlag = null, bool bUseBacklash = false,
bool bUsePriority = false, int[] movePriority = null)
{
int iResult = SUCCESS;
// safety check
iResult = CheckForHandlerAxisMove();
if (iResult != SUCCESS)
{
return(iResult);
}
// assume move all axis if bMoveFlag is null
if (bMoveFlag == null)
{
bMoveFlag = new bool[DEF_MAX_COORDINATE] {
true, true, true, true
};
}
// Load Position으로 가는 것이면 Align Offset을 초기화해야 한다.
if (iPos == (int)EHandlerPos.LOAD)
{
AxHandlerInfo.InitAlignOffset();
}
// trans to array
double[] dTargetPos;
sPos.TransToArray(out dTargetPos);
// backlash
if (bUseBacklash)
{
// 나중에 작업
}
// 1. move Z Axis to Safety Up. but when need to move z axis only, don't need to move z to safety pos
if (bMoveFlag[DEF_X] == false && bMoveFlag[DEF_Y] == false && bMoveFlag[DEF_T] == false &&
bMoveFlag[DEF_Z] == true)
{
;
}
else
{
bool bStatus;
iResult = IsHandlerAxisInSafetyZone(DEF_Z, out bStatus);
if (iResult != SUCCESS)
{
return(iResult);
}
if (bStatus == false)
{
iResult = MoveHandlerToSafetyPos(DEF_Z);
if (iResult != SUCCESS)
{
return(iResult);
}
}
}
// 2. move X, Y, T
if (bMoveFlag[DEF_X] == true || bMoveFlag[DEF_Y] == true || bMoveFlag[DEF_T] == true)
{
// set priority
if (bUsePriority == true && movePriority != null)
{
m_RefComp.AxHandler.SetAxesMovePriority(movePriority);
}
// move
bMoveFlag[DEF_Z] = false;
iResult = m_RefComp.AxHandler.Move(DEF_ALL_COORDINATE, bMoveFlag, dTargetPos, bUsePriority);
if (iResult != SUCCESS)
{
WriteLog("fail : move handler x y t axis", ELogType.Debug, ELogWType.Error);
return(iResult);
}
}
// 3. move Z Axis
if (bMoveFlag[DEF_Z] == true)
{
bool[] bTempFlag = new bool[DEF_MAX_COORDINATE] {
false, false, false, true
};
iResult = m_RefComp.AxHandler.Move(DEF_ALL_COORDINATE, bTempFlag, dTargetPos);
if (iResult != SUCCESS)
{
WriteLog("fail : move handler z axis", ELogType.Debug, ELogWType.Error);
return(iResult);
}
}
// set working pos
if (iPos > (int)EHandlerPos.NONE)
{
AxHandlerInfo.PosInfo = iPos;
}
string str = $"success : move handler to pos:{iPos} {sPos.ToString()}";
WriteLog(str, ELogType.Debug, ELogWType.Normal);
return(SUCCESS);
}