Files
ATV_STDLabelAttach/Handler/Project_form2/Util/Util_Mot.cs
2025-07-17 16:11:46 +09:00

452 lines
20 KiB
C#

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
namespace Project
{
public struct sPositionData
{
public double position;
public double acc;
public double dcc;
public double speed;
public Boolean isError;
public string message;
public void Clear()
{
position = 0;
acc = 0;
dcc = 0;
speed = 0;
isError = true;
message = "Not Set";
}
}
public static partial class Util_Mot
{
#region "Get Axis Position"
public static sPositionData GetAxPXPos(eAxisPXPos pos) { return GetAxPos(eAxis.X_PICKER, (int)pos); }
public static sPositionData GetAxPZPos(eAxisPZPos pos) { return GetAxPos(eAxis.Z_PICKER, (int)pos); }
public static sPositionData GetAxPLMPos(eAxisPLMovePos pos) { return GetAxPos(eAxis.PL_MOVE, (int)pos); }
public static sPositionData GetAxPLZPos(eAxisPLUPDNPos pos) { return GetAxPos(eAxis.PL_UPDN, (int)pos); }
public static sPositionData GetAxPRMPos(eAxisPRMovePos pos) { return GetAxPos(eAxis.PR_MOVE, (int)pos); }
public static sPositionData GetAxPRZPos(eAxisPRUPDNPos pos) { return GetAxPos(eAxis.PR_UPDN, (int)pos); }
public static sPositionData GetAxPTPos(eAxisPTPos pos) { return GetAxPos(eAxis.Z_THETA, (int)pos); }
public static sPositionData GetAxPos(eAxis axis, int pos)
{
return GetAxpos((int)axis, pos);
}
public static sPositionData GetAxpos(int axis, int pos)
{
var retval = new sPositionData();
retval.Clear();
if (Pub.Result.mModel == null || Pub.Result.mModel.isSet == false)
{
retval.message = "모션 모델이 설정되어 있지 않습니다";
return retval;
}
var data = Pub.Result.mModel.Position[axis][pos];
if (data.index == -1)
{
retval.message = string.Format("축:{0})의 위치:{1} 의 값이 존재하지 않습니다", axis, pos);// "모션 모델이 설정되어 있지 않습니다";
return retval;
}
retval.position = data.value;
retval.speed = data.speed;
retval.acc = data.acc;
if (data.dcc < 1) retval.dcc = retval.acc;
else retval.dcc = data.dcc;
//환경설정에서 저속모드로 설정했다면 지정된 속도로만 처리한다
if (Pub.setting.Enable_SpeedLimit == true)
retval.speed = Math.Min(retval.speed, Pub.setting.LimitSpeed);
retval.isError = false;
retval.message = string.Empty;
return retval;
}
#endregion
#region "Get Position Match"
public static Boolean getPositionMatch(eAxisPXPos pos, double offset = 0.1)
{
var posData = GetAxPXPos(pos);
return getPositionMatch(eAxis.X_PICKER, posData.position, offset);
}
public static Boolean getPositionMatch(eAxisPZPos pos, double offset = 0.1)
{
var posData = GetAxPZPos(pos);
return getPositionMatch(eAxis.Z_PICKER, posData.position, offset);
}
public static Boolean getPositionMatch(eAxisPTPos pos, double offset = 0.1)
{
var posData = GetAxPTPos(pos);
return getPositionMatch(eAxis.Z_THETA, posData.position, offset);
}
public static Boolean getPositionMatch(eAxisPLMovePos pos, double offset = 0.1)
{
var posData = GetAxPLMPos(pos);
return getPositionMatch(eAxis.PL_MOVE, posData.position, offset);
}
public static Boolean getPositionMatch(eAxisPLUPDNPos pos, double offset = 0.1)
{
var posData = GetAxPLZPos(pos);
return getPositionMatch(eAxis.PL_UPDN, posData.position, offset);
}
public static Boolean getPositionMatch(eAxisPRMovePos pos, double offset = 0.1)
{
var posData = GetAxPRMPos(pos);
return getPositionMatch(eAxis.PR_MOVE, posData.position, offset);
}
public static Boolean getPositionMatch(eAxisPRUPDNPos pos, double offset = 0.1)
{
var posData = GetAxPRZPos(pos);
return getPositionMatch(eAxis.PR_UPDN, posData.position, offset);
}
#endregion
#region "Get Position Name"
public static ePickYPosition GetPKX_PosName(double Pos = -1)
{
//홈을 잡지 않았다면 오류로 처리함\
//eYPPosition retval = eYPPosition.Unknown;
var motIndex = (int)eAxis.X_PICKER;
if (Pub.mot.IsInit == false) return ePickYPosition.ERROR; //200213
if (Pub.mot.isHomeSet[motIndex] == false) return ePickYPosition.ERROR;
if (Pub.Result == null || Pub.Result.isSetmModel == false) return ePickYPosition.ERROR;
//위치가 입력되지 않았다면 현재 위치를 사용한다
if (Pos == -1) Pos = Pub.mot.dACTPOS[motIndex];
var PosT = 0.0;
//지정한 위치가 저장된 어느위치에 속해잇느지 확인해서 반환한다
PosT = Util_Mot.GetAxPXPos(eAxisPXPos.PICKON).position;
if (MatchPosition(Pos, PosT)) return ePickYPosition.PICKON;
PosT = Util_Mot.GetAxPXPos(eAxisPXPos.PICKOFFR).position;
if (MatchPosition(Pos, PosT)) return ePickYPosition.PICKOFFR;
PosT = Util_Mot.GetAxPXPos(eAxisPXPos.PICKOFFL).position;
if (MatchPosition(Pos, PosT)) return ePickYPosition.PICKOFFL;
PosT = Util_Mot.GetAxPXPos(eAxisPXPos.READYL).position;
if (MatchPosition(Pos, PosT)) return ePickYPosition.READYL;
PosT = Util_Mot.GetAxPXPos(eAxisPXPos.READYR).position;
if (MatchPosition(Pos, PosT)) return ePickYPosition.READYR;
if (Pub.mot.isLimitN[motIndex]) return ePickYPosition.LIMITN;
else if (Pub.mot.isLimitP[motIndex]) return ePickYPosition.LIMITP;
else if (Pub.mot.isOrg[motIndex]) return ePickYPosition.HOME;
else return ePickYPosition.UNKNOWN;
}
public static ePickZPosition GetPKZ_PosName(double Pos = -1)
{
//홈을 잡지 않았다면 오류로 처리함\
//eYPPosition retval = eYPPosition.Unknown;
var motIndex = (int)eAxis.Z_PICKER;
if (Pub.mot.IsInit == false) return ePickZPosition.ERROR; //200213
if (Pub.mot.isHomeSet[motIndex] == false) return ePickZPosition.ERROR;
if (Pub.Result == null || Pub.Result.isSetmModel == false) return ePickZPosition.ERROR;
//위치가 입력되지 않았다면 현재 위치를 사용한다
if (Pos == -1) Pos = Pub.mot.dACTPOS[motIndex];
var PosT = 0.0;
//지정한 위치가 저장된 어느위치에 속해잇느지 확인해서 반환한다
PosT = Util_Mot.GetAxPZPos(eAxisPZPos.PICKOFFR).position;
if (MatchPosition(Pos, PosT)) return ePickZPosition.PICKOFFR;
PosT = Util_Mot.GetAxPZPos(eAxisPZPos.PICKOFFL).position;
if (MatchPosition(Pos, PosT)) return ePickZPosition.PICKOFFL;
PosT = Util_Mot.GetAxPZPos(eAxisPZPos.PICKON).position;
if (MatchPosition(Pos, PosT)) return ePickZPosition.PICKON;
PosT = Util_Mot.GetAxPZPos(eAxisPZPos.READY).position;
if (MatchPosition(Pos, PosT)) return ePickZPosition.READY;
if (Pub.mot.isLimitN[motIndex]) return ePickZPosition.LIMITN;
else if (Pub.mot.isLimitP[motIndex]) return ePickZPosition.LIMITP;
else if (Pub.mot.isOrg[motIndex]) return ePickZPosition.HOME;
else return ePickZPosition.UNKNOWN;
}
public static eThetaPosition GetPT_PosName(double Pos = -1)
{
//홈을 잡지 않았다면 오류로 처리함\
//eYPPosition retval = eYPPosition.Unknown;
var motIndex = (int)eAxis.Z_THETA;
if (Pub.mot.IsInit == false) return eThetaPosition.ERROR; //200213
if (Pub.mot.isHomeSet[motIndex] == false) return eThetaPosition.ERROR;
if (Pub.Result == null || Pub.Result.isSetmModel == false) return eThetaPosition.ERROR;
//위치가 입력되지 않았다면 현재 위치를 사용한다
if (Pos == -1) Pos = Pub.mot.dACTPOS[motIndex];
var PosT = 0.0;
//지정한 위치가 저장된 어느위치에 속해잇느지 확인해서 반환한다
PosT = Util_Mot.GetAxPTPos(eAxisPTPos.READY).position;
if (MatchPosition(Pos, PosT)) return eThetaPosition.READY;
if (Pub.mot.isLimitN[motIndex]) return eThetaPosition.LIMITN;
else if (Pub.mot.isLimitP[motIndex]) return eThetaPosition.LIMITP;
else if (Pub.mot.isOrg[motIndex]) return eThetaPosition.HOME;
else return eThetaPosition.UNKNOWN;
}
public static ePrintYPosition GetPLM_PosName(double Pos = -1)
{
//홈을 잡지 않았다면 오류로 처리함\
//eYPPosition retval = eYPPosition.Unknown;
var motIndex = (int)eAxis.PL_MOVE;
if (Pub.mot.IsInit == false) return ePrintYPosition.ERROR; //200213
if (Pub.mot.isHomeSet[motIndex] == false) return ePrintYPosition.ERROR;
if (Pub.Result == null || Pub.Result.isSetmModel == false) return ePrintYPosition.ERROR;
//위치가 입력되지 않았다면 현재 위치를 사용한다
if (Pos == -1) Pos = Pub.mot.dACTPOS[motIndex];
var PosT = 0.0;
//지정한 위치가 저장된 어느위치에 속해잇느지 확인해서 반환한다
PosT = Util_Mot.GetAxPLMPos(eAxisPLMovePos.PRINTH07).position;
if (MatchPosition(Pos, PosT)) return ePrintYPosition.PRINTH07;
PosT = Util_Mot.GetAxPLMPos(eAxisPLMovePos.PRINTH13).position;
if (MatchPosition(Pos, PosT)) return ePrintYPosition.PRINTH13;
PosT = Util_Mot.GetAxPLMPos(eAxisPLMovePos.PRINTL07).position;
if (MatchPosition(Pos, PosT)) return ePrintYPosition.PRINTL07;
PosT = Util_Mot.GetAxPLMPos(eAxisPLMovePos.PRINTL13).position;
if (MatchPosition(Pos, PosT)) return ePrintYPosition.PRINTL13;
PosT = Util_Mot.GetAxPLMPos(eAxisPLMovePos.PRINTM07).position;
if (MatchPosition(Pos, PosT)) return ePrintYPosition.PRINTM07;
PosT = Util_Mot.GetAxPLMPos(eAxisPLMovePos.PRINTM13).position;
if (MatchPosition(Pos, PosT)) return ePrintYPosition.PRINTM13;
PosT = Util_Mot.GetAxPLMPos(eAxisPLMovePos.READY).position;
if (MatchPosition(Pos, PosT)) return ePrintYPosition.READY;
if (Pub.mot.isLimitN[motIndex]) return ePrintYPosition.LIMITN;
else if (Pub.mot.isLimitP[motIndex]) return ePrintYPosition.LIMITP;
else if (Pub.mot.isOrg[motIndex]) return ePrintYPosition.HOME;
else return ePrintYPosition.UNKNOWN;
}
public static ePrintYPosition GetPRM_PosName(double Pos = -1)
{
//홈을 잡지 않았다면 오류로 처리함\
//eYPPosition retval = eYPPosition.Unknown;
var motIndex = (int)eAxis.PR_MOVE;
if (Pub.mot.IsInit == false) return ePrintYPosition.ERROR; //200213
if (Pub.mot.isHomeSet[motIndex] == false) return ePrintYPosition.ERROR;
if (Pub.Result == null || Pub.Result.isSetmModel == false) return ePrintYPosition.ERROR;
//위치가 입력되지 않았다면 현재 위치를 사용한다
if (Pos == -1) Pos = Pub.mot.dACTPOS[motIndex];
var PosT = 0.0;
//지정한 위치가 저장된 어느위치에 속해잇느지 확인해서 반환한다
PosT = Util_Mot.GetAxPRMPos(eAxisPRMovePos.PRINTH07).position;
if (MatchPosition(Pos, PosT)) return ePrintYPosition.PRINTH07;
PosT = Util_Mot.GetAxPRMPos(eAxisPRMovePos.PRINTL07).position;
if (MatchPosition(Pos, PosT)) return ePrintYPosition.PRINTL07;
PosT = Util_Mot.GetAxPRMPos(eAxisPRMovePos.PRINTH13).position;
if (MatchPosition(Pos, PosT)) return ePrintYPosition.PRINTH13;
PosT = Util_Mot.GetAxPRMPos(eAxisPRMovePos.PRINTL13).position;
if (MatchPosition(Pos, PosT)) return ePrintYPosition.PRINTL13;
PosT = Util_Mot.GetAxPRMPos(eAxisPRMovePos.READY).position;
if (MatchPosition(Pos, PosT)) return ePrintYPosition.READY;
if (Pub.mot.isLimitN[motIndex]) return ePrintYPosition.LIMITN;
else if (Pub.mot.isLimitP[motIndex]) return ePrintYPosition.LIMITP;
else if (Pub.mot.isOrg[motIndex]) return ePrintYPosition.HOME;
else return ePrintYPosition.UNKNOWN;
}
public static ePrintZPosition GetPLZ_PosName(double Pos = -1)
{
//홈을 잡지 않았다면 오류로 처리함\
//eYPPosition retval = eYPPosition.Unknown;
var motIndex = (int)eAxis.PL_UPDN;
if (Pub.mot.IsInit == false) return ePrintZPosition.ERROR; //200213
if (Pub.mot.isHomeSet[motIndex] == false) return ePrintZPosition.ERROR;
if (Pub.Result == null || Pub.Result.isSetmModel == false) return ePrintZPosition.ERROR;
//위치가 입력되지 않았다면 현재 위치를 사용한다
if (Pos == -1) Pos = Pub.mot.dACTPOS[motIndex];
var PosT = 0.0;
//지정한 위치가 저장된 어느위치에 속해잇느지 확인해서 반환한다
PosT = Util_Mot.GetAxPLZPos(eAxisPLUPDNPos.PICKOFF).position;
if (MatchPosition(Pos, PosT)) return ePrintZPosition.PICKOFF;
PosT = Util_Mot.GetAxPLZPos(eAxisPLUPDNPos.PICKON).position;
if (MatchPosition(Pos, PosT)) return ePrintZPosition.PICKON;
PosT = Util_Mot.GetAxPLZPos(eAxisPLUPDNPos.READY).position;
if (MatchPosition(Pos, PosT)) return ePrintZPosition.READY;
if (Pub.mot.isLimitN[motIndex]) return ePrintZPosition.LIMITN;
else if (Pub.mot.isLimitP[motIndex]) return ePrintZPosition.LIMITP;
else if (Pub.mot.isOrg[motIndex]) return ePrintZPosition.HOME;
else return ePrintZPosition.UNKNOWN;
}
public static ePrintZPosition GetPRZ_PosName(double Pos = -1)
{
//홈을 잡지 않았다면 오류로 처리함\
//eYPPosition retval = eYPPosition.Unknown;
var motIndex = (int)eAxis.PR_UPDN;
if (Pub.mot.IsInit == false) return ePrintZPosition.ERROR; //200213
if (Pub.mot.isHomeSet[motIndex] == false) return ePrintZPosition.ERROR;
if (Pub.Result == null || Pub.Result.isSetmModel == false) return ePrintZPosition.ERROR;
//위치가 입력되지 않았다면 현재 위치를 사용한다
if (Pos == -1) Pos = Pub.mot.dACTPOS[motIndex];
var PosT = 0.0;
//지정한 위치가 저장된 어느위치에 속해잇느지 확인해서 반환한다
PosT = Util_Mot.GetAxPRZPos(eAxisPRUPDNPos.PICKOFF).position;
if (MatchPosition(Pos, PosT)) return ePrintZPosition.PICKOFF;
PosT = Util_Mot.GetAxPRZPos(eAxisPRUPDNPos.PICKON).position;
if (MatchPosition(Pos, PosT)) return ePrintZPosition.PICKON;
PosT = Util_Mot.GetAxPRZPos(eAxisPRUPDNPos.READY).position;
if (MatchPosition(Pos, PosT)) return ePrintZPosition.READY;
if (Pub.mot.isLimitN[motIndex]) return ePrintZPosition.LIMITN;
else if (Pub.mot.isLimitP[motIndex]) return ePrintZPosition.LIMITP;
else if (Pub.mot.isOrg[motIndex]) return ePrintZPosition.HOME;
else return ePrintZPosition.UNKNOWN;
}
#endregion
/// <summary>
/// Z-L축이 안전위치에있는가?(Ready 보다 위에있으면 안전위치이다)
/// </summary>
/// <returns></returns>
public static Boolean isAxisSaftyZone(eAxis axis, int allowoffset = 2)
{
//홈을 잡지 않았다면 오류로 처리함
var motIndex = (int)axis;
if (Pub.mot.IsInit == false) return false;
if (Pub.mot.isHomeSet[motIndex] == false) return false;
if (Pub.Result == null || Pub.Result.isSetmModel == false) return false;
sPositionData readypos;
if (axis == eAxis.X_PICKER)
{
//피커는 안전위이가 2개 있다
readypos = GetAxPXPos(eAxisPXPos.PICKON);
//var readypos1 = GetAxPXPos(eAxisPXPos.ReadyR);
//var offset1 = getPositionOffset(axis, readypos.position);
//var offset2 = getPositionOffset(axis, readypos1.position);
//if (offset1 < allowoffset || offset2 < allowoffset ) return true; //오차가 2미만이라면 안전하다
//return false;
}
else if (axis == eAxis.Z_PICKER) readypos = GetAxPZPos(eAxisPZPos.READY);
else if (axis == eAxis.Z_THETA) readypos = GetAxPTPos(eAxisPTPos.READY);
else if (axis == eAxis.PL_MOVE) readypos = GetAxPLMPos(eAxisPLMovePos.READY);
else if (axis == eAxis.PR_MOVE) readypos = GetAxPRMPos(eAxisPRMovePos.READY);
else if (axis == eAxis.PR_UPDN) readypos = GetAxPRZPos(eAxisPRUPDNPos.READY);
else if (axis == eAxis.PL_UPDN) readypos = GetAxPLZPos(eAxisPLUPDNPos.READY);
else return false;
var offset = getPositionOffset(axis, readypos.position);
if (offset < allowoffset) return true; //오차가 2미만이라면 안전하다
return false;
}
private static Boolean Home_Validation(eAxis axis, out string errorMessage)
{
Boolean retval = true;
// int axis = (int)axis_;
if (!Move_Validation(axis, out errorMessage)) retval = false; //이동이 불가한 경우 체크
else if (axis == eAxis.X_PICKER)
{
//Z축 홈이 필요하다
var zFHome = Pub.mot.isHomeSet[(int)eAxis.Z_PICKER];
var zRHome = Pub.mot.isHomeSet[(int)eAxis.Z_THETA];
if (zFHome == false || zRHome == false)
{
//errorMessage = "Z-PICKER,Z-THETA 축 홈이 필요 합니다";
//retval = false;
}
}
else if (axis == eAxis.PL_MOVE) //Z축의 경우 Y축이 홈으로 된 상태여야 한다.
{
if (Pub.mot.isHomeSet[(int)eAxis.PL_UPDN] == false)
{
errorMessage = "PRINT-L Z 축 홈이 필요 합니다";
retval = false;
}
}
else if (axis == eAxis.PR_MOVE) //Z축의 경우 Y축이 홈으로 된 상태여야 한다.
{
if (Pub.mot.isHomeSet[(int)eAxis.PR_UPDN] == false)
{
errorMessage = "PRINT-R Z 축 홈이 필요 합니다";
retval = false;
}
}
return retval;
}
private static Boolean Move_Validation(eAxis axis, out string errorMessage)
{
errorMessage = string.Empty;
if (Util_DO.isEmergencyOn() == true)
{
errorMessage = ("비상정지 상태일때에는 움직일 수 없습니다.");
return false;
}
return true;
}
}
}