FurnaceELK/Furnace/FurnaceRT/Equipments/CarrierRobots/CarrierRobotGoto.cs

using Aitex.Core.RT.Device;
using Aitex.Core.RT.Event;
using Aitex.Core.RT.Routine;
using Aitex.Core.RT.SCCore;
using Aitex.Core.Util;
using Aitex.Sorter.Common;
using MECF.Framework.Common.Device.Bases;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.Schedulers;
using MECF.Framework.Common.SubstrateTrackings;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robot;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots.RobotBase;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using FurnaceRT.Equipments.FIMSs;
using FurnaceRT.Equipments.Stockers;
using FurnaceRT.Equipments.Systems;
using static FurnaceRT.Equipments.FIMSs.FIMSModule;
using FurnaceRT.Equipments.LPs;

namespace FurnaceRT.Equipments.CarrierRobots
{
    public class CarrierRobotGoto : ModuleRoutine, IRoutine
    {
        enum RoutineStep
        {
            Goto,
        }

        private CarrierRobotModule _cassetteRobotModule;
        private ModuleName _destination;
        private int _destinationSlot;
        private Hand _blade;
        private int _timeout = 0;
        private RD_TRIG _holdTrig = new RD_TRIG();
        private R_TRIG _emergencyStopTrig = new R_TRIG();
        private R_TRIG _pauseTrig = new R_TRIG();
        private R_TRIG _resumeTrig = new R_TRIG();
        private RoutineStep _routineStep;
        private double _durationTime = 0;
        private bool _isHasAlarm = false;
        private bool _needStartCheck = true;
        private bool _isPickReady;

        public CarrierRobotGoto(CarrierRobotModule cassetteModule)
        {
            _cassetteRobotModule = cassetteModule;
            Module = cassetteModule.Module;
            Name = "Goto";
        }

        public void Init(ModuleName destination, int destinationSlot, Hand blade, bool isPickReady, bool isHasAlarm)
        {
            _destination = destination;
            _destinationSlot = destinationSlot;
            _blade = blade;
            _isPickReady = isPickReady;

            var para = new List<object> { _destination, _destinationSlot, _blade, _isPickReady, true };
            _cassetteRobotModule.GotoTimeoutAlarm.RetryMessage = (int)CarrierRobotModule.MSG.GotoRetry;
            _cassetteRobotModule.GotoTimeoutAlarm.RetryMessageParas = para.ToArray();

            _cassetteRobotModule.GotoFailAlarm.RetryMessage = (int)CarrierRobotModule.MSG.GotoRetry;
            _cassetteRobotModule.GotoFailAlarm.RetryMessageParas = para.ToArray();

            _isHasAlarm = isHasAlarm;
            if (!_isHasAlarm)
                _needStartCheck = true;
        }

        public Result Start(params object[] objs)
        {
            // 抛出过alarm就不reset
            if (!_isHasAlarm)
            {
                Reset();
            }
            else
            {
                _historySteps.Remove((int)_routineStep);
                _isHasAlarm = false;
                ResetState();
            }

            _holdTrig.RST = true;
            _emergencyStopTrig.RST = true;
            _pauseTrig.RST = true;
            _resumeTrig.RST = true;

            _timeout = SC.GetValue<int>($"{Module}.MotionTimeout");

            Notify($"Start");

            return Result.RUN;
        }

        public void Abort()
        {
            _cassetteRobotModule.Stop();
        }

        public override Result Monitor()
        {
            try
            {
                PauseRountine(_cassetteRobotModule.CarrierRobotDevice.IsPause);
                if (_cassetteRobotModule.CarrierRobotDevice.IsPause)
                    return Result.RUN;

                Goto((int)RoutineStep.Goto, _destination, _destinationSlot, _blade, _isPickReady, _timeout);
            }
            catch (RoutineBreakException)
            {
                return Result.RUN;
            }
            catch (RoutineFaildException ex)
            {
                return Result.FAIL;
            }

            Notify("Finished");
            return Result.DONE;
        }

        private void Goto(int id, ModuleName target, int slot, Hand hand, bool isPickReady, int timeout)
        {
            _routineStep = (RoutineStep)Enum.Parse(typeof(RoutineStep), id.ToString());
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Send goto to {target} command to robot device");
                string reason;
                _cassetteRobotModule.RobotGoto(target, slot, hand, isPickReady, out reason);
                return true;
            }, () =>
            {
                if (_cassetteRobotModule.CarrierRobotDevice.IsError)
                    return null;

                if (_cassetteRobotModule.CarrierRobotDevice.IsReady() && !_cassetteRobotModule.CarrierRobotDevice.IsError)
                {
                    return true;
                }

                return false;
            }, timeout * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    //_cassetteRobotModule.PlaceCassetteFailAlarm.Description = $"{_cassetteRobotModule.CassetteRobotDevice.ErrorCode}";
                    _cassetteRobotModule.GotoFailAlarm.Set($"goto to {target} failed for robot error, error code={_cassetteRobotModule.CarrierRobotDevice.ErrorCode}");
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    //_cassetteRobotModule.PlaceCassetteTimeoutAlarm.Description = $"timeout over {timeout} seconds";
                    _cassetteRobotModule.GotoTimeoutAlarm.Set($"timeout over {timeout} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }
    }
}