721 lines
26 KiB
C#
721 lines
26 KiB
C#
/*
|
|
* Copyright (c) 2025 LG Electronics Inc.
|
|
* SPDX-License-Identifier: LicenseRef-LGE-Proprietary
|
|
*/
|
|
using Aurora.Api;
|
|
using Aurora.Api.DataType;
|
|
using Aurora.Api.Devices;
|
|
using VisionCommon;
|
|
using Hermes;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Reactive.Linq;
|
|
using System.Threading;
|
|
using System.Windows;
|
|
using VisionCommonUI;
|
|
using VisionCommonUI.Resources;
|
|
|
|
namespace VisionSequence.Setup {
|
|
/// <summary>
|
|
/// 자동 초점(축) 모드
|
|
/// </summary>
|
|
public enum eFocusMode {
|
|
/// <summary>대략적으로</summary>
|
|
Roughly,
|
|
/// <summary>섬세하게</summary>
|
|
Delicately,
|
|
/// <summary>종료</summary>
|
|
Finished
|
|
}
|
|
|
|
/// <summary>
|
|
/// 자동 초점(축) 로그
|
|
/// </summary>
|
|
public class AutoFocusLog : ICloneable {
|
|
/// <summary>공정 시간</summary>
|
|
public string Time { get; set; }
|
|
/// <summary>스텝 번호</summary>
|
|
public int Step { get; set; }
|
|
/// <summary>자동 초점(축) 모드</summary>
|
|
public string Mode { get; set; }
|
|
/// <summary>Sharpness 측정 값</summary>
|
|
public int Sharpness { get; set; }
|
|
/// <summary>Z축 위치</summary>
|
|
public double PosZ { get; set; }
|
|
|
|
/// <summary>
|
|
/// Clone (Copy)
|
|
/// </summary>
|
|
/// <returns>자동 초점(축) 로그</returns>
|
|
public object Clone() {
|
|
return new AutoFocusLog() {
|
|
Time = Time,
|
|
Step = this.Step,
|
|
Mode = this.Mode,
|
|
Sharpness = this.Sharpness,
|
|
PosZ = this.PosZ
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// 생성자
|
|
/// </summary>
|
|
public AutoFocusLog() { }
|
|
|
|
/// <summary>
|
|
/// 생성자
|
|
/// </summary>
|
|
/// <param name="step">스텝 번호</param>
|
|
/// <param name="mode">자동 초점(축) 모드</param>
|
|
/// <param name="sharpness">Sharpness 측정 값</param>
|
|
/// <param name="posZ">Z축 위치</param>
|
|
public AutoFocusLog(int step, eFocusMode mode, int sharpness, double posZ) {
|
|
Step = step;
|
|
Mode = mode.ToString();
|
|
Sharpness = sharpness;
|
|
PosZ = posZ;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 자동 초첨(축) 로그 매니저
|
|
/// </summary>
|
|
public class AutoFocusLogManager : ItemManagerBase {
|
|
private static readonly object _Lock = new object();
|
|
|
|
/// <summary>
|
|
/// 자동 초첨(축) 로그에 공정시간을 더해 리턴한다.
|
|
/// </summary>
|
|
/// <param name="subSysInfo">서브 시스템 키 정보</param>
|
|
/// <param name="log">자동 초첨(축) 로그</param>
|
|
/// <returns>자동 초첨(축) 로그</returns>
|
|
public static AutoFocusLog GetAutoFocusLog(SubSysInfo subSysInfo, AutoFocusLog log) {
|
|
lock (_Lock) {
|
|
return new AutoFocusLog() {
|
|
Time = GetTimeStamp(subSysInfo, log),
|
|
Step = log.Step,
|
|
Mode = log.Mode,
|
|
Sharpness = log.Sharpness,
|
|
PosZ = log.PosZ
|
|
};
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 자동 초점(축) 셋업 시퀀스 요청 파라미터
|
|
/// </summary>
|
|
public class SetupSeqParams_AutoFocus {
|
|
/// <summary>FOV 키 정보</summary>
|
|
public FovInfo FovInfo { get; set; }
|
|
/// <summary>자동 초점(축) 레시피</summary>
|
|
public AutoFocusRecipe AutoFocusRecipe { get; set; }
|
|
/// <summary>메인 화면 타입</summary>
|
|
public eDisplayType DisplayType { get; set; }
|
|
|
|
/// <summary>
|
|
/// 자동 초점(축) 셋업 시퀀스 요청 파라미터
|
|
/// </summary>
|
|
/// <param name="fovInfo">FOV 키 정보</param>
|
|
/// <param name="autoFocusRecipe">자동 초점(축) 레시피</param>
|
|
/// <param name="displayType">메인 화면 타입</param>
|
|
public SetupSeqParams_AutoFocus(
|
|
FovInfo fovInfo,
|
|
AutoFocusRecipe autoFocusRecipe,
|
|
eDisplayType displayType = eDisplayType.AutoFocus) {
|
|
this.FovInfo = fovInfo;
|
|
this.AutoFocusRecipe = autoFocusRecipe;
|
|
this.DisplayType = displayType;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 자동 초점(축) 셋업 시퀀스 결과 파라미터
|
|
/// </summary>
|
|
public class SetupSeqResultParams_AutoFocus {
|
|
/// <summary>최적의 스텝 번호</summary>
|
|
public int BestStep { get; set; }
|
|
/// <summary>최적의 Sharpness 측정 값</summary>
|
|
public int BestSharpness { get; set; }
|
|
/// <summary>최적의 Z축 위치</summary>
|
|
public double BestPosZ { get; set; }
|
|
|
|
/// <summary>
|
|
/// 생성자
|
|
/// </summary>
|
|
/// <param name="bestStep">최적의 스텝 번호</param>
|
|
/// <param name="bestSharpness">최적의 Sharpness 측정 값</param>
|
|
/// <param name="bestPosZ">최적의 Z축 위치</param>
|
|
public SetupSeqResultParams_AutoFocus(int bestStep, int bestSharpness, double bestPosZ) {
|
|
this.BestStep = bestStep;
|
|
this.BestSharpness = bestSharpness;
|
|
this.BestPosZ = bestPosZ;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 자동 초점(축) 조절
|
|
/// </summary>
|
|
public class SetupSeq_AutoFocus {
|
|
private readonly Logger LOG = new Logger(typeof(SetupSeq_AutoFocus).Name);
|
|
private SetupSeqParams_AutoFocus _Params;
|
|
private FovRecipe _FovRecipe;
|
|
private CamConfig _CamConfig;
|
|
private CameraAgentBase _CameraAgent;
|
|
private ICameraDisplay _CameraDisplay;
|
|
private CsvManager _CsvManager;
|
|
private ImageManager _ImageManager = new ImageManager();
|
|
|
|
private List<AutoFocusLog> AutoFocusLogList = new List<AutoFocusLog>();
|
|
private eFocusMode _FocusMode = eFocusMode.Finished;
|
|
|
|
private double _MoveBackStepValue = 1.5;
|
|
private double _MoveStepValue = 0.1;
|
|
private double _MoveStepMaxCnt = 30;
|
|
|
|
private bool _IsObservableSet = false;
|
|
private int _CurrentStep;
|
|
private int _CurrentSharpness;
|
|
private double _CurrentPositionZ;
|
|
|
|
private object _GrabImage = null;
|
|
|
|
#region Sequence Id
|
|
private const string SEQ_ID_INIT = "INIT";
|
|
private const string SEQ_ID_MOVE_TEACHING_POSITION = "MOVE_TEACHING_POSITION";
|
|
private const string SEQ_ID_CHECK_FOCUS_MODE = "CHECK_FOCUS_MODE";
|
|
private const string SEQ_ID_CLEAR = "CLEAR";
|
|
private const string SEQ_ID_MOVE_BACK_STEP = "MOVE_BACK_STEP";
|
|
private const string SEQ_ID_MOVE_BEST_POS = "MOVE_BEST_POS";
|
|
private const string SEQ_ID_SAVE_BEST_POS = "SAVE_BEST_POS";
|
|
private const string SEQ_ID_MOVE_STEP = "MOVE_STEP";
|
|
private const string SEQ_ID_GRAB = "GRAB";
|
|
private const string SEQ_ID_SAVE_IMAGE = "SAVE_IMAGE";
|
|
private const string SEQ_ID_FINAL_GRAB = "FINAL_GRAB";
|
|
private const string SEQ_ID_MEASURE_SHARPNESS = "MEASURE_SHARPNESS";
|
|
private const string SEQ_ID_MEASURE_NG = "MEASURE_NG";
|
|
private const string SEQ_ID_MOVE_FINAL_POSITION = "MOVE_FINAL_POSITION";
|
|
private const string SEQ_ID_SEQ_COMPLETE = "SEQ_COMPLETE";
|
|
#endregion
|
|
|
|
/// <summary>
|
|
/// 생성자
|
|
/// </summary>
|
|
public SetupSeq_AutoFocus() {
|
|
}
|
|
|
|
private void SetActions(SequenceBuilder sb) {
|
|
sb.AddAction(SEQ_ID_INIT, Init);
|
|
sb.AddAction(SEQ_ID_MOVE_TEACHING_POSITION, MoveTeachingPos);
|
|
sb.AddDecision(SEQ_ID_CHECK_FOCUS_MODE, CheckFocusMode);
|
|
sb.AddAction(SEQ_ID_CLEAR, Clear);
|
|
sb.AddAction(SEQ_ID_MOVE_BACK_STEP, MoveBackStep);
|
|
sb.AddAction(SEQ_ID_MOVE_STEP, MoveStep);
|
|
sb.AddAction(SEQ_ID_GRAB, Grab);
|
|
sb.AddDecision(SEQ_ID_MEASURE_SHARPNESS, MeasureSharpness);
|
|
sb.AddAction(SEQ_ID_MEASURE_NG, MeasureNG);
|
|
sb.AddAction(SEQ_ID_MOVE_BEST_POS, MoveBestPos);
|
|
sb.AddAction(SEQ_ID_FINAL_GRAB, Grab);
|
|
sb.AddAction(SEQ_ID_SAVE_IMAGE, SaveImage);
|
|
sb.AddAction(SEQ_ID_SAVE_BEST_POS, SaveBestPos);
|
|
sb.AddAction(SEQ_ID_MOVE_FINAL_POSITION, MoveFinalPos);
|
|
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_POSITION]
|
|
> sb[SEQ_ID_CHECK_FOCUS_MODE]
|
|
> sb[SEQ_ID_CLEAR]
|
|
> sb[SEQ_ID_MOVE_BACK_STEP]
|
|
> sb[SEQ_ID_MOVE_STEP]
|
|
> sb[SEQ_ID_GRAB]
|
|
> sb[SEQ_ID_SAVE_IMAGE]
|
|
> sb[SEQ_ID_MEASURE_SHARPNESS]
|
|
);
|
|
|
|
sb.Flows(
|
|
sb[SEQ_ID_CHECK_FOCUS_MODE]
|
|
> sb[SEQ_ID_FINAL_GRAB]
|
|
> sb[SEQ_ID_SAVE_BEST_POS]
|
|
> sb[SEQ_ID_MOVE_FINAL_POSITION]
|
|
> sb[SEQ_ID_SEQ_COMPLETE]
|
|
> sb.Final
|
|
);
|
|
|
|
sb.Flows(
|
|
sb[SEQ_ID_MEASURE_SHARPNESS]
|
|
> sb[SEQ_ID_MEASURE_NG]
|
|
> sb[SEQ_ID_MOVE_FINAL_POSITION]
|
|
> sb[SEQ_ID_SEQ_COMPLETE]
|
|
> sb.Final
|
|
);
|
|
|
|
sb.Flows(
|
|
sb[SEQ_ID_MEASURE_SHARPNESS]
|
|
> sb[SEQ_ID_MOVE_STEP]
|
|
);
|
|
|
|
sb.Flows(
|
|
sb[SEQ_ID_MEASURE_SHARPNESS]
|
|
> sb[SEQ_ID_MOVE_BEST_POS]
|
|
);
|
|
|
|
sb.Flows(
|
|
sb[SEQ_ID_MOVE_BEST_POS]
|
|
> sb[SEQ_ID_CHECK_FOCUS_MODE]
|
|
);
|
|
|
|
#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_AutoFocus;
|
|
}
|
|
else {
|
|
LOG.E($"Param is null.");
|
|
return null;
|
|
}
|
|
|
|
_FocusMode = eFocusMode.Roughly;
|
|
_FovRecipe = VisionRecipeManager.GetFovRecipe(_Params.FovInfo);
|
|
_CamConfig = VisionEnv.AuroraEnv.DeviceManager.
|
|
GetDeviceConfig(_FovRecipe.CamRecipe.CamId.ToString()) as CamConfig;
|
|
_CameraDisplay = CameraDisplayManager.GetDisplay(_Params.FovInfo, _Params.DisplayType);
|
|
_CsvManager = new CsvManager();
|
|
|
|
SetCameraAgent();
|
|
SetObservable();
|
|
InitGrabImageBuffer();
|
|
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
private void SetCameraAgent() {
|
|
_CameraAgent = CameraAgentManager.GetCameraAgent(_Params.FovInfo);
|
|
}
|
|
|
|
private void SetObservable() {
|
|
if (_IsObservableSet) {
|
|
return;
|
|
}
|
|
|
|
if (_FovRecipe != null) {
|
|
VisionEnv.AuroraEnv.EventObservable.AsObservable(VisionEventId.UPDATE_GRAB_IMAGE_BUFFER).
|
|
Where(x => x.From == _FovRecipe.CamRecipe.CamId.ToString()).Subscribe(_ => UpdateGrabImageBuffer());
|
|
_IsObservableSet = true;
|
|
}
|
|
}
|
|
|
|
// 사용하는 곳에서 그랩 버퍼 할당, 디바이스에서는 복사본 전달
|
|
private void InitGrabImageBuffer() {
|
|
if (_GrabImage == null) {
|
|
_CameraAgent = CameraAgentManager.GetCameraAgent(_Params.FovInfo);
|
|
_GrabImage = _CameraAgent.CloneImageBuffer();
|
|
}
|
|
else {
|
|
ImageSize camImageSize = _CameraAgent.GetImageSize();
|
|
ImageSize grabImageSize = IpAdapter.Instance.IpEnvironment.GetImageSize(_GrabImage);
|
|
|
|
bool isSameSize = IpAdapter.Instance.IpEnvironment.ImageSizeEquals(camImageSize, grabImageSize);
|
|
if (!isSameSize) {
|
|
UpdateGrabImageBuffer();
|
|
}
|
|
}
|
|
}
|
|
|
|
// 디바이스 연결시, 이미지 회전이 발생했다면 이미지버퍼 재할당
|
|
private void UpdateGrabImageBuffer() {
|
|
if (_FovRecipe != null) {
|
|
IpAdapter.Instance.IpEnvironment.FreeImage(_GrabImage);
|
|
_GrabImage = null;
|
|
_CameraAgent = CameraAgentManager.GetCameraAgent(_Params.FovInfo);
|
|
_GrabImage = _CameraAgent.CloneImageBuffer();
|
|
}
|
|
}
|
|
|
|
private SequenceActionResult MoveTeachingPos(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
int res = EquipAdapter.Instance.TeachingMotionManager.MovePosition(_Params.FovInfo, ePositionType.Teaching);
|
|
|
|
if (res == E1.OK) {
|
|
return SequenceAction.Ok();
|
|
}
|
|
else {
|
|
return SequenceAction.Error(E2.ABORTED);
|
|
}
|
|
}
|
|
|
|
private string CheckFocusMode(
|
|
SequenceContext context,
|
|
string id,
|
|
ICollection<string> branches,
|
|
SequenceActionResult param) {
|
|
|
|
switch (_FocusMode) {
|
|
case eFocusMode.Roughly:
|
|
_MoveBackStepValue = _Params.AutoFocusRecipe.BackStep_1;
|
|
_MoveStepValue = _Params.AutoFocusRecipe.MoveStep_1;
|
|
_MoveStepMaxCnt = _Params.AutoFocusRecipe.MaxStepCnt_1;
|
|
return SEQ_ID_CLEAR;
|
|
case eFocusMode.Delicately:
|
|
_MoveBackStepValue = _Params.AutoFocusRecipe.BackStep_2;
|
|
_MoveStepValue = _Params.AutoFocusRecipe.MoveStep_2;
|
|
_MoveStepMaxCnt = _Params.AutoFocusRecipe.MaxStepCnt_2;
|
|
return SEQ_ID_CLEAR;
|
|
}
|
|
|
|
return SEQ_ID_FINAL_GRAB;
|
|
}
|
|
|
|
private SequenceActionResult Clear(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
InitParams(context);
|
|
AutoFocusLogList.Clear();
|
|
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
private SequenceActionResult MoveBackStep(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
// Auto Focus 시작 위치로 이동. 현재 위치에서 1.5mm 뒤로 이동
|
|
var moveDelta = new Position();
|
|
switch (_Params.AutoFocusRecipe.Axis) {
|
|
case eAxis.X:
|
|
moveDelta.X = -(int)_Params.AutoFocusRecipe.Direction * _MoveBackStepValue;
|
|
break;
|
|
case eAxis.Y:
|
|
moveDelta.Y = -(int)_Params.AutoFocusRecipe.Direction * _MoveBackStepValue;
|
|
break;
|
|
case eAxis.Z:
|
|
moveDelta.Z = -(int)_Params.AutoFocusRecipe.Direction * _MoveBackStepValue;
|
|
break;
|
|
}
|
|
|
|
var res = EquipAdapter.Instance.TeachingMotionManager.RelMove(_Params.FovInfo, moveDelta);
|
|
if (res == E1.OK) {
|
|
return SequenceAction.Ok();
|
|
}
|
|
else {
|
|
return SequenceAction.Error(E2.ABORTED);
|
|
}
|
|
}
|
|
|
|
private SequenceActionResult MoveStep(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
UpdateParams(context);
|
|
|
|
if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
var moveDelta = new Position();
|
|
switch (_Params.AutoFocusRecipe.Axis) {
|
|
case eAxis.X:
|
|
moveDelta.X = (int)_Params.AutoFocusRecipe.Direction * _MoveStepValue;
|
|
break;
|
|
case eAxis.Y:
|
|
moveDelta.Y = (int)_Params.AutoFocusRecipe.Direction * _MoveStepValue;
|
|
break;
|
|
case eAxis.Z:
|
|
moveDelta.Z = (int)_Params.AutoFocusRecipe.Direction * _MoveStepValue;
|
|
break;
|
|
}
|
|
|
|
var res = EquipAdapter.Instance.TeachingMotionManager.RelMove(_Params.FovInfo, moveDelta);
|
|
if (res == E1.OK) {
|
|
return SequenceAction.Ok();
|
|
}
|
|
else {
|
|
return SequenceAction.Error(E2.ABORTED);
|
|
}
|
|
}
|
|
|
|
private SequenceActionResult Grab(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
Thread.Sleep(150); // 그랩 딜레이
|
|
|
|
int result = E1.GENERIC;
|
|
|
|
if (_GrabImage != null) {
|
|
result = _CameraAgent.GrabSync(_CameraDisplay, ref _GrabImage);
|
|
}
|
|
else {
|
|
LOG.FuncE("_GrabImage is null.");
|
|
}
|
|
|
|
if (result.Equals(E1.OK)) {
|
|
return SequenceAction.Ok(_GrabImage);
|
|
}
|
|
else {
|
|
return SequenceAction.Error();
|
|
}
|
|
}
|
|
|
|
private SequenceActionResult SaveImage(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) { // Param : GrabImage
|
|
|
|
object grabImage = param.Param;
|
|
|
|
if (VisionSettingManager.VisionSetting.SaveSetUpImage) {
|
|
_ImageManager.SaveImageInternal(_Params.FovInfo, eSaveDataType.AutoFocus, grabImage);
|
|
}
|
|
|
|
return SequenceAction.Ok(grabImage);
|
|
}
|
|
|
|
private string MeasureSharpness(
|
|
SequenceContext context,
|
|
string id,
|
|
ICollection<string> branches,
|
|
SequenceActionResult param) { // Param : GrabImage
|
|
|
|
if (param.Error != E1.OK || param.Param == null) {
|
|
return SEQ_ID_MEASURE_NG;
|
|
}
|
|
|
|
int currentStep = context.GetValue<int>("CurrentStep");
|
|
if (currentStep > _MoveStepMaxCnt) {
|
|
return SEQ_ID_MOVE_BEST_POS;
|
|
}
|
|
else {
|
|
object image = param.Param;
|
|
_CurrentSharpness = IpAdapter.Instance.IpEnvironment.GetSharpnessWithCropRoi(image, _Params.AutoFocusRecipe.SharpnessRoi);
|
|
var autoFocusLog = new AutoFocusLog(_CurrentStep, _FocusMode, _CurrentSharpness, _CurrentPositionZ);
|
|
var log = AutoFocusLogManager.GetAutoFocusLog(_Params.FovInfo.ToSubSysInfo(), autoFocusLog);
|
|
AutoFocusLogList.Add(log);
|
|
|
|
VisionEnv.AuroraEnv.EventPublisher.Publish(
|
|
new EventMessage(_Params.FovInfo.GetKey(), VisionEventId.UPDATE_AUTO_FOCUS_LOG, log));
|
|
|
|
_CsvManager.SaveCsv(_Params.FovInfo.ToSubSysInfo(), log, eSaveDataType.AutoFocus);
|
|
|
|
LOG.FuncI(string.Format(
|
|
$"[{_FocusMode}][{_CurrentStep}/{_MoveStepMaxCnt}] _CurrentSharpness : {_CurrentSharpness}, _CurrentPositionZ : {_CurrentPositionZ}"));
|
|
|
|
return SEQ_ID_MOVE_STEP;
|
|
}
|
|
}
|
|
|
|
private SequenceActionResult MoveBestPos(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
// 최적의 Z축 위치 산출
|
|
double maxSharpness = 0;
|
|
int maxIdx = 0;
|
|
for (int i = 0; i < AutoFocusLogList.Count; i++) {
|
|
if (maxSharpness < AutoFocusLogList[i].Sharpness) {
|
|
maxSharpness = AutoFocusLogList[i].Sharpness;
|
|
maxIdx = i;
|
|
}
|
|
}
|
|
|
|
var bestStep = AutoFocusLogList[maxIdx].Step;
|
|
var bestSharpness = AutoFocusLogList[maxIdx].Sharpness;
|
|
var bestPos = AutoFocusLogList[maxIdx].PosZ;
|
|
var autoFocusResult = new SetupSeqResultParams_AutoFocus(bestStep, bestSharpness, bestPos);
|
|
context.PutValue(string.Format(VisionContextKeys.CNTXT_KEY_AUTO_FOCUS_RESULT, _Params.FovInfo.GetKey()), autoFocusResult);
|
|
var autoFocusLog = new AutoFocusLog(bestStep, _FocusMode,bestSharpness, bestPos);
|
|
|
|
if (_FocusMode == eFocusMode.Roughly) {
|
|
//VisionEnv.AuroraEnv.EventPublisher.Publish(
|
|
// new EventMessage(_Params.FovInfo.GetKey(), VisionEventId.CLEAR_AUTO_FOCUS_LOG));
|
|
_FocusMode = eFocusMode.Delicately;
|
|
}
|
|
else if (_FocusMode == eFocusMode.Delicately) {
|
|
VisionEnv.AuroraEnv.EventPublisher.Publish(
|
|
new EventMessage(_Params.FovInfo.GetKey(), VisionEventId.UPDATE_AUTO_FOCUS_BEST_POS, autoFocusLog));
|
|
_FocusMode = eFocusMode.Finished;
|
|
}
|
|
|
|
LOG.FuncI(string.Format($"[{_FocusMode}] BestStep : {bestStep}, BestPosZ : {bestPos}, BestSharpness : {bestSharpness}"));
|
|
|
|
if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
// 최적의 Z축 위치로 돌아갈 이동량 계산
|
|
double curPos = GetCurrentPosition();
|
|
double backStepValue = curPos - bestPos;
|
|
|
|
// 이동량이 4mm 이상이면 리밋 NG 처리
|
|
if (Math.Abs(backStepValue) >= 4) {
|
|
MessageBoxUtil.ShowMessageBoxError(
|
|
string.Format(StrTokens.MSG_E_BackStepValueLimitOver, backStepValue));
|
|
return null;
|
|
}
|
|
|
|
var moveDelta = new Position();
|
|
switch (_Params.AutoFocusRecipe.Axis) {
|
|
case eAxis.X:
|
|
moveDelta.X = backStepValue;
|
|
break;
|
|
case eAxis.Y:
|
|
moveDelta.Y = backStepValue;
|
|
break;
|
|
case eAxis.Z:
|
|
moveDelta.Z = backStepValue;
|
|
break;
|
|
}
|
|
|
|
var res = EquipAdapter.Instance.TeachingMotionManager.RelMove(_Params.FovInfo, moveDelta);
|
|
if (res == E1.OK) {
|
|
return SequenceAction.Ok();
|
|
}
|
|
else {
|
|
return SequenceAction.Error(E2.ABORTED);
|
|
}
|
|
}
|
|
|
|
private double GetCurrentPosition() {
|
|
Position position = EquipAdapter.Instance.TeachingMotionManager.GetPosition(_Params.FovInfo);
|
|
|
|
switch (_Params.AutoFocusRecipe.Axis) {
|
|
case eAxis.X:
|
|
return position.X;
|
|
case eAxis.Y:
|
|
return position.Y;
|
|
case eAxis.Z:
|
|
return position.Z;
|
|
}
|
|
|
|
return position.Z;
|
|
}
|
|
|
|
private SequenceActionResult SaveBestPos(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
var autoFocusResult = context.GetValue<SetupSeqResultParams_AutoFocus>(string.Format(VisionContextKeys.CNTXT_KEY_AUTO_FOCUS_RESULT, _Params.FovInfo.GetKey()));
|
|
if (MessageBoxUtil.ShowMessageBoxQuestion(
|
|
string.Format(
|
|
StrTokens.MSG_Q_SyncPatternOriginFormat, autoFocusResult.BestPosZ)) == MessageBoxResult.Yes) {
|
|
EquipAdapter.Instance.VisionSetup.SaveBestPos(autoFocusResult.BestPosZ);
|
|
}
|
|
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
private SequenceActionResult MeasureNG(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
LOG.E("Fail to grab image.");
|
|
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
private SequenceActionResult MoveFinalPos(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) {
|
|
|
|
if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
// 시퀀스 종료시, 다시 안전위치로 이동
|
|
int res = EquipAdapter.Instance.TeachingMotionManager.MovePosition(_Params.FovInfo, ePositionType.Ready);
|
|
if (res == E1.OK) {
|
|
return SequenceAction.Ok();
|
|
}
|
|
else {
|
|
return SequenceAction.Error(E2.ABORTED);
|
|
}
|
|
}
|
|
|
|
private SequenceActionResult SeqComplete(
|
|
SequenceContext context,
|
|
string id,
|
|
SequenceActionResult param) { // Param : AutoFocusResult
|
|
|
|
var autoFocusResult = context.GetValue<SetupSeqResultParams_AutoFocus>(string.Format(VisionContextKeys.CNTXT_KEY_AUTO_FOCUS_RESULT, _Params.FovInfo.GetKey()));
|
|
|
|
return SequenceAction.Ok(autoFocusResult);
|
|
}
|
|
|
|
void OnStopped(
|
|
SequenceContext context) {
|
|
LOG.I($"{context.SequenceId} - STOP Handler");
|
|
HandleAbnormalFinish(context);
|
|
}
|
|
|
|
void OnForceError(
|
|
SequenceContext context, string id) {
|
|
LOG.E($"{context.SequenceId} - FORCE ERROR Handler");
|
|
HandleAbnormalFinish(context);
|
|
}
|
|
|
|
void HandleAbnormalFinish(
|
|
SequenceContext context) {
|
|
}
|
|
|
|
private void InitParams(SequenceContext context) {
|
|
_CurrentStep = 0;
|
|
_CurrentPositionZ = 0;
|
|
_CurrentSharpness = 0;
|
|
|
|
context.PutValue("CurrentStep", _CurrentStep);
|
|
}
|
|
|
|
private void UpdateParams(SequenceContext context) {
|
|
_CurrentStep++;
|
|
|
|
if (EquipAdapter.Instance.EquipMode.IsSimulMode()) {
|
|
_CurrentPositionZ = Math.Round(123.456789, 4);
|
|
}
|
|
else {
|
|
// 모터 현재 위치 가져오기
|
|
double curPos = GetCurrentPosition();
|
|
_CurrentPositionZ = Math.Round(curPos, 4);
|
|
|
|
}
|
|
context.PutValue("CurrentStep", _CurrentStep);
|
|
}
|
|
}
|
|
} |