628 lines
22 KiB
C#
628 lines
22 KiB
C#
#pragma warning disable IDE1006 // 명명 스타일
|
|
using arDev.MOT;
|
|
using AR;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Drawing;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using System.Threading.Tasks;
|
|
|
|
namespace Project
|
|
{
|
|
public static partial class MOT
|
|
{
|
|
|
|
/// <summary>
|
|
/// 상대위치로 모터를 이동합니다
|
|
/// </summary>
|
|
/// <param name="value"></param>
|
|
/// <param name="src"></param>
|
|
public static void Rotation(double value,string src)
|
|
{
|
|
//모터회전
|
|
var thpos = MOT.GetPTPos(ePTLoc.READY); //적용할 속도값용
|
|
MOT.Move(eAxis.Z_THETA, value, thpos.Speed, thpos.Acc, true);
|
|
PUB.log.AddI($"Theta Rorate:{value},Reason:{src}");
|
|
}
|
|
|
|
public static Boolean CheckMotionLimitError()
|
|
{
|
|
for (short i = 0; i < PUB.mot.DeviceCount; i++)
|
|
{
|
|
if (PUB.mot.IsUse(i) == false) continue; //미사용축은 제외한다.
|
|
//if (SETTING.System.UseAxis(i) == false) continue;
|
|
//if (SETTING.System.UseOriginSignal(i) == false) continue;
|
|
if (PUB.mot.IsLimit(i)) return true;
|
|
}
|
|
return false;
|
|
|
|
}
|
|
|
|
#region "Check Motion Position"
|
|
|
|
public static Boolean CheckMotionPos(TimeSpan stepTime, sPositionData posdata, string source, Boolean useInterLocak = true, int timeoutSec = 0)
|
|
{
|
|
if (posdata.Axis < 0) throw new Exception("CheckMotionPos:Motion number not found");
|
|
//return CheckMotionPos((eAxis)posdata.Axis, stepTime, posdata.Position, posdata.Speed, posdata.Acc, posdata.Dcc, source, useInterLocak, timeoutSec, posdata.inpositionrange);
|
|
return CheckMotionPos((eAxis)posdata.Axis, stepTime, posdata.Position, posdata.Speed, posdata.Acc, posdata.Dcc, source, useInterLocak, timeoutSec, posdata.inpositionrange);
|
|
}
|
|
|
|
|
|
public static Boolean CheckMotionPos(eAxis Motaxis, TimeSpan stepTime, double pos, double speed, double acc, double dcc, string source, Boolean useInterLocak = true, int timeoutSec = 0, float inpaccr = 0f)
|
|
{
|
|
var axis = (short)Motaxis;
|
|
|
|
//타임아웃 적용 191213
|
|
if (timeoutSec == 0) timeoutSec = AR.SETTING.Data.Timeout_MotionCommand;
|
|
|
|
//accr 범위가 지정되잇다며 ㄴ그것을 사용한다
|
|
if (inpaccr == 0f)
|
|
inpaccr = 0.1f;// SETTING.System.INPAccurary(axis);
|
|
|
|
|
|
//X축을 그립위치까지 이동함
|
|
if (PUB.mot.IsInp(axis) && PUB.mot.IsMotion(axis) == false)
|
|
{
|
|
var offset = Math.Abs(PUB.mot.GetCmdPos(axis) - pos);
|
|
var offsetReal = Math.Abs(PUB.mot.GetCmdPos(axis) - PUB.mot.GetActPos(axis));
|
|
|
|
//커맨드위치는 오차가 없어야 한다
|
|
if (offset > 0.001 || offsetReal > inpaccr) //220214 0.01)
|
|
{
|
|
//모션을 옴겨야하는 상황인데 최대시간을 초과했다면 오류로 처리한다
|
|
if (timeoutSec != -1 && stepTime.TotalSeconds >= timeoutSec)
|
|
{
|
|
PUB.Result.SetResultTimeOutMessage(Motaxis, eECode.MOT_CMD, eNextStep.PAUSE, source, pos, PUB.mot.ErrorMessage);
|
|
return false;
|
|
}
|
|
|
|
if (MOT.Move(Motaxis, pos, speed, acc, false, true, useInterLocak) == false)
|
|
{
|
|
Console.WriteLine("move error {0},pos={1} => {2}", Motaxis, pos, PUB.mot.ErrorMessage);
|
|
return false;
|
|
}
|
|
|
|
//모션을 이동시켰으니 False 처리한다
|
|
//return false;
|
|
}
|
|
}
|
|
|
|
//축이 이동중이면 처리하지 않는다.
|
|
if (PUB.mot.IsInp(axis) == false || PUB.mot.IsMotion(axis) == true)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
//X축실위치값 확인
|
|
if (MOT.getPositionMatch(axis, pos, inpaccr) == false)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
#endregion
|
|
|
|
|
|
#region "Get Axis Position"
|
|
|
|
public static sPositionData GetPXPos(ePXLoc pos) { return GetAxPos(eAxis.PX_PICK, (int)pos); }
|
|
public static sPositionData GetPZPos(ePZLoc pos) { return GetAxPos(eAxis.PZ_PICK, (int)pos); }
|
|
public static sPositionData GetLMPos(eLMLoc pos) { return GetAxPos(eAxis.PL_MOVE, (int)pos); }
|
|
public static sPositionData GetLZPos(eLZLoc pos) { return GetAxPos(eAxis.PL_UPDN, (int)pos); }
|
|
public static sPositionData GetRMPos(eRMLoc pos) { return GetAxPos(eAxis.PR_MOVE, (int)pos); }
|
|
public static sPositionData GetRZPos(eRZLoc pos) { return GetAxPos(eAxis.PR_UPDN, (int)pos); }
|
|
public static sPositionData GetPTPos(ePTLoc pos) { return GetAxPos(eAxis.Z_THETA, (int)pos); }
|
|
|
|
|
|
#endregion
|
|
|
|
|
|
|
|
/// <summary>
|
|
/// 지정된 축의 위치값에 속한 이름을 반환합니다.
|
|
/// 특정 구간에 속하지 않은 경우 UNKNOWN 을 반환 합니다
|
|
/// </summary>
|
|
/// <param name="axis"></param>
|
|
/// <param name="Pos"></param>
|
|
/// <returns></returns>
|
|
public static String GetPosName(eAxis axis, double Pos = -1, double CheckOffset = 0.1)
|
|
{
|
|
//홈을 잡지 않았다면 오류로 처리함\
|
|
//eYPPosition retval = eYPPosition.Unknown;
|
|
|
|
var motIndex = (short)axis;
|
|
|
|
if (PUB.mot.IsInit == false) return "ERROR";// (T)ePickYPosition.ERROR; //200213
|
|
if (PUB.mot.IsHomeSet(motIndex) == false) return "ERROR";//ePickYPosition.ERROR;
|
|
if (PUB.Result == null || PUB.Result.isSetmModel == false) return "ERROR";//return ePickYPosition.ERROR;
|
|
|
|
if (PUB.mot.IsUse(motIndex) == false) return "DISABLE";
|
|
else if (PUB.mot.IsServAlarm(motIndex)) return "SVALM";
|
|
else if (PUB.mot.IsServOn(motIndex) == false) return "SVOFF";
|
|
else if (PUB.mot.IsLimitN(motIndex)) return "LIMITN";
|
|
else if (PUB.mot.IsLimitP(motIndex)) return "LIMITP";
|
|
else if (PUB.mot.IsLimitSWN(motIndex)) return "LIMITN(SW)";
|
|
else if (PUB.mot.IsLimitSWP(motIndex)) return "LIMITP(SW)";
|
|
else if (PUB.mot.IsOrg(motIndex)) return "HOME";
|
|
else
|
|
{
|
|
//특정위치에있는지 확인한다. match 명령사용
|
|
if (Pos == -1) Pos = PUB.mot.GetActPos(motIndex);
|
|
var PosT = 0.0;
|
|
|
|
//해당 모션이 속한 좌표를 모두 가져온다
|
|
var pts = PUB.Result.mModel.Position[motIndex].OrderBy(t => t.value).ToList();
|
|
|
|
//각위치별값을 확인한다.
|
|
for (int i = 0; i < pts.Count; i++)
|
|
{
|
|
var pValue = pts[i];
|
|
if (Math.Abs(pValue.value - Pos) <= CheckOffset)
|
|
{
|
|
//해당위치에 있다i
|
|
if (axis == eAxis.PX_PICK) return ((ePXLoc)i).ToString();
|
|
else if (axis == eAxis.PZ_PICK) return ((ePZLoc)i).ToString();
|
|
else if (axis == eAxis.Z_THETA) return ((ePTLoc)i).ToString();
|
|
|
|
else if (axis == eAxis.PL_MOVE) return ((eLMLoc)i).ToString();
|
|
else if (axis == eAxis.PL_UPDN) return ((eLZLoc)i).ToString();
|
|
|
|
else if (axis == eAxis.PR_MOVE) return ((eRMLoc)i).ToString();
|
|
else if (axis == eAxis.PR_UPDN) return ((eRZLoc)i).ToString();
|
|
|
|
else return $"P#{i}";
|
|
}
|
|
}
|
|
|
|
//위치를 찾지 못했다
|
|
if (Math.Abs(Pos) <= CheckOffset) return "ZERO";
|
|
else return "UNKNOWN";
|
|
}
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// Z-L축이 안전위치에있는가?(Ready 보다 위에있으면 안전위치이다)
|
|
/// </summary>
|
|
/// <returns></returns>
|
|
public static Boolean isAxisSaftyZone(eAxis axis, int allowoffset = 2)
|
|
{
|
|
//홈을 잡지 않았다면 오류로 처리함
|
|
var motIndex = (short)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.PX_PICK)
|
|
{
|
|
//피커는 안전위이가 2개 있다
|
|
readypos = GetPXPos(ePXLoc.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.PZ_PICK) readypos = GetPZPos(ePZLoc.READY);
|
|
else if (axis == eAxis.Z_THETA) readypos = GetPTPos(ePTLoc.READY);
|
|
else if (axis == eAxis.PL_MOVE) readypos = GetLMPos(eLMLoc.READY);
|
|
else if (axis == eAxis.PR_MOVE) readypos = GetRMPos(eRMLoc.READY);
|
|
else if (axis == eAxis.PR_UPDN) readypos = GetRZPos(eRZLoc.READY);
|
|
else if (axis == eAxis.PL_UPDN) readypos = GetLZPos(eLZLoc.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_;
|
|
|
|
var CW = false; //홈검색은 항상 뒤로 움직인다.
|
|
if (!Move_Validation(axis, CW, out errorMessage)) retval = false; //이동이 불가한 경우 체크
|
|
else if (axis == eAxis.PX_PICK)
|
|
{
|
|
//Z축 홈이 필요하다
|
|
var zFHome = PUB.mot.IsHomeSet((int)eAxis.PZ_PICK);
|
|
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 axis home is required";
|
|
retval = false;
|
|
}
|
|
}
|
|
else if (axis == eAxis.PR_MOVE) //Z축의 경우 Y축이 홈으로 된 상태여야 한다.
|
|
{
|
|
if (PUB.mot.IsHomeSet((int)eAxis.PR_UPDN) == false)
|
|
{
|
|
errorMessage = "PRINT-R Z axis home is required";
|
|
retval = false;
|
|
}
|
|
}
|
|
return retval;
|
|
}
|
|
|
|
//private static Boolean Move_Validation(eAxis axis, out string errorMessage)
|
|
//{
|
|
// errorMessage = string.Empty;
|
|
|
|
// if (DIO.IsEmergencyOn() == true)
|
|
// {
|
|
// errorMessage = ("비상정지 상태일때에는 움직일 수 없습니다.");
|
|
// return false;
|
|
// }
|
|
|
|
// return true;
|
|
//}
|
|
private static Boolean Move_Validation(eAxis axis, Boolean MoveCW, out string errorMessage)
|
|
{
|
|
errorMessage = string.Empty;
|
|
|
|
if (DIO.IsEmergencyOn() == true)
|
|
{
|
|
errorMessage = ("Cannot move when in emergency stop state.");
|
|
return false;
|
|
}
|
|
|
|
//홈이 잡힌상태일때
|
|
if (PUB.mot.IsHomeSet((short)axis))
|
|
{
|
|
if (axis == eAxis.PX_PICK)
|
|
{
|
|
var Pos = MOT.GetPXPos(ePXLoc.PICKON);
|
|
var PosOffset = MOT.getPositionOffset(Pos);
|
|
|
|
if(AR.SETTING.Data.Log_Debug)
|
|
PUB.logDbg.Add($"X-axis jog center offset:{PosOffset:N3}");
|
|
|
|
if (MoveCW==false && PosOffset < -1) //좌측위치로 이동하는 경우
|
|
{
|
|
if(PUB.mot.IsHomeSet((int)eAxis.PL_MOVE))
|
|
{
|
|
var PosY = MOT.GetLMPos(eLMLoc.READY);
|
|
var PosYOffse = MOT.getPositionOffset(PosY);
|
|
if(PosYOffse < -1)
|
|
{
|
|
//프린터 Y축이 더 안쪽으로 들어와있으니 충돌할 수 있다.
|
|
errorMessage = "Possible collision with Print(L) axis";
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
else if(MoveCW && PosOffset > 1) //우측으로 이동하는 경우
|
|
{
|
|
if (PUB.mot.IsHomeSet((int)eAxis.PR_MOVE))
|
|
{
|
|
var PosY = MOT.GetRMPos(eRMLoc.READY);
|
|
var PosYOffse = MOT.getPositionOffset(PosY);
|
|
if (PosYOffse < -1)
|
|
{
|
|
//프린터 Y축이 더 안쪽으로 들어와있으니 충돌할 수 있다.
|
|
errorMessage = "Possible collision with Print(R) axis";
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
#region "Common Util"
|
|
|
|
/// <summary>
|
|
/// 지정한 축의 모델정보에서 해당 위치값을 찾아 반환 합니다
|
|
/// </summary>
|
|
/// <param name="axis"></param>
|
|
/// <param name="pos"></param>
|
|
/// <returns></returns>
|
|
private static sPositionData GetAxPos(eAxis axis, int pos)
|
|
{
|
|
return GetAxpos((short)axis, pos);
|
|
}
|
|
private static sPositionData GetAxpos(short axis, int pos)
|
|
{
|
|
var retval = new sPositionData();
|
|
retval.Clear();
|
|
if (PUB.Result.mModel == null || PUB.Result.mModel.isSet == false)
|
|
{
|
|
retval.Message = "Motion model is not set";
|
|
return retval;
|
|
}
|
|
|
|
if(axis >= PUB.Result.mModel.Position.Count)
|
|
{
|
|
retval.Message = $"Motion model (axis) information not found ({axis}/{PUB.Result.mModel.Position.Count})";
|
|
return retval;
|
|
}
|
|
|
|
if (pos >= PUB.Result.mModel.Position[axis].Length)
|
|
{
|
|
retval.Message = $"Motion model (position) information not found ({pos}/{PUB.Result.mModel.Position[axis].Length})";
|
|
return retval;
|
|
}
|
|
|
|
var data = PUB.Result.mModel.Position[axis][pos];
|
|
if (data.index == -1)
|
|
{
|
|
retval.Message = string.Format("Value for axis:{0} position:{1} does not exist", axis, pos);
|
|
return retval;
|
|
}
|
|
retval.Axis = axis; //220301
|
|
retval.Position = data.value;
|
|
retval.Speed = data.speed;
|
|
retval.Acc = data.acc;
|
|
|
|
|
|
//환경설정에서 저속모드로 설정했다면 지정된 속도로만 처리한다
|
|
if (AR.SETTING.Data.Enable_SpeedLimit == true)
|
|
retval.Speed = Math.Min(retval.Speed, AR.SETTING.Data.LimitSpeed);
|
|
|
|
////시스템설정의 속도 체크 220524
|
|
//var maxspeed = SETTING.System.GetMaxSpeed;
|
|
//var motidx = (int)axis;
|
|
//if (motidx >= 0 && motidx < maxspeed.Length && maxspeed[motidx] > 0)
|
|
//{
|
|
// retval.Speed = Math.Min(retval.Speed, maxspeed[motidx]);
|
|
//}
|
|
|
|
////시스템설정의 가속도체크
|
|
//var maxAcc = SETTING.System.GetMaxAcc;
|
|
//if (motidx >= 0 && motidx < maxAcc.Length && maxAcc[motidx] > 0)
|
|
//{
|
|
// retval.Acc = Math.Min(retval.Acc, maxAcc[motidx]);
|
|
//}
|
|
|
|
if (data.dcc < 1) retval.Dcc = retval.Acc;
|
|
else retval.Dcc = data.dcc;
|
|
|
|
//retval.isError = false;
|
|
retval.Message = string.Empty;
|
|
return retval;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 지정축의 모션을 중지 합니다.
|
|
/// </summary>
|
|
/// <param name="axis"></param>
|
|
/// <param name="reason"></param>
|
|
/// <param name="estop"></param>
|
|
public static void Stop(eAxis axis, string reason, Boolean estop = false)
|
|
{
|
|
if (PUB.mot.IsInit == false) return;
|
|
if (PUB.mot.IsServAlarm((short)axis)) return;
|
|
PUB.mot.MoveStop(reason, (short)axis, estop);
|
|
|
|
}
|
|
|
|
//지정한 옵셋범위에 들어있는지 체크합니다.
|
|
static Boolean MatchPosition(double cpos, double tpos, double diff = 0.1)
|
|
{
|
|
var offset = Math.Abs(tpos - cpos);
|
|
return offset <= diff;
|
|
}
|
|
public static double getPositionOffset(eAxis axis, double cmdPos)
|
|
{
|
|
return getPositionOffset((short)axis, cmdPos);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 현재위치에서 지정된 위치를 뺀 값입니다. +가 반환되면 현재 위치가 지정위치보다 멀리 있다는 뜻입니다.
|
|
/// </summary>
|
|
/// <param name="Pos"></param>
|
|
/// <returns></returns>
|
|
public static double getPositionOffset(sPositionData Pos)
|
|
{
|
|
return getPositionOffset(Pos.Axis, Pos.Position);
|
|
}
|
|
/// <summary>
|
|
/// 현재위치에서 지정된 위치를 뺀 값입니다. +가 반환되면 현재 위치가 지정위치보다 멀리 있다는 뜻입니다.
|
|
/// </summary>
|
|
/// <param name="axis"></param>
|
|
/// <param name="cmdPos"></param>
|
|
/// <returns></returns>
|
|
public static double getPositionOffset(short axis, double cmdPos)
|
|
{
|
|
var coffset = (PUB.mot.GetActPos(axis) - cmdPos);
|
|
return coffset;// return coffset < offset;
|
|
}
|
|
|
|
public static bool getPositionMatch(eAxis axis, double cmdPos, double offset = 0.1)
|
|
{
|
|
return getPositionMatch((short)axis, cmdPos, offset);
|
|
}
|
|
|
|
public static bool getPositionMatch(sPositionData pos, double offset = 0.1)
|
|
{
|
|
return getPositionMatch(pos.Axis, pos.Position, offset);
|
|
}
|
|
|
|
|
|
public static bool getPositionMatch(short axis, double cmdPos, double offset = 0.1)
|
|
{
|
|
var actpos = PUB.mot.GetActPos(axis);
|
|
var coffset = Math.Abs(actpos - cmdPos);
|
|
return coffset <= offset;
|
|
}
|
|
|
|
public static List<string> GetActiveLockList(eAxis axis, CInterLock Lck)
|
|
{
|
|
var locklist = new List<string>();
|
|
|
|
if (Lck.IsEmpty() == false)
|
|
{
|
|
for (int i = 0; i < Lck.Length; i++)
|
|
{
|
|
if (Lck.get(i))
|
|
{
|
|
var vStr = $"[{i}]";
|
|
vStr = ((eILock)i).ToString();
|
|
locklist.Add(vStr);
|
|
}
|
|
}
|
|
}
|
|
return locklist;
|
|
}
|
|
|
|
public static Boolean Home(string reason, eAxis axis_, Boolean useValidcheck = true)
|
|
{
|
|
string Message;
|
|
int axis = (int)axis_;
|
|
if (useValidcheck == true && !Home_Validation(axis_, out Message))
|
|
{
|
|
PUB.mot.RaiseMessage(string.Format("[{0}-Axis] Move Error : {1}", axis, Message), true);
|
|
return false;
|
|
}
|
|
|
|
var HomespdH = PUB.system_mot.GetHomeSpeedHigh;
|
|
var HomespdL = PUB.system_mot.GetHomeSpeedLow;
|
|
var homespdA = PUB.system_mot.GetHomeSpeedAcc;
|
|
var useOrgSensor = PUB.system_mot.GetUseOrigin;
|
|
|
|
//Pub.mot.Home(1, arDev.AzinAxt.eMotionDirection.Negative, arDev.AzinAxt.eSoftLimitStopMode.SStop, COMM.SETTING.Data.HZSpeed, COMM.SETTING.Data.HZAcc);
|
|
//Boolean useOrgSensor = !disable_org[axis]; // (axis_ != eAxis.Marking); //A센서는 ORG가 없으므로 NE센서만으로 처리해야 함
|
|
return PUB.mot.Home(reason, (short)axis, MOTION_DIRECTION.Negative,
|
|
STOPTYPE.EStop,
|
|
HomespdH[axis],
|
|
HomespdL[axis],
|
|
homespdA[axis],
|
|
useOrgSensor[axis]);
|
|
}
|
|
|
|
|
|
private static DateTime[] MotCmdtime = new DateTime[] { DateTime.Now, DateTime.Now, DateTime.Now, DateTime.Now, DateTime.Now, DateTime.Now, DateTime.Now, DateTime.Now };
|
|
private static double[] MotCmdPos = new double[] { 0, 0, 0, 0, 0, 0, 0, 0 };
|
|
|
|
public static Boolean Move(eAxis axis, sPositionData posdata)
|
|
{
|
|
return Move(axis, posdata.Position, posdata.Speed, posdata.Acc);
|
|
}
|
|
|
|
public static Boolean Move(sPositionData posdata)
|
|
{
|
|
return Move((eAxis)posdata.Axis, posdata);
|
|
}
|
|
|
|
public static Boolean Move(eAxis axis, double pos_mm, double speed, double acc = 1000, Boolean relative = false, Boolean validchk = true, Boolean UserInterLock = true)
|
|
{
|
|
|
|
//너무빠른시간 동작하지 못하게 한다
|
|
if (MotCmdPos[(int)axis] == pos_mm)
|
|
{
|
|
var ts = DateTime.Now - MotCmdtime[(int)axis];
|
|
if (ts.TotalMilliseconds < 1000)
|
|
{
|
|
//너무 빠르게 재시도하지 않게 한다 210115
|
|
Console.WriteLine("mot command skip : " + axis.ToString() + "pos:" + pos_mm.ToString() + " too short");
|
|
MotCmdtime[(int)axis] = DateTime.Now.AddMilliseconds(-1500);
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
MotCmdtime[(int)axis] = DateTime.Now;
|
|
MotCmdPos[(int)axis] = pos_mm;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
MotCmdtime[(int)axis] = DateTime.Now;
|
|
MotCmdPos[(int)axis] = pos_mm;
|
|
}
|
|
|
|
//이동유효성검사
|
|
string Message;
|
|
if (validchk == true)
|
|
{
|
|
//현재위치보다 작으면 ccw
|
|
var isCW = pos_mm >= PUB.mot.GetActPos((short)axis);
|
|
if (!Move_Validation(axis, isCW, out Message))
|
|
{
|
|
PUB.log.AddE(string.Format("[{0}-Axis] Move Error : {1}", axis, Message));
|
|
//Pub.mot.RaiseMessage(, true);
|
|
return false;
|
|
}
|
|
}
|
|
|
|
//해당 축 ILOCK체크
|
|
if (UserInterLock)
|
|
{
|
|
//lock이 걸린경우 lock 걸린 항목을 모두 확인하면 좋다
|
|
var ilock = PUB.iLock[(int)axis];
|
|
if (ilock.IsEmpty() == false)
|
|
{
|
|
var locklist = MOT.GetActiveLockList(axis, ilock);
|
|
PUB.mot.ErrorMessage = $"{ilock.Tag} Interlock(" + string.Join(",", locklist) + ")";
|
|
//PUB.Result.SetResultMessage(eResult.MOTION, eECode.INTERLOCK, eNextStep.PAUSE, PUB.mot.ErrorMessage);
|
|
return false;
|
|
}
|
|
}
|
|
|
|
var pos_pulse = pos_mm;// *10;
|
|
var result = PUB.mot.Move((short)axis, pos_pulse, speed, acc, acc, relative, false, validchk);
|
|
if (!result) PUB.mot.RaiseMessage("Move(X) Error : " + PUB.mot.ErrorMessage, true);
|
|
return result;
|
|
}
|
|
|
|
|
|
public static Boolean JOG(short _Axis, MOTION_DIRECTION Dir, double _Vel, double _Acc, double _Dec, Boolean sCurve = false, Boolean EnableValidCheck = true, Boolean UserInterLock = true)
|
|
{
|
|
eAxis axis = (eAxis)_Axis;
|
|
|
|
//웨이퍼가 감지되는 상태일때 OPEN 되어있다면 이동하지 못하게 한다
|
|
if (EnableValidCheck == true)
|
|
{
|
|
var curpost = PUB.mot.GetActPos(_Axis);
|
|
var IsMoveCW = false;
|
|
|
|
IsMoveCW = Dir == MOTION_DIRECTION.Positive;
|
|
|
|
if (!Move_Validation(axis, IsMoveCW, out string Message))
|
|
{
|
|
PUB.log.AddE(string.Format("[{0}-Axis] JOG Error : {1}", axis, Message));
|
|
return false;
|
|
}
|
|
}
|
|
else PUB.log.AddAT($"Validation check disabled during jog movement (axis:{axis})");
|
|
|
|
//해당 축 ILOCK체크
|
|
if (UserInterLock)
|
|
{
|
|
//lock이 걸린경우 lock 걸린 항목을 모두 확인하면 좋다
|
|
if (PUB.iLock[_Axis].IsEmpty() == false)
|
|
{
|
|
var locklist = MOT.GetActiveLockList(axis, PUB.iLock[_Axis]);
|
|
PUB.mot.ErrorMessage = $"{axis} Interlock(" + string.Join(",", locklist) + ")";
|
|
return false;
|
|
}
|
|
}
|
|
else PUB.log.AddAT($"Interlock check disabled during jog movement (axis:{axis})");
|
|
|
|
|
|
return PUB.mot.JOG(_Axis, Dir, _Vel, _Acc, sCurve, EnableValidCheck);
|
|
}
|
|
|
|
|
|
|
|
|
|
#endregion
|
|
}
|
|
}
|