/// <summary>
/// iPos 좌표로 선택된 축들을 이동시킨다.
/// </summary>
/// <param name="iPos">목표 위치</param>
/// <param name="bUpdatedPosInfo">목표위치값을 update 할지의 여부</param>
/// <param name="bMoveFlag">이동시킬 축 선택 </param>
/// <param name="dMoveOffset">임시 옵셋값 </param>
/// <param name="bUseBacklash"></param>
/// <param name="bUsePriority">우선순위 이동시킬지 여부 </param>
/// <param name="movePriority">우선순위 </param>
/// <returns></returns>
public int MoveHandlerPos(int iPos, bool bUpdatedPosInfo = true, bool[] bMoveFlag = null, double[] dMoveOffset = null, bool bUseBacklash = false,
bool bUsePriority = false, int[] movePriority = null)
{
int iResult = SUCCESS;
// Load Position으로 가는 것이면 Align Offset을 초기화해야 한다.
if (iPos == (int)EHandlerPos.LOAD)
{
AxHandlerInfo.InitAlignOffset();
}
CPos_XYTZ sTargetPos = AxHandlerInfo.GetTargetPos(iPos);
if (dMoveOffset != null)
{
sTargetPos = sTargetPos + dMoveOffset;
}
if (bUpdatedPosInfo == false)
{
iPos = (int)EHandlerPos.NONE;
}
iResult = MoveHandlerPos(sTargetPos, iPos, bMoveFlag, bUseBacklash, bUsePriority, movePriority);
if (iResult != SUCCESS)
{
return(iResult);
}
return(SUCCESS);
}
public void Init()
{
for (int i = 0; i < Pos.Length; i++)
{
Pos[i] = new CPos_XYTZ();
}
}
public static CPos_XYTZ operator +(CPos_XYTZ s1, double[] dAdd)
{
CPos_XYTZ s = new CPos_XYTZ();
s.dX = s1.dX + dAdd[0];
s.dY = s1.dY + dAdd[1];
s.dT = s1.dT + dAdd[2];
s.dZ = s1.dZ + dAdd[3];
return(s);
}
public static CPos_XYTZ operator -(CPos_XYTZ s1, CPos_XYTZ s2)
{
CPos_XYTZ s = new CPos_XYTZ();
s.dX = s1.dX - s2.dX;
s.dY = s1.dY - s2.dY;
s.dT = s1.dT - s2.dT;
s.dZ = s1.dZ - s2.dZ;
return(s);
}
public static CPos_XYTZ operator -(CPos_XYTZ s1, double[] dSub)
{
CPos_XYTZ s = new CPos_XYTZ();
s.dX = s1.dX - dSub[0];
s.dY = s1.dY - dSub[1];
s.dT = s1.dT - dSub[2];
s.dZ = s1.dZ - dSub[3];
return(s);
}
public static CPos_XYTZ operator *(CPos_XYTZ s1, double[] dMul)
{
CPos_XYTZ s = new CPos_XYTZ();
s.dX = s1.dX * dMul[0];
s.dY = s1.dY * dMul[1];
s.dT = s1.dT * dMul[2];
s.dZ = s1.dZ * dMul[3];
return(s);
}
public static CPos_XYTZ operator +(CPos_XYTZ s1, CPos_XYTZ s2)
{
CPos_XYTZ s = new CPos_XYTZ();
s.dX = s1.dX + s2.dX;
s.dY = s1.dY + s2.dY;
s.dT = s1.dT + s2.dT;
s.dZ = s1.dZ + s2.dZ;
return(s);
}
/// <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);
}
public CPos_XYTZ GetTargetPos(int index)
{
Debug.Assert((int)EUnitPos.LOAD <= index && index < PosLength);
CPos_XYTZ target = FixedPos.Pos[index] + ModelPos.Pos[index] + OffsetPos.Pos[index];
// index가 Loading 위치가 아니고, 얼라인이 되어있다면 얼라인 보정값 적용
if (index != (int)EUnitPos.LOAD && IsMarkAligned == true)
{
target = target + AlignOffset;
}
return(target);
}
public CPos_XYTZ AlignOffset; // 얼라인 결과값
public CMovingObject(int PosLength)
{
Debug.Assert(0 < PosLength);
this.PosLength = PosLength;
FixedPos = new CUnitPos(PosLength);
ModelPos = new CUnitPos(PosLength);
OffsetPos = new CUnitPos(PosLength);
PosInfo = (int)EUnitPos.NONE;
AlignOffset = new CPos_XYTZ();
}
public override bool Equals(object obj)
{
if (obj == null)
{
return(false);
}
if (!(obj is CPos_XYTZ))
{
return(false);
}
CPos_XYTZ s2 = (CPos_XYTZ)obj;
return(Math.Equals(dX, s2.dX) && Math.Equals(dY, s2.dY) &&
Math.Equals(dT, s2.dT) && Math.Equals(dZ, s2.dZ));
}
public int GetCurPos(out CPos_XYTZ pos, int iCoordID = DEF_ALL_COORDINATE)
{
int iResult = SUCCESS;
double[] dPos;
GetCurPos(out dPos, iCoordID);
pos = new CPos_XYTZ();
if (iCoordID == DEF_ALL_COORDINATE)
{
pos.TransFromArray(dPos);
}
else
{
pos.SetPosition(iCoordID, dPos[0]);
}
return(SUCCESS);
}
public int SetAlignData(CPos_XYTZ offset)
{
int iResult;
CPos_XYTZ curPos;
// 현재 Align Offset 값을 읽어옴
iResult = GetStageCurPos(out curPos);
if (iResult != SUCCESS)
{
return(iResult);
}
// AlignData 적용
curPos += offset;
// AlignOffet 적용
AxStageInfo.SetAlignOffset(curPos);
return(SUCCESS);
}
public int CompareHandlerPos(int iPos, out bool bResult, bool bCheck_TAxis, bool bCheck_ZAxis, bool bSkipError = true)
{
int iResult = SUCCESS;
bResult = false;
CPos_XYTZ targetPos = AxHandlerInfo.GetTargetPos(iPos);
if (iResult != SUCCESS)
{
return(iResult);
}
iResult = CompareHandlerPos(targetPos, out bResult, bCheck_TAxis, bCheck_ZAxis, bSkipError);
if (iResult != SUCCESS)
{
return(iResult);
}
return(SUCCESS);
}
/// <summary>
/// iPos 좌표로 선택된 축들을 이동시킨다.
/// </summary>
/// <param name="iPos">목표 위치</param>
/// <param name="SlotNum">목표 Slow 위치</param>
/// <param name="bUpdatedPosInfo">목표위치값을 update 할지의 여부</param>
/// <param name="bMoveFlag">이동시킬 축 선택 </param>
/// <param name="dMoveOffset">임시 옵셋값 </param>
/// <param name="bUseBacklash"></param>
/// <param name="bUsePriority">우선순위 이동시킬지 여부 </param>
/// <param name="movePriority">우선순위 </param>
/// <returns></returns>
public int MoveElevatorPos(int iPos,int SlotNum = 0,bool bUpdatedPosInfo = true,
bool[] bMoveFlag = null,double[] dMoveOffset = null,bool bUseBacklash = false,
bool bUsePriority = false,int[] movePriority = null)
{
int iResult = SUCCESS;
// Load Position으로 가는 것이면 Align Offset을 초기화해야 한다.
if (iPos == (int)EElevatorPos.LOAD)
{
AxElevatorInfo.InitAlignOffset();
}
// Slot Position으로 가는 것이면 Slot번호와 Pitch를 곱해서 Offset을 적용한다.
if (iPos == (int)EElevatorPos.SLOT)
{
dMoveOffset[DEF_X] = 0.0;
dMoveOffset[DEF_Y] = 0.0;
dMoveOffset[DEF_T] = 0.0;
dMoveOffset[DEF_Z] = (double)SlotNum * m_Data.CassetteData.dSlotPitch;
}
// 이동할 위치의 값을 읽어옴.
CPos_XYTZ sTargetPos = AxElevatorInfo.GetTargetPos(iPos);
if (dMoveOffset != null)
{
sTargetPos = sTargetPos + dMoveOffset;
}
if (bUpdatedPosInfo == false)
{
//iPos = (int)EElevatorPos.NONE;
return(GenerateErrorCode(ERR_Elevator_UNABLE_TO_USE_POSITION));
}
iResult = MoveElevatorPos(sTargetPos,iPos,bMoveFlag,bUseBacklash,bUsePriority,movePriority);
if (iResult != SUCCESS)
{
return(iResult);
}
return(SUCCESS);
}
public static CPos_XYTZ operator /(CPos_XYTZ s1, double[] dDiv)
{
CPos_XYTZ s = new CPos_XYTZ();
if (dDiv[0] != 0)
{
s.dX = s1.dX / dDiv[0];
}
if (dDiv[1] != 0)
{
s.dY = s1.dY / dDiv[1];
}
if (dDiv[2] != 0)
{
s.dT = s1.dT / dDiv[2];
}
if (dDiv[3] != 0)
{
s.dZ = s1.dZ / dDiv[3];
}
return(s);
}
public int CompareElevatorPos(int iPos,out bool bResult,out int nSlotNum,bool bSkipError = true)
{
int iResult = SUCCESS;
bool bCheck_XAxis = false;
bool bCheck_YAxis = false;
bool bCheck_TAxis = false;
bResult = false;
nSlotNum = -1;
CPos_XYTZ targetPos = AxElevatorInfo.GetTargetPos(iPos);
if (iResult != SUCCESS)
{
return(iResult);
}
iResult = CompareElevatorPos(targetPos,out bResult,bCheck_XAxis,bCheck_YAxis,bCheck_TAxis,bSkipError);
if (iResult != SUCCESS)
{
return(iResult);
}
// Slot Position Cals (Result가 true일 경우)
if (iPos == (int)EElevatorPos.SLOT && bResult)
{
double dReferencePos = 0.0;
CPos_XYTZ LoadPos = AxElevatorInfo.GetTargetPos((int)EUnitPos.LOAD);
dReferencePos = targetPos.dZ - LoadPos.dZ;
m_Data.CurrentSlotNum = (int)(dReferencePos / m_Data.CassetteData.dSlotPitch);
nSlotNum = m_Data.CurrentSlotNum;
}
return(SUCCESS);
}
//===============================================================================================
#endregion
// Stage Servo 구동
#region Stage Move 동작
public int GetStageCurPos(out CPos_XYTZ pos)
{
m_RefComp.AxStage.GetCurPos(out pos);
return(SUCCESS);
}
public int GetHandlerCurPos(out CPos_XYTZ pos)
{
m_RefComp.AxHandler.GetCurPos(out pos);
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);
}
public void GetAlignOffset(out CPos_XYTZ offset)
{
offset = ObjectExtensions.Copy(AlignOffset);
}
public void SetAlignOffset(CPos_XYTZ offset)
{
IsMarkAligned = true;
AlignOffset = ObjectExtensions.Copy(offset);
}
public int GetElevatorCurPos(out CPos_XYTZ pos)
{
m_RefComp.AxElevator.GetCurPos(out pos);
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);
}