2026-01-26 18:56:53 +09:00

555 lines
23 KiB
C#

using Aurora.Api;
using AuroraCAS;
using AuroraLaserScanner;
using DB;
using Hermes;
using Matrox.MatroxImagingLibrary;
using MilCommon;
using System;
using System.CodeDom.Compiler;
using System.Collections.Generic;
using System.Configuration;
using System.Diagnostics;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Windows.Documents;
using VisionCommon;
using WeldingCommon.Alarm;
using WeldingCommon.Data;
using WeldingCommon.Devices;
using WeldingCommon.Settings;
using WeldingCommon.Utils;
namespace WeldingCommon.Sequences
{
class SeqScoutFlyingVision
{
static readonly Logger Log = new Logger("SeqScoutFlyingVision");
const string SEQ_ENTER = "[SeqScoutFlyingVision] SEQ_ENTER"; // inputParam 으로 받자.
const string SEQ_CHECK_INIT_CONDITION = "[SeqScoutFlyingVision] Check Init Condition"; // Check Init Condition
const string SEQ_FLYING_VISION_START = "[SeqScoutFlyingVision] Flying Vision Start";
const string SEQ_UPLOAD_SCANNER_LIST = "[SeqScoutFlyingVision] Upload Scanner List";
const string SEQ_EXECUTE_SCANNER_LIST = "[SeqScoutFlyingVision] Execute Scanner List";
const string SEQ_SAVE_IMAGE_FILE = "[SeqScoutFlyingVision] Save Image File";
const string SEQ_SAVE_RESULT_DATA = "[SeqScoutFlyingVision] Save Result Data";
const string SEQ_WRITE_COMPLETION_PLC_BIT = "[SeqScoutFlyingVision] Write Completion PLC Bit";
const string SEQ_COMPLETE = "[SeqScoutFlyingVision] SEQ_COMPLETE"; // 완료
static class ContextKey
{
public const string KEY_SEQUENCE_RESULT = "KEY_SEQUENCE_RESULT_SeqScoutFlyingVision";
public const string KEY_SEQUENCE_START_TIME = "KEY_SEQUENCE_START_TIME_SeqScoutFlyingVision";
}
private SeqScoutFlyingVision_InitParam _InitParam;
private Scanner _Scanner;
private PLCController _PLC;
private SystemParameter _SystemParameter;
private FovRecipe _FovRecipe;
private CameraAgentBase _CameraAgent;
private ICameraDisplay _CameraDisplay;
private CamConfig _CamConfig;
private FieldScanParams _FieldScanParams;
private int _ScoutNo = 0;
private static List<ChildBufRowCol> _ChildBufRowCols;
private static List<int> _ScoutNoList = new List<int>();
private MIL_ID _FlyingImage = MIL.M_NULL;
private Stopwatch _StopWatchTotal;
private Stopwatch _StopWatchExecute; // ian0513, 재사용을 위해서 사용하나 봄. local 로 빼는게 좋을 듯
private Stopwatch _StopWatchFlyScout;
private int _ScoutRowCount;
private int _ScoutColumnCount;
public SequenceBuilder Builder()
{
var sb = new SequenceBuilder(1000, true);
////var sb = new SequenceBuilder(1000, false); // With log
sb.AddAction(SEQ_ENTER, Sequence_Enter);
sb.AddDecision(SEQ_CHECK_INIT_CONDITION, CheckInitCondition); //분기
sb.AddAction(SEQ_FLYING_VISION_START, FlyingVisionStart);
sb.AddAction(SEQ_UPLOAD_SCANNER_LIST, UploadScannerList);
sb.AddAction(SEQ_EXECUTE_SCANNER_LIST, ExecuteScannerList);
sb.AddAction(SEQ_SAVE_IMAGE_FILE, SaveImageFile);
sb.AddAction(SEQ_SAVE_RESULT_DATA, SaveResultData); // 해당 공정의 정보를 기록 함.
sb.AddAction(SEQ_WRITE_COMPLETION_PLC_BIT, WriteCompletionPLCBit); // 정상적으로 종료 되면, complete bit write
sb.AddAction(SEQ_COMPLETE, Complete);
sb.Flows(sb.Init > sb[SEQ_ENTER] > sb[SEQ_CHECK_INIT_CONDITION]
> sb[SEQ_FLYING_VISION_START] > sb[SEQ_UPLOAD_SCANNER_LIST] > sb[SEQ_EXECUTE_SCANNER_LIST] > sb[SEQ_SAVE_IMAGE_FILE]
> sb[SEQ_SAVE_RESULT_DATA] > sb[SEQ_WRITE_COMPLETION_PLC_BIT] > sb[SEQ_COMPLETE]);
sb.Flows(sb[SEQ_CHECK_INIT_CONDITION] > sb[SEQ_COMPLETE] > sb.Final);
sb.OnStopped(OnStopped);
sb.OnForceError(OnForceError);
//sb.Draw("SeqScoutFlyingVision.pdf");
return sb;
}
private SequenceActionResult Sequence_Enter(SequenceContext context, string id, SequenceActionResult param)
{
Log.I("SeqScoutFlyingVision Start");
_InitParam = param.Param as SeqScoutFlyingVision_InitParam;
_SystemParameter = context.GetSetting<SystemParameter>(CommonSettingIds.SystemParameter);
InitDev(context);
if (!CheckDeviceReady(context)) { //Check Device
AlarmManager.Alarm(AlarmCode.ALARM_SEQUENCE_ScoutFlyingVision_DeviceNotReady);
Log.E("The devices are not active.");
return null;
}
// TODO : SMW용
if (_InitParam.UseMovingJig && (_PLC.IsActivated && !_PLC.IsSimul)) {
DateTime _InterfaceStartTime = DateTime.Now;
int plc_TIMEOUT = 5;
while (!context.Stopped) {
if (_PLC.ReadBit((int)_PLC.PLC_JIG_LOAD_COMPLETEAddr).data) {
Log.I(string.Format($"[PLCInterface]GET PLC_JIG_LOAD_COMPLETE True"));
_PLC.WriteBit((int)_PLC.PC_JIG_LOAD_COMPLETE_ACKAddr, true);
Log.I(string.Format($"[PLCInterface]SET PC_JIG_LOAD_COMPLETE_ACK True"));
break;
}
TimeSpan _InterfaceSpanTime = DateTime.Now.Subtract(_InterfaceStartTime);
if (_InterfaceSpanTime.TotalMilliseconds > (plc_TIMEOUT * 1000)) {
Log.E($"TIMEOUT_PLC_JIG_LOAD_COMPLETE : {plc_TIMEOUT} sec");
return null;
}
Thread.Sleep(10);
}
}
if (InitSequenceVariable(context) == false) {
AlarmManager.Alarm(AlarmCode.ALARM_SEQUENCE_ScoutFlyingVision_InitVariableError);
return null;
}
// ian0513, 이게 여기 있어야 할까??, 무엇을 측정 하기 위한건지 명확히 알고 실제 공정이면 다른 밑의 Node 로 변경
_StopWatchTotal = new Stopwatch();
_StopWatchTotal.Start();
return SequenceAction.Ok();
}
private bool InitSequenceVariable(SequenceContext context)
{
// VisionDevice 추가
// InitializeCameraDevice
_FovRecipe = VisionRecipeManager.GetFovRecipe(_InitParam.ScoutSeqParams.FovInfo);
_CameraAgent = CameraAgentManager.GetCameraAgent(_InitParam.ScoutSeqParams.FovInfo);
_CameraDisplay = CameraDisplayManager.GetDisplay(_InitParam.ScoutSeqParams.FovInfo, _InitParam.ScoutSeqParams.DisplayType);
_CamConfig = VisionEnv.AuroraEnv.DeviceManager.GetDeviceConfig(eCamId.CAM1.ToString()) as CamConfig;
// ScoutRow, ScoutColumnCount Cal
//FiledScanParams 클래스에서 변수 받아오도록 수정
//_FieldScanParams = FieldScanParams.GetInstance();
//imageWidthChild = _FieldScanParams.GetimageSizeWidthChild();
//imageHeightChild = _FieldScanParams.GetimageSizeHeightChild();
//_ScoutRowCount = _FieldScanParams.GetrowCount();
//_ScoutColumnCount = _FieldScanParams.GetcolCount();
// ScoutRow, ScoutColumnCount Cal end
//CamConfig의 PosIndex 순서로 ScoutNoList를 파싱해서 사용하도록 함
_ChildBufRowCols = _CamConfig.ChildImage.ChildBufRowCols;
_ScoutNoList = GenerateSortedScoutNoList(_ChildBufRowCols);
_ScoutNo = 0;
return true;
}
private void InitDev(SequenceContext context)
{
//Device Init
_Scanner = context.GetLaserScanner();
_PLC = context.GetPLC();
}
private bool CheckDeviceReady(SequenceContext context)
{
List<string> deviceListToCheck = new List<string>() { CommonDeviceIds.Scanner };
bool isAllDeviceActivated = true;
foreach (string item in deviceListToCheck) {
if (!context.Devices[item].IsActivated) {
isAllDeviceActivated = isAllDeviceActivated && false;
}
}
return isAllDeviceActivated;
}
private bool CheckCondition()
{
if (CheckCondition_Interlock()) {
if (InitializeDeviceStatus()) {
return true;
}
}
return false;
}
private bool CheckCondition_Interlock()
{
// PLC, Data, IO 등 Interlock checking
#region Recipe Field Pos Index Checking
// SMW 에서는 순서가 중요하지만, LHS에서는 모두 0으로 사용 중..
// 공통으로 사용 가능하게 개선 후, 적용 예정.
//List<int> posIndexList = new List<int>(_ChildBufRowCols.Select(g => g.PosIdx).ToList());
//int count = posIndexList.Count();
//if (count <= 0) {
// Log.E("Recipe Field pos index error.");
// return false;
//}
//
//var numberSet = new HashSet<int>(posIndexList);
//// 1. 중복 확인
//if (numberSet.Count != count)
//{
// Log.E("Recipe Field pos index error.");
// return false;
//}
//
//// 2. 누락 및 범위 확인
//if (numberSet.SetEquals(Enumerable.Range(0, count)) == false)
//{
// Log.E("Recipe Field pos index error.");
// return false;
//}
#endregion
return true;
}
private bool InitializeDeviceStatus()
{
//Vision Correction File 선택
_Scanner.SelectCorrectionTable(1);
return true;
}
private string CheckInitCondition(SequenceContext context, string id, ICollection<string> branches, SequenceActionResult param)
{
if (!CheckCondition()) {
AlarmManager.Alarm(AlarmCode.ALARM_SEQUENCE_ScoutFlyingVision_CheckInitCondition);
Log.E("Condition is not in state to start sequence.");
context.PutValue(ContextKey.KEY_SEQUENCE_RESULT, false);
return SEQ_COMPLETE;
}
var startTime = DateTime.Now;
context.PutValue(ContextKey.KEY_SEQUENCE_START_TIME, startTime);
Log.D("ScoutFlyingVision Sequence CheckInitCondition");
return SEQ_FLYING_VISION_START;
}
private SequenceActionResult FlyingVisionStart(SequenceContext context, string id, SequenceActionResult param)
{
if (_InitParam.IsRealTimeScout == false) {
// 초기 위치 이동, AutoRun에서는 본 시퀀스에서 초기위치 이동시켜 놓는게 Tacttime에 유리할듯
_ScoutNo = _ScoutNoList[0];
if (_CamConfig.ChildImage.ChildBufRowCols[_ScoutNo].Enabled) {
double XPos = _CamConfig.ChildImage.ChildBufRowCols[_ScoutNo].MotorPos.X + _CamConfig.ChildImage.ChildBufRowCols[_ScoutNo].MotorPosOffset.X; //카메라 위치에 대한 보정값 입력
double YPos = _CamConfig.ChildImage.ChildBufRowCols[_ScoutNo].MotorPos.Y + _CamConfig.ChildImage.ChildBufRowCols[_ScoutNo].MotorPosOffset.Y; //카메라 위치에 대한 보정값 입력
Log.I(string.Format($"FlyingVisionScout First Pos Move. Field Scan Pos(X,Y:{XPos},{YPos})"));
//1.스캐너 필드 위치 이동(Jump)
_Scanner.JumpAbs(XPos, YPos, eFunctionType.COMMAND);
}
}
//RCC에서 Record는 불필요, 나중에 필요 시 주석 해제하자
//_Scanner.SetTrigger(_scanner.Config.RecordPeriod, _scanner.Config.RecordSignalType1, _scanner.Config.RecordSignalType2, _scanner.Config.RecordSignalType3, _scanner.Config.RecordSignalType4);
return SequenceAction.Ok();
}
private SequenceActionResult UploadScannerList(SequenceContext context, string id, SequenceActionResult param)
{
//Scanner Pos 이동 및 레이저 트리거 발생
//리스트 최초 1회만 넣어줘도 되면 1회 후 return 하도록 하자
if (_InitParam.SequenceAutoRunCount == 0) {
//초기 위치이 외 나머지 위치는 List에 넣고 Jump -> Trigger를 반복하도록 한다.
_Scanner.SetStartList(_InitParam.WeldingScannerListNo);
//Jumpdelay 셋팅(usec)
_Scanner.SetScannerDelay(_SystemParameter.ScoutJumpDelay, 0, 0);
//JumpSpeed 셋팅(mm/sec)
_Scanner.SetJumpSpeed(_SystemParameter.FieldScanJumpSpeed);
Thread.Sleep(10); //plc에서 주던거 가져옴
foreach (var scoutNo in _ScoutNoList) {
if (_CamConfig.ChildImage.ChildBufRowCols[scoutNo].Enabled) {
double xPos = _CamConfig.ChildImage.ChildBufRowCols[scoutNo].MotorPos.X + _CamConfig.ChildImage.ChildBufRowCols[scoutNo].MotorPosOffset.X; //카메라 위치에 대한 보정값 입력
double yPos = _CamConfig.ChildImage.ChildBufRowCols[scoutNo].MotorPos.Y + _CamConfig.ChildImage.ChildBufRowCols[scoutNo].MotorPosOffset.Y; //카메라 위치에 대한 보정값 입력
Log.I(string.Format($"Move No.{scoutNo + 1} Field Scan Pos(X,Y:{xPos},{yPos})"));
//1.스캐너 필드 위치 이동(Jump)
_Scanner.JumpAbs(xPos, yPos, eFunctionType.LIST);
// TODO : SMW용, 마지막 scoutNo일 경우 Jig가 완전 회피할 시간을 기다려줘야하기 때문에 Dealy 추가. 150msec → 50msec 단축 필요 0811
if (_InitParam.UseMovingJig && (_PLC.IsActivated && !_PLC.IsSimul)) {
if (scoutNo == _ScoutNoList.Last()) {
_Scanner.LongDelay(150);
}
}
//2. Jump 후 대기 시간은 SetScannerDelay로 셋팅 되어 있지만, 이미지 블러 현상 발생으로 Jump 후 1msec longdelay추가해주자
_Scanner.LongDelay(1); //msec
//3. Laser Digital Out1 On
_Scanner.SetLaserPinOutList(0x01);
//4. Trigger On 해주고 delay시간을 주자
_Scanner.LongDelay(1); //msec
//5. Laser Digital Out1 off
_Scanner.SetLaserPinOutList(0x00);
//6. 카메라 노출시간 만큼 대기 필요. TriggerOn 이후 이미 1msec 넣어줬으니 1msec 빼주자
_Scanner.LongDelay(_SystemParameter.CameraExposureDelay - 1);
}
}
//_Scanner.SetTrigger(0, _Scanner.Config.RecordSignalType1, _Scanner.Config.RecordSignalType2, _Scanner.Config.RecordSignalType3, _Scanner.Config.RecordSignalType4);
_Scanner.SetEndList();
}
return SequenceAction.Ok();
}
private List<int> GenerateSortedScoutNoList(List<ChildBufRowCol> childBufRowCols)
{
var scoutNoPosIdxList = childBufRowCols.Select((childBufRowCol, index) => new { ScoutNo = index, PosIdx = childBufRowCol.PosIdx }).ToList();
var sortedScoutNoPosIdxList = scoutNoPosIdxList.OrderBy(item => item.PosIdx).ToList();
var sortedScoutNoList = sortedScoutNoPosIdxList.Select(item => item.ScoutNo).ToList();
return sortedScoutNoList;
}
private List<int> GetOuterClockwiseIndices(int rows, int cols)
{
List<int> indices = new List<int>();
//배열의 인덱스를 뒤집어 저장할 배열 생성
int[,] arr = new int[rows, cols];
int count = 0;
//2D 배열을 주어진 방식대로 채운다
for (int i = 0; i < rows; i++) {
if (i % 2 == 0) {
for (int j = 0; j < cols; j++) {
arr[i, j] = count++;
}
} else {
for (int j = cols - 1; j >= 0; j--) {
arr[i, j] = count++;
}
}
}
// Top row (left to right)
for (int col = 0; col < cols; col++) {
indices.Add(arr[0, col]);
}
// Right column (top to bottom)
for (int row = 1; row < rows; row++) {
indices.Add(arr[row, cols - 1]);
}
// Bottom row (right to left), 뒤집어서 추가
if (rows > 1) {
for (int col = cols - 2; col >= 0; col--) {
indices.Add(arr[rows - 1, col]);
}
}
// Left column (bottom to top), 뒤집어서 추가
if (cols > 1) {
for (int row = (rows - 2); row > 0; row--) {
indices.Add(arr[row, 0]);
}
}
return indices;
}
private SequenceActionResult ExecuteScannerList(SequenceContext context, string id, SequenceActionResult param)
{
//CamConfig에서도 HW 트리거로 변경하고 Flying 사용 체크를 하자
_StopWatchFlyScout = new Stopwatch();
_StopWatchFlyScout.Start();
int res = _CameraAgent.StartFlying(_InitParam.ScoutSeqParams.FovInfo);
if (res != E1.OK) {
Log.E(string.Format("CaemraAgent StartFlying Error"));
return null;
}
Log.I(string.Format("Start FlyingVision"));
//Dialog는 필요시 사용하자
//ProjectUtil.ShowSeqDialog(_systemParameter);
_StopWatchExecute = new Stopwatch();
_StopWatchExecute.Start();
// TODO
//Thread.Sleep(50);
_Scanner.ExecuteList(_InitParam.WeldingScannerListNo);
_Scanner.WaitForListTaskCompleted(_SystemParameter.WeldLinesTaskTimeOut);
_StopWatchExecute.Stop();
Log.D(string.Format($"[SeqFlyingVisionScout] Execute List Complete Time : {_StopWatchExecute.ElapsedMilliseconds}"));
res = _CameraAgent.StopFlying();
if (res != E1.OK) {
Log.E(string.Format("CaemraAgent StopFlying Error"));
return null;
}
Log.I(string.Format("Stop FlyingVision"));
// TODO : SMW 전용 수동 테스트(Merge X), B0812,PC_MANUAL_SCOUT_JIG_MOVE_REQUEST, PC 수동 Scout Jig 회피 요청
if (_InitParam.UseMovingJig && (_PLC.IsActivated && !_PLC.IsSimul)) {
_PLC.WriteBit((int)_PLC.PC_JIG_LOAD_COMPLETE_ACKAddr, false);
Log.I(string.Format($"[PLCInterface]SET PC_MANUAL_SCOUT_JIG_MOVE_REQUEST false"));
}
return SequenceAction.Ok();
}
private SequenceActionResult SaveImageFile(SequenceContext context, string id, SequenceActionResult param)
{
try {
_FlyingImage = (MIL_ID)_CameraAgent.GetParentImage();
if (_FlyingImage != null) {
Status.GetProcessingWeldingData().ScannerFiledImage = _FlyingImage;
}
_StopWatchFlyScout.Stop();
Log.D(string.Format($"[SeqFlyingVisionScout] Scout Complete Time : {_StopWatchFlyScout.ElapsedMilliseconds}"));
//6. 스캔필드 완료 후 초기 위치 0,0로 이동
_Scanner.JumpAbs(0, 0, eFunctionType.COMMAND);
Log.I(string.Format($"Complete Scanner mover Origin"));
if (_InitParam.IsRealTimeScout) {
// real time 이면, 그냥 로그만 한줄 찍자.
Log.I(string.Format("When Real Time Scout Seq is running, Pass Save Image File"));
} else {
Log.I(string.Format($"Start to store completed field scan image"));
///7. 이미지 확인을 위해 PNG 파일로 저장
//4.스캐너 필드 이미지 파일 저장
string imageFilePath = ProjectUtil.GetDataLogSetting().GetFilePath_Scout_Flying_Image("FlyingVisionScout");
_SystemParameter.ScannerFieldImageFile = imageFilePath;
//5.압축하여 저장
ImageUtil.SaveImage(_FlyingImage, _SystemParameter.ScannerFieldImageFile, 10, false);
//8. 캔버스 필드 이미지 업데이트(AutoRun, Teaching 화면 업데이트), SystemparameterILSOS의 ScannerFieldImageFile의 경로를 변경해주면 Teaching에서 자동 변경 됨
context.Settings.Save(CommonSettingIds.SystemParameter, _SystemParameter, "SeqFlyingVisionScout");
Log.I(string.Format($"[SeqFlyingVisionScout] Complete Save ImageFile"));
}
} catch (Exception e) {
Log.W($"{e.Message}");
}
return SequenceAction.Ok();
}
private SequenceActionResult SaveResultData(SequenceContext context, string id, SequenceActionResult param)
{
// 결과 Data 를 업데이트 함.
context.PutValue(ContextKey.KEY_SEQUENCE_RESULT, true);
return SequenceAction.Ok();
}
private SequenceActionResult WriteCompletionPLCBit(SequenceContext context, string id, SequenceActionResult param)
{
// 완료 비트 write 하는 Node 추가 필요. format 유지, SeqScoutNormal 없음
Log.D("ScoutFlyingVision Sequence WriteCompletionPLCBit");
return SequenceAction.Ok();
}
private SequenceActionResult Complete(SequenceContext context, string id, SequenceActionResult param)
{
RestoreDeviceStatus();
Log.I("ScoutFlyingVision Complete");
context.TryGetValue(ContextKey.KEY_SEQUENCE_RESULT, out bool seqResult);
SeqScoutFlyingVision_OutParam result = new SeqScoutFlyingVision_OutParam() { Success = seqResult };
if (seqResult == false) {
AlarmManager.Alarm(AlarmCode.ALARM_SEQUENCE_ScoutFlyingVision_ResultFailed);
}
Log.D("ScoutFlyingVision Sequence Complete");
return SequenceAction.Ok(result);
}
private bool RestoreDeviceStatus()
{
return true;
}
private void OnForceError(SequenceContext arg1, string arg2)
{
Log.I("SequenceAction Ok : OnForceError");
}
private void OnStopped(SequenceContext obj)
{
Log.I("SequenceAction Ok : OnStopped");
}
}
public class SeqScoutFlyingVision_InitParam : InitParam
{
public bool IsRealTimeScout { get; set; }
public ScoutSeqParams ScoutSeqParams { get; set; }
public eListNo WeldingScannerListNo { get; set; }
public int SequenceAutoRunCount { get; set; }
public bool UseMovingJig { get; set; }
}
public class SeqScoutFlyingVision_OutParam : OutParam
{
}
}