363 lines
14 KiB
C#
363 lines
14 KiB
C#
/*
|
|
* Copyright (c) 2025 LG Electronics Inc.
|
|
* SPDX-License-Identifier: LicenseRef-LGE-Proprietary
|
|
*/
|
|
using Aurora.Api;
|
|
using Aurora.Api.Devices;
|
|
using VisionAligner;
|
|
using VisionCalibrator;
|
|
using VisionCommon;
|
|
using Hermes;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Threading;
|
|
using VisionSequence.Pattern;
|
|
using VisionCommonUI;
|
|
using VisionCommonUI.Resources;
|
|
using System.Linq;
|
|
using VisionDirector;
|
|
|
|
namespace VisionSequence.Setup {
|
|
/// <summary>
|
|
/// 센터 튜닝 셋업 시퀀스 요청 파라미터
|
|
/// </summary>
|
|
public class SetupSeqParams_CenterTuning {
|
|
/// <summary>서브 시스템 정보</summary>
|
|
public SubSysInfo SubSysInfo { get; set; }
|
|
/// <summary>센터 튜닝 레시피</summary>
|
|
public CenterTuningRecipe CenterTuningRecipe { get; set; }
|
|
/// <summary>메인 화면 타입</summary>
|
|
public eDisplayType DisplayType { get; set; }
|
|
|
|
/// <summary>얼라인 델타를 로그 뷰에 출력하기 위한 콜백</summary>
|
|
public Action<Position> CallBackAlignDelta;
|
|
|
|
/// <summary>
|
|
/// 생성자
|
|
/// </summary>
|
|
/// <param name="subSysInfo">서브 시스템 정보</param>
|
|
/// <param name="centerTuningRecipe">센터 튜닝 레시피</param>
|
|
/// <param name="displayType">메인 화면 타입</param>
|
|
public SetupSeqParams_CenterTuning(
|
|
SubSysInfo subSysInfo,
|
|
CenterTuningRecipe centerTuningRecipe,
|
|
eDisplayType displayType = eDisplayType.CenterTuning) {
|
|
this.SubSysInfo = subSysInfo;
|
|
this.CenterTuningRecipe = centerTuningRecipe;
|
|
this.DisplayType = displayType;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 반복 얼라인을 통해 화면 중심에 패턴이 오도록 튜닝
|
|
/// </summary>
|
|
public class SetupSeq_CenterTuning {
|
|
private readonly Logger LOG = new Logger(typeof(SetupSeq_CenterTuning).Name);
|
|
private SetupSeqParams_CenterTuning _Params;
|
|
private ICameraDisplay _CameraDisplay;
|
|
private SubSysRecipe _SubSysRecipe;
|
|
private FovInfo _FovInfo;
|
|
private FovRecipe _FovRecipe;
|
|
private PatternAgentBase _PatternAgent;
|
|
private ImageManager _ImageManager = new ImageManager();
|
|
|
|
private int _CurrentCnt = 0;
|
|
|
|
#region Sequence Id
|
|
private const string SEQ_ID_INIT = "INIT";
|
|
private const string SEQ_ID_MOVE_TEACHING_POS = "MOVE_TEACHING_POS";
|
|
private const string SEQ_ID_SET_GRAB_FIND_PROC_SEQ_PARAMS = "SET_GRAB_FIND_PROC_SEQ_PARAMS";
|
|
private const string SEQ_ID_GRAB_FIND_PROC_SEQ = "GRAB_FIND_PROC_SEQ";
|
|
private const string SEQ_ID_SAVE_IMAGE = "SEQ_ID_SAVE_IMAGE";
|
|
private const string SEQ_ID_CHECK_FOUND_STATUS = "CHECK_FOUND_STATUS";
|
|
private const string SEQ_ID_ALIGN = "ALIGN";
|
|
private const string SEQ_ID_MOVE_CENTER = "MOVE_CENTER";
|
|
private const string SEQ_ID_CHECK_INRANGE = "CHECK_INRANGE";
|
|
private const string SEQ_ID_SEQ_COMPLETE = "SEQ_COMPLETE";
|
|
#endregion
|
|
|
|
private void SetActions(SequenceBuilder sb) {
|
|
SequenceBuilder grabFindProcSeqBuilder = SubSeqManager.CreateSubSeqBuilder(new GrabFindProcSeq(), "GrabFindProcSeq");
|
|
|
|
sb.AddAction(SEQ_ID_INIT, Init);
|
|
sb.AddAction(SEQ_ID_MOVE_TEACHING_POS, MoveTeachingPos);
|
|
sb.AddAction(SEQ_ID_SET_GRAB_FIND_PROC_SEQ_PARAMS, SetGrabFindProcSeqParams);
|
|
sb.AddSubsequence(SEQ_ID_GRAB_FIND_PROC_SEQ, grabFindProcSeqBuilder);
|
|
sb.AddAction(SEQ_ID_SAVE_IMAGE, SaveImage);
|
|
sb.AddDecision(SEQ_ID_CHECK_FOUND_STATUS, CheckFoundStatus);
|
|
sb.AddAction(SEQ_ID_ALIGN, Align);
|
|
sb.AddDecision(SEQ_ID_CHECK_INRANGE, CheckInrange);
|
|
sb.AddAction(SEQ_ID_MOVE_CENTER, MoveCenter);
|
|
sb.AddAction(SEQ_ID_SEQ_COMPLETE, SeqComplete);
|
|
}
|
|
|
|
public SequenceBuilder Builder() {
|
|
SequenceBuilder sb = new SequenceBuilder(1000);
|
|
SetActions(sb);
|
|
sb.OnStopped(OnStopped);
|
|
sb.OnForceError(OnForceError);
|
|
|
|
sb.Flows(
|
|
sb.Init
|
|
> sb[SEQ_ID_INIT]
|
|
> sb[SEQ_ID_MOVE_TEACHING_POS]
|
|
> sb[SEQ_ID_SET_GRAB_FIND_PROC_SEQ_PARAMS]
|
|
> sb[SEQ_ID_GRAB_FIND_PROC_SEQ]
|
|
> sb[SEQ_ID_SAVE_IMAGE]
|
|
> sb[SEQ_ID_CHECK_FOUND_STATUS]
|
|
> sb[SEQ_ID_ALIGN]
|
|
> sb[SEQ_ID_CHECK_INRANGE]
|
|
);
|
|
sb.Flows(
|
|
sb[SEQ_ID_CHECK_FOUND_STATUS]
|
|
> sb[SEQ_ID_SEQ_COMPLETE]
|
|
);
|
|
sb.Flows(
|
|
sb[SEQ_ID_CHECK_INRANGE]
|
|
> sb[SEQ_ID_MOVE_CENTER]
|
|
> sb[SEQ_ID_SET_GRAB_FIND_PROC_SEQ_PARAMS]
|
|
);
|
|
|
|
sb.Flows(
|
|
sb[SEQ_ID_CHECK_INRANGE]
|
|
> sb[SEQ_ID_SEQ_COMPLETE]
|
|
> sb.Final
|
|
);
|
|
|
|
#if SEQ_DRAW
|
|
sb.Draw($"{GetType().Name}.pdf");
|
|
#endif
|
|
|
|
return sb;
|
|
}
|
|
|
|
private SequenceActionResult Init(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
if (param.Param != null) {
|
|
_Params = param.Param as SetupSeqParams_CenterTuning;
|
|
}
|
|
else {
|
|
LOG.E($"Param is null.");
|
|
return null;
|
|
}
|
|
|
|
_CurrentCnt = 0;
|
|
_FovInfo = new FovInfo(_Params.SubSysInfo, eFovId.Fov1, eFovId.Fov1.ToString());
|
|
_SubSysRecipe = VisionRecipeManager.GetSubSysRecipe(_FovInfo.UnitId, _FovInfo.SubSysId);
|
|
_FovRecipe = VisionRecipeManager.GetFovRecipe(_FovInfo);
|
|
_PatternAgent = PatternAgentManager.GetAgent(_FovInfo);
|
|
_CameraDisplay = CameraDisplayManager.GetDisplay(_FovInfo, _Params.DisplayType);
|
|
|
|
GrabFindSeqUtil.Clear(_Params.SubSysInfo);
|
|
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
private SequenceActionResult MoveTeachingPos(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
// 하나의 서브시스템에 Fov가 4개라면? 어떤 FovId를 넘겨야 하는가? (4점 얼라인)
|
|
int res = EquipAdapter.Instance.TeachingMotionManager.MovePosition(_FovInfo, ePositionType.Teaching);
|
|
if (res == E1.OK) {
|
|
return SequenceAction.Ok();
|
|
}
|
|
else {
|
|
return SequenceAction.Error(E2.ABORTED);
|
|
}
|
|
}
|
|
|
|
private SequenceActionResult SetGrabFindProcSeqParams(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
var procPatternInfo = VisionRecipeManager.GetProcPatternInfo(_FovInfo, "Inspect", eProcPatternId.ProcPattern1);
|
|
var patternInfo = _PatternAgent.GetPatternInfo(_FovInfo, procPatternInfo.PatternId);
|
|
|
|
patternInfo.PatternName = procPatternInfo.PatternName;
|
|
eSeqMode seqMode = eSeqMode.Manual; // 그랩을 Skip하지 않기 위함.
|
|
|
|
return SequenceAction.Ok(new GrabFindProcSeqParams(_FovInfo, patternInfo, _Params.DisplayType, seqMode));
|
|
}
|
|
|
|
private SequenceActionResult SaveImage(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
if (VisionSettingManager.VisionSetting.SaveSetUpImage) {
|
|
var display = CameraDisplayManager.GetDisplay(_FovInfo, _Params.DisplayType);
|
|
object image = display.GetImageObject();
|
|
|
|
if (image != null) {
|
|
_ImageManager.SaveImageInternal(_FovInfo, eSaveDataType.CenterTuning, image);
|
|
}
|
|
}
|
|
|
|
var patternResult = param.Param as PatternResult;
|
|
|
|
return SequenceAction.Ok(patternResult);
|
|
}
|
|
|
|
private string CheckFoundStatus(
|
|
SequenceContext context,
|
|
string id,
|
|
ICollection<string> branches,
|
|
SequenceActionResult param) {
|
|
|
|
var patternResult = param.Param as PatternResult;
|
|
|
|
if (patternResult.Status == eFoundStatus.FoundOK) {
|
|
return SEQ_ID_ALIGN;
|
|
}
|
|
else {
|
|
var alignInfo = VisionRecipeManager.GetAlignInfo(_Params.SubSysInfo.UnitId, _Params.SubSysInfo.SubSysId, eAlignId.Align1);
|
|
context.PutValue<AlignResult>(
|
|
string.Format(VisionContextKeys.CNTXT_KEY_ALIGN_RESULT, alignInfo.GetKey()), new AlignResult() {
|
|
AlignName = patternResult.PatternName,
|
|
Status = AlignStatus.NOT_FOUND,
|
|
AlignAlgorithm = _SubSysRecipe.AlignRecipes[Convert.ToInt32(alignInfo.AlignId)].ProcParams.AlignAlgorithm
|
|
});
|
|
return SEQ_ID_SEQ_COMPLETE;
|
|
}
|
|
}
|
|
|
|
// Align Params
|
|
private SequenceActionResult Align(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) { // param : PatternResult
|
|
|
|
var patternResult = param.Param as PatternResult;
|
|
|
|
var refPoint = _FovRecipe.CalFovRecipe.FovCenter + _FovRecipe.RefPointRecipe.Offset;
|
|
|
|
var meaPoint = patternResult.Results.ToList()[0].MaterialPosition.Clone();
|
|
var meaAngle = patternResult.Results.ToList()[0].Angle.Clone();
|
|
|
|
// Align
|
|
var alignInfo = VisionRecipeManager.GetAlignInfo(_Params.SubSysInfo.UnitId, _Params.SubSysInfo.SubSysId, eAlignId.Align1);
|
|
var aligner = AlignAgentManager.GetAlignAgent(alignInfo);
|
|
var calibrator = CalibratorManager.GetCalibrator(new FovInfo(_Params.SubSysInfo, eFovId.Fov1));
|
|
var alignResult = aligner.Align(meaPoint, refPoint, (Angle)meaAngle, (int)calibrator.GetFlipY());
|
|
|
|
if (alignResult == null) {
|
|
MessageBoxUtil.ShowMessageBoxError(
|
|
string.Format(StrTokens.MSG_E_AlignResultIsNull));
|
|
return SequenceAction.Error();
|
|
}
|
|
else {
|
|
context.PutValue(string.Format(VisionContextKeys.CNTXT_KEY_ALIGN_RESULT, alignInfo.GetKey()), alignResult);
|
|
}
|
|
|
|
return SequenceAction.Ok(alignResult);
|
|
}
|
|
|
|
private string CheckInrange(
|
|
SequenceContext context,
|
|
string id,
|
|
ICollection<string> branches,
|
|
SequenceActionResult param) { // Param : AlignResult
|
|
|
|
_CurrentCnt++;
|
|
if (_CurrentCnt > _Params.CenterTuningRecipe.MaxCnt) {
|
|
return SEQ_ID_SEQ_COMPLETE;
|
|
}
|
|
|
|
//var alignResult = param.Param as AlignResult;
|
|
var alignInfo = VisionRecipeManager.GetAlignInfo(_Params.SubSysInfo.UnitId, _Params.SubSysInfo.SubSysId, eAlignId.Align1);
|
|
var alignResult = context.GetValue<AlignResult>(string.Format(VisionContextKeys.CNTXT_KEY_ALIGN_RESULT, alignInfo.GetKey()));
|
|
var alignDelta = new Position() { X = alignResult.Delta.X, Y = alignResult.Delta.Y, T = alignResult.Delta.T.Degree };
|
|
_Params.CallBackAlignDelta(alignDelta);
|
|
|
|
if (_Params.CenterTuningRecipe.Spec.T == 0) {
|
|
|
|
if (Math.Abs(_Params.CenterTuningRecipe.Spec.X) > Math.Abs(alignResult.Delta.X) &&
|
|
Math.Abs(_Params.CenterTuningRecipe.Spec.Y) > Math.Abs(alignResult.Delta.Y)) {
|
|
return SEQ_ID_SEQ_COMPLETE;
|
|
}
|
|
else {
|
|
return SEQ_ID_MOVE_CENTER;
|
|
}
|
|
}
|
|
else {
|
|
if (Math.Abs(_Params.CenterTuningRecipe.Spec.X) > Math.Abs(alignResult.Delta.X) &&
|
|
Math.Abs(_Params.CenterTuningRecipe.Spec.Y) > Math.Abs(alignResult.Delta.Y) &&
|
|
Math.Abs(_Params.CenterTuningRecipe.Spec.T) > Math.Abs(alignResult.Delta.T.Degree)) {
|
|
return SEQ_ID_SEQ_COMPLETE;
|
|
}
|
|
else {
|
|
return SEQ_ID_MOVE_CENTER;
|
|
}
|
|
}
|
|
}
|
|
|
|
private SequenceActionResult MoveCenter(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
var alignInfo = VisionRecipeManager.GetAlignInfo(_Params.SubSysInfo.UnitId, _Params.SubSysInfo.SubSysId, eAlignId.Align1);
|
|
var alignResult = context.GetValue<AlignResult>(string.Format(VisionContextKeys.CNTXT_KEY_ALIGN_RESULT, alignInfo.GetKey()));
|
|
|
|
if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
|
|
Thread.Sleep(300);
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
var delta = new Position() { X = alignResult.Delta.X, Y = alignResult.Delta.Y };
|
|
if (_Params.CenterTuningRecipe.Spec.T != 0) {
|
|
delta.T = alignResult.Delta.T.Degree;
|
|
}
|
|
|
|
var teachingDirector = TeachingDirectorManager.Instance.GetDirector(_FovInfo.ToSubSysInfo());
|
|
var motionDelta = teachingDirector.ApplyDirection(delta);
|
|
|
|
var res = EquipAdapter.Instance.TeachingMotionManager.RelMove(_FovInfo, motionDelta);
|
|
if (res != E1.OK) {
|
|
return SequenceAction.Error(E2.ABORTED);
|
|
}
|
|
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
private SequenceActionResult SeqComplete(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
if (_CurrentCnt > _Params.CenterTuningRecipe.MaxCnt) {
|
|
return SequenceAction.Ok(_Params.CenterTuningRecipe.MaxCnt);
|
|
}
|
|
else {
|
|
return SequenceAction.Ok(_CurrentCnt);
|
|
}
|
|
}
|
|
|
|
void OnStopped(
|
|
SequenceContext context) {
|
|
LOG.I($"{context.SequenceId} - STOP Handler");
|
|
HandleAbnormalFinish(context);
|
|
}
|
|
|
|
void OnForceError(
|
|
SequenceContext context, string id) {
|
|
LOG.I($"{context.SequenceId} - FORCE ERROR Handler");
|
|
HandleAbnormalFinish(context);
|
|
}
|
|
|
|
void HandleAbnormalFinish(
|
|
SequenceContext context) {
|
|
}
|
|
}
|
|
}
|