LGES-OC5-PtoP-Welder/Machines/ILSOS_PrismaticRD_PTOP/Sequences/SeqWaitWeldOrChangePage.cs

using Aurora;
using Aurora.Api;
using AuroraLaser;
using AuroraLaser.Trumpf;
using CoreUnit.Control.UserControl.Canvas;
using CoreUnit.Scanner.UserControl.ScannerCanvas;
using Hermes;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Numerics;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Windows.Controls.Primitives;
using WeldingCommon;
using WeldingCommon.Devices;
using WeldingCommon.Sequences;
using WeldingCommon.Settings;
using static WeldingCommon.Devices.PLCController;

namespace ILSOS_PrismaticRD_PTOP.Sequences
{
    public class SeqWaitWeldOrChangePage
    {
        static readonly Logger Log = new Logger("SeqWaitWeldOrChangePage");

        const string SEQ_ENTER = "[SeqWaitWeldOrChangePage] SEQ_ENTER";
        const string SEQ_CHECK_WELD_OR_PAGE_CHANGE = "[SeqWaitWeldOrChangePage] Check Init Condition";  // Check Weld Request or Page Change Request
        const string SEQ_GET_PLC_PAGE_CHANGE_REQ_ON = "[SeqWaitWeldOrChangePage] Get PLC Page Change Request On"; // PLC Page Change Request ON
        const string SEQ_SET_PC_PAGE_CHANGE_ACK_ON = "[SeqWaitWeldOrChangePage] Set PC Page Change Ack On"; // PC Page Change Reqeust ACK ON
        const string SEQ_GET_PLC_PAGE_CHANGE_REQ_OFF = "[SeqWaitWeldOrChangePage] Get PLC Page Change Request Off"; // PLC Page Change Request OFF
        const string SEQ_SET_PC_PAGE_CHANGE_ACK_OFF = "[SeqWaitWeldOrChangePage] Set PC Page Change Ack Off"; // PC Page Change Reqeust OFF
        const string SEQ_GET_PLC_WELD_REQ_ON = "[SeqWaitWeldOrChangePage] Get PLC Weld Request On"; // PLC Weld Request ON
        const string SEQ_COMPLETE = "[SeqWaitWeldOrChangePage] SEQ_COMPLETE";         // 완료

        private PLCController _PLC;

        // 레이저 상태 변화
        private Laser _Laser;
        private ILaser _LaserDevice;
        private LaserTruFiber _LaserTruFiber;
        private LaserPowerCalParam _LaserPowerCalParam;
        private static DataModel Models { get; set; }
        private static List<IDrawObject> _UnpackDrawObject;

        private SeqWaitWeldOrChangePage_InitParam _InitParam;
        private SeqWaitWeldOrChangePage_OutParam _OutParam;

        private static short _PLCPageNo = 1;

        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_WELD_OR_PAGE_CHANGE, CheckInitCondition); //분기

            // Page Change Request
            sb.AddAction(SEQ_GET_PLC_PAGE_CHANGE_REQ_ON, GetPLCPageChangeRequestOn);
            sb.AddAction(SEQ_SET_PC_PAGE_CHANGE_ACK_ON, SetPCPageChangeRequestACKOn);
            sb.AddAction(SEQ_GET_PLC_PAGE_CHANGE_REQ_OFF, GetPLCPageChangeRequestOff);
            sb.AddAction(SEQ_SET_PC_PAGE_CHANGE_ACK_OFF, SetPCPageChangeRequestACKOff);

            // Weld Reqeust
            // Weld Request I/F는 없어도 됨. 왜냐하면, PLC에서 Busy 신호만 보기 때문임.
            sb.AddAction(SEQ_GET_PLC_WELD_REQ_ON, GetPLCWeldRequestOn);

            sb.AddAction(SEQ_COMPLETE, Complete);


            sb.Flows(sb.Init > sb[SEQ_ENTER] > sb[SEQ_CHECK_WELD_OR_PAGE_CHANGE]
                    > sb[SEQ_GET_PLC_PAGE_CHANGE_REQ_ON] > sb[SEQ_SET_PC_PAGE_CHANGE_ACK_ON]
                    > sb[SEQ_GET_PLC_PAGE_CHANGE_REQ_OFF] > sb[SEQ_SET_PC_PAGE_CHANGE_ACK_OFF]
                    > sb[SEQ_CHECK_WELD_OR_PAGE_CHANGE]);

            sb.Flows(sb[SEQ_CHECK_WELD_OR_PAGE_CHANGE]
                    > sb[SEQ_GET_PLC_WELD_REQ_ON]
                    > sb[SEQ_COMPLETE]
                    > sb.Final);

            sb.OnForceError(OnForceError);

            sb.OnStopped(OnStopped);

            //sb.Draw("SeqWaitWeldOrPageChange.pdf");

            return sb;
        }

        private SequenceActionResult Sequence_Enter(SequenceContext context, string id, SequenceActionResult param)
        {
            Log.I("SeqWaitWledORPageChange Start");

            InitDev(context);
            if (!CheckDeviceReady(context)) {
                Log.E("The devices are not active.");
                return null;
            }

            if (InitSequenceVariable(context) == false) {
                return null;
            }
            _InitParam = param.Param as SeqWaitWeldOrChangePage_InitParam;

            if (_InitParam.NeedPageNo)
                _PLCPageNo = _InitParam.CurrentPageNo;
            else
                _PLCPageNo = 1;

            // 레이저 Word 값을 위함
            Models = _InitParam.Models;

            _Laser = context.GetLaser();
            _Laser.GetLaserDevice(ref _LaserDevice);
            _LaserTruFiber = _LaserDevice as LaserTruFiber;

            _LaserPowerCalParam = context.GetSetting<LaserPowerCalParam>(CommonSettingIds.LaserPowerCalParam);//우선 AuroraRecipe로 상속받아서 함수 사용하도록 함
            //laserPowerCalParam 초기화(CSV 파일 불러오기)
            _LaserPowerCalParam.Initialize(_LaserPowerCalParam.PowerCalibrationCenterBeamCSVFile, _LaserPowerCalParam.PowerCalibrationRingBeamCSVFile);

            return SequenceAction.Ok();
        }
        private string CheckInitCondition(SequenceContext context, string id, ICollection<string> branches, SequenceActionResult param)
        {
            var ret = context.RetryUntilSuccess(CheckRequest, int.MaxValue, interval: 10); // 최대 596시간동안 대기
            if (ret == E2.TIMEOUT)
            {
                Log.E("대기 시간 초과로 시퀀스 정지");
                return null;
            }
            else if (ret == E2.CANCELLED)
            {
                Log.E("시퀀스 정지 요청에 의한 CheckRequest 취소");
                return null;
            }

            if (pageChangeRequestBit == true) // Page Change Request
                return SEQ_GET_PLC_PAGE_CHANGE_REQ_ON;
            else if (weldRequestBit == true) // Weld Request
                return SEQ_GET_PLC_WELD_REQ_ON;
            else
            {
                Log.E($"NULL ERROR IN Interface >> pageChangeRequestBit({pageChangeRequestBit} >> weldRequestBit{weldRequestBit}");
                return null;
            }
        }

        private SequenceActionResult GetPLCPageChangeRequestOn(SequenceContext context, string id, SequenceActionResult param)
        {
            var res = context.RetryUntilSuccess(() => _PLC.ReadBit(eCommonBitInput.PLC_PAGE_CHANGE_REQ).data, _InitParam.TimeoutSec * 1000, interval: 10);
            if(res == E2.TIMEOUT) {
                Log.FuncE("Get PLC Page Change Request On Timeout !");
                return null;
            }
            Log.W(string.Format("PLC Page Change Request On"));

            return SequenceAction.Ok();
        }
        private SequenceActionResult SetPCPageChangeRequestACKOn(SequenceContext context, string id, SequenceActionResult param)
        {
            if (_InitParam.NeedPageNo) {
                Thread.Sleep(100);
                _PLCPageNo = _PLC.ReadWord(eCommonWordInput.PLC_PAGE_NO).data;
                _PLC.WriteWord(eCommonWordOutput.PC_PAGE_NO, (short)_PLCPageNo);
                Log.D($"When PageChangeReq is ON;{_PLCPageNo}");


                #region Tact Time 절감을 위한 페이지 체인지 인터페이스 중 Program No 변경
                // Unpack 관련 함수
                _UnpackDrawObject?.Clear();
                _UnpackDrawObject = new List<IDrawObject>();

                if (Models.Layers.Count() > _PLCPageNo - 1) UnpackRecipes(Models.Layers[_PLCPageNo - 1].Objects);
                else
                {
                    Log.E($"[Page Change Req] Page No is not proper;{Models.Layers.Count()};{_PLCPageNo}");
                    return null;
                }

                int paramIndex = 0;

                if (_UnpackDrawObject.Count > 0)
                {
                    paramIndex = _UnpackDrawObject[0].ScannerParamID - 1;
                }
                else
                {
                    paramIndex = 0;
                    Log.E("[Page Change Req] Draw Objects is zero");
                }

                _CenterBeamPower = (int)ScannerCanvasUserCtrl.ScannerParams[paramIndex].LaserPower1;
                _RingBeamPower = (int)ScannerCanvasUserCtrl.ScannerParams[paramIndex].LaserPower2;
                _CenterRingRatio = 0;

                if (_CenterBeamPower == 0 && _RingBeamPower == 0)
                    _CenterRingRatio = 100;
                else
                    _CenterRingRatio = (int)Math.Round((double)(100 * _CenterBeamPower) / (double)(_CenterBeamPower + _RingBeamPower), 0); // Discrete 한 값이라서 Ring은 오차 발생

                if (_Laser.Config.TruFiberType == eTruFiberType.TruFiberP)
                {
                    _CenterBeamPower = _LaserPowerCalParam.CalibrateWattToWatt(_CenterBeamPower, (uint)LaserPowerCalParam.beamMode.Center);
                    _RingBeamPower = _LaserPowerCalParam.CalibrateWattToWatt(_RingBeamPower, (uint)LaserPowerCalParam.beamMode.Ring);
                }
                // Ready 상태 변경 확인(Complete 상태에서 Laser Ready로 변환되기까지 시간이 걸림)
                // 여기서는 왜 IW03과 Center를 비교하는 것인지.... 아직 값을 써주지 않았으므로 레이저 출사 가능 여부만 확인하면 되지 않을까.
                var res = context.RetryUntilSuccess(_Laser.GetLaserReady, _InitParam.TimeoutSec * 1000, 1);

                if (res == E2.TIMEOUT)
                {
                    Log.E(string.Format($"[Page Change Req] Laser Program Ready Timeout When LaserReady"));
                    return null;
                }
                Log.D(string.Format($"[Page Change Req] Laser Program Ready OK"));

                // Laser Program Parameters 설정(Laser Ready 상태는 용접 직전에 확인)
                List<(int, int, bool)> datas = new List<(int, int, bool)>();
                datas.Add((csWriteNodeNo.IW03, _CenterBeamPower, true));
                datas.Add((csWriteNodeNo.IW04, _RingBeamPower, true));
                datas.Add((csWriteNodeNo.IW05, _CenterRingRatio, true));

                // 업데이트 될 경우, 프로그램 전환을 위한 프로그램 종료 시퀸스 진행됨.
                if (_LaserTruFiber.UpdatesDatasIfChanged(datas))
                {

                    // 프로그램이 종료 될 때까지 대기.
                    res = context.RetryUntilSuccess(_LaserTruFiber.GetProgramCompleted, _InitParam.TimeoutSec * 1000, 10);

                    if (res == E2.TIMEOUT)
                    {
                        Log.E(string.Format($"[Page Change Req] Laser Program Completed Timeout When Parameter Changes"));
                        return null;
                    }
                    Log.D(string.Format($"[Page Change Req] Laser Program Completed OK When Data Updated"));
                }

                // 만일 프로그램이 정상적으로 종료 및 변경 원할 경우, 레이저 스레드에서 Ready 상태로 만들어주고 있을 것임.
                // Ready 상태 변경 확인
                res = context.RetryUntilSuccess(() => LaserReadyResponse(), _InitParam.TimeoutSec * 1000, 1);
                if (res == E2.TIMEOUT)
                {
                    Log.E(string.Format($"[Page Change Req] Laser Program Ready Timeout When LaserReadyResponse"));
                    return null;
                }
                Log.D(string.Format($"[Page Change Req] Laser Program Ready OK When ReadyResponse"));
                #endregion
            }

            _PLC.WriteBit(eCommonBitOutput.PC_PAGE_CHANGE_COMPLETE, true);

            Log.W(string.Format("Set PC Page Change Request ACK On"));

            return SequenceAction.Ok();
        }

        #region 레이저 값 변경을 위함
        private int _CenterBeamPower;
        private int _RingBeamPower;
        private int _CenterRingRatio;
        private static void UnpackRecipes(List<IDrawObject> recipes)
        {
            // Recipe를 재귀적으로 탐색해서 List로 재구성하는 함수
            if (recipes == null)
                return;

            for (int i = 0; i < recipes.Count; i++)
            {
                if (recipes[i] is Group)
                {
                    Group subGroup = recipes[i] as Group;
                    List<IDrawObject> subrecipes = subGroup.SubObjects as List<IDrawObject>;
                    UnpackRecipes(subrecipes);
                }
                else
                {
                    _UnpackDrawObject.Add(recipes[i]);
                }
            }
        }
        private bool LaserReadyResponse()
        {
            if (ApplicationExt.IsSimulation)
                return true;

            // Ready 상태를 확인하기 위해서는 프로그램 파라메터가 최신 상태인지, Ready 상태인지 둘 다 확인해야 한다.
            return Convert.ToInt32(_LaserTruFiber.GetWriteInputWord(csWriteNodeNo.IW03)) == _CenterBeamPower
                && Convert.ToInt32(_LaserTruFiber.GetWriteInputWord(csWriteNodeNo.IW04)) == _RingBeamPower
                && Convert.ToInt32(_LaserTruFiber.GetWriteInputWord(csWriteNodeNo.IW05)) == _CenterRingRatio
                //&& _LaserTruFiber.GetProgramNo() == _BeforeProgramNo
                && _LaserTruFiber.GetLaserReady();
        }
        #endregion

        private SequenceActionResult GetPLCPageChangeRequestOff(SequenceContext context, string id, SequenceActionResult param)
        {
            //// 계속 페이지를 저장해줄 필요는 없음.
            //var res = context.RetryUntilSuccess(UpdatePLCPageWord, _InitParam.TimeoutSec * 1000, interval: 10);
            var res = context.RetryUntilSuccess(() => !_PLC.ReadBit(eCommonBitInput.PLC_PAGE_CHANGE_REQ).data, _InitParam.TimeoutSec * 1000, interval: 10);

            if (res == E2.TIMEOUT) {
                Log.FuncE(string.Format("Get PLC Page Change Request OFF Timeout ! : {0}", eCommonBitInput.PLC_PAGE_CHANGE_REQ));
                return null;
            } else Log.W(string.Format("Get PLC Page Change Request OFF Complete : {0}", eCommonBitInput.PLC_PAGE_CHANGE_REQ));

            return SequenceAction.Ok();
        }
        private SequenceActionResult SetPCPageChangeRequestACKOff(SequenceContext context, string id, SequenceActionResult param)
        {
            if (_InitParam.NeedPageNo) {
                _PLCPageNo = _PLC.ReadWord(eCommonWordInput.PLC_PAGE_NO).data;
                // PLC와 Request ON 시점에서 써주기로 하였으므로, 여기서는 굳이 써주지 않음.(확인용)
                //_PLC.WriteWord(_InitParam.SetPageNoAddress, (short)_PLCPageNo, AuroraPLC.PLCBase.ePLCDevice.D);
                Log.D($"When PageChangeReq is OFF;{_PLCPageNo}");
            }

            _PLC.WriteBit(eCommonBitOutput.PC_PAGE_CHANGE_COMPLETE, false);
            Log.W(string.Format("Set PC Page Change Request ACK Off"));

            return SequenceAction.Ok();
        }
        static DateTime WeldReqTime;
        private SequenceActionResult GetPLCWeldRequestOn(SequenceContext context, string id, SequenceActionResult param)
        {
            WeldReqTime = DateTime.Now;

            context.RetryUntilSuccess(() => _PLC.ReadBit(eCommonBitInput.PLC_WELD_TRIGGER_REQ).data, _InitParam.TimeoutSec * 1000, interval: 10);

            Log.W(string.Format("Get PLC Weld Request On"));

            return SequenceAction.Ok();
        }
        private SequenceActionResult Complete(SequenceContext context, string id, SequenceActionResult param)
        {

            SeqWaitWeldOrChangePage_OutParam result = new SeqWaitWeldOrChangePage_OutParam() {PLCPageNo = (short)_PLCPageNo, Success = true, _WeldReqTime = WeldReqTime };

            Log.D("CheckWeldingComplete Sequence Complete");

            return SequenceAction.Ok(result);
        }

        private void OnStopped(SequenceContext context)
        {
            Log.W("SequenceAction Ok : OnStopped");
        }

        private void OnForceError(SequenceContext context, string arg2)
        {
            Log.W("SequenceAction Ok : OnForceError");
        }
        private bool UpdatePLCPageWord()
        {
            if (_InitParam.NeedPageNo) {
                _PLCPageNo = _PLC.ReadWord(eCommonWordInput.PLC_PAGE_NO).data;
                _PLC.WriteWord(eCommonWordOutput.PC_PAGE_NO, (short)_PLCPageNo);
            }
            return !_PLC.ReadBit(eCommonBitInput.PLC_PAGE_CHANGE_REQ).data;
        }
        static private bool pageChangeRequestBit = false;
        static private bool weldRequestBit = false;
        private bool CheckRequest()
        {
            pageChangeRequestBit = _PLC.ReadBit(eCommonBitInput.PLC_PAGE_CHANGE_REQ).data;
            weldRequestBit = _PLC.ReadBit(eCommonBitInput.PLC_WELD_TRIGGER_REQ).data;
            return (pageChangeRequestBit || weldRequestBit);
        }

        #region Device 및 상태 확인 함수 (시퀸스 공용)
        private bool InitSequenceVariable(SequenceContext context)
        {
            // TODO: [장비구현필요] Initialize Variable 추가
            return true;
        }

        private void InitDev(SequenceContext context)
        {
            //Device Init
            _PLC = context.GetPLC();
        }

        private bool CheckDeviceReady(SequenceContext context)
        {
            List<string> deviceListToCheck = new List<string>() { CommonDeviceIds.PLCDevice };
            bool isAllDeviceActivated = true;
            foreach (string item in deviceListToCheck) {
                if (!context.Devices[item].IsActivated) {
                    isAllDeviceActivated = isAllDeviceActivated && false;
                }
            }

            return isAllDeviceActivated;
        }

        private bool CheckCondition(SequenceContext context)
        {
            if (CheckCondition_Interlock()) {
                if (InitializeDeviceStatus(context)) {
                    return true;
                }
            }
            return false;
        }

        private bool InitializeDeviceStatus(SequenceContext context)
        {
            // format 유지, ReceiveCellInfo 없음
            return true;
        }

        private bool CheckCondition_Interlock()
        {
            // PLC, Data, IO 등 Interlock checking

            return true;
        }
        #endregion
    }

    public class SeqWaitWeldOrChangePage_InitParam : InitParam
    {
        public short CurrentPageNo { get; set; }
        public bool NeedPageNo { get; set; }
        public DataModel Models { get; set; }
        public int TimeoutSec { get; set; }
    }
    public class SeqWaitWeldOrChangePage_OutParam : OutParam
    {
        public bool IsComplete { get; set; }
        public short PLCPageNo { get; set; }
        public DateTime _WeldReqTime { get; set; }
    }
}