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

using Aitex.Core.RT.Routine;
using Aitex.Core.RT.SCCore;
using Aitex.Sorter.Common;
using MECF.Framework.Common.Equipment;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace FurnaceRT.Equipments.CarrierRobots
{
    public class CarrierRobotSwap : ModuleRoutine, IRoutine
    {
        enum RoutineStep
        {
            Loop,
            Pick1,
            Pick2,
            PickInit1,
            PickInit2,
            Place1,
            Place2,
            PlaceInit1,
            PlaceInit2,
            EndLoop,
            IncreaseLoopCount,
        }

        private CarrierRobotModule _cassetteRobotModule;
        private ModuleName _source;
        private ModuleName _dest;
        private Hand _hand;
        private CarrierRobotPick _pick;
        private CarrierRobotPlace _place;
        private int _slot;
        private int _cycleNumber;

        public CarrierRobotSwap(CarrierRobotModule cassetteModule)
        {
            _pick = new CarrierRobotPick(cassetteModule);
            _place = new CarrierRobotPlace(cassetteModule);
            _cassetteRobotModule = cassetteModule;
            Module = cassetteModule.Name;
            Name = "Transfer";
        }

        public Result Start(params object[] objs)
        {
            Reset();

            _cycleNumber = SC.GetValue<int>($"{Module}.TransferModeNumber");
            Notify("Start");

            return Result.RUN;
        }

        public void Init(ModuleName source, ModuleName dest, int slot, Hand hand)
        {
            _source = source;
            _dest = dest;
            _slot = slot;
            _hand = hand;
        }

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

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

                Loop((int)RoutineStep.Loop, _cycleNumber);

                PickInit((int)RoutineStep.PickInit1, _source, _slot, _hand);

                ExecuteRoutine((int)RoutineStep.Pick1, _pick);

                PlaceInit((int)RoutineStep.PlaceInit1, _dest, _slot, _hand);

                ExecuteRoutine((int)RoutineStep.Place1, _place);

                IncreaseLoopCount((int)RoutineStep.IncreaseLoopCount);

                if(LoopCounter < LoopTotalTime)
                {
                    PickInit((int)RoutineStep.PickInit2, _dest, _slot, _hand);

                    ExecuteRoutine((int)RoutineStep.Pick2, _pick);

                    PlaceInit((int)RoutineStep.PlaceInit2, _source, _slot, _hand);

                    ExecuteRoutine((int)RoutineStep.Place2, _place);
                }

                EndLoop((int)RoutineStep.EndLoop);
            }
            catch (RoutineBreakException)
            {
                return Result.RUN;
            }
            catch (RoutineFaildException)
            {
                return Result.FAIL;
            }

            Notify("Finished");

            return Result.DONE;
        }

        private void PickInit(int id, ModuleName source, int slot, Hand blade)
        {
            Tuple<bool, Result> ret = Execute(id, () =>
            {
                Notify($"Pick from {source} {slot} init");
                string reason = string.Empty;

                _pick.Init(source, slot, blade, false);

                return true;
            });

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
            }
        }

        private void PlaceInit(int id, ModuleName dest, int slot, Hand blade)
        {
            Tuple<bool, Result> ret = Execute(id, () =>
            {
                Notify($"Place to {dest} {slot} init");
                string reason = string.Empty;

                _place.Init(dest, slot, blade, false);

                return true;
            });

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
            }
        }
        private void IncreaseLoopCount(int id)
        {
            Tuple<bool, Result> ret = Execute(id, () =>
            {
                LoopCounter++;

                return true;
            });

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
            }
        }
    }
}