Mars200/Mars/JetEfem/Efems/EfemGotoRoutine.cs

using System;
using Aitex.Core.RT.Routine;
using Aitex.Core.RT.SCCore;
using Aitex.Sorter.Common;
using MECF.Framework.Common.Equipment;

namespace JetEfemLib.Efems
{
    public class EfemGotoRoutine : ModuleRoutineBase, IStepRoutine
    {
        enum RoutineStep
        {
            CheckBeforeGoto,
            RobotGoto,
            End,
        }

        private int _gotoTimeout;
        private ModuleName _source;
        private int _sourceSlot;
        private Hand _hand;
 
        private EfemModule _robotModule;
 
        public EfemGotoRoutine(EfemModule robotModule) : base("EfemRobot")
        {
            Name = "Goto";

            _robotModule = robotModule;
        }

        public RState Start(params object[] objs)
        {
            _gotoTimeout = SC.GetValue<int>("EFEM.EfemRobot.GotoTimeout");

            Reset();

            Notify($"Start, Goto {_source} slot {_sourceSlot + 1}, by {_hand}");

            return Runner.Start("EfemRobot", Name);
        }


        public void Init(ModuleName source, int slot, Hand hand)
        {
 
            _source = source;
            _sourceSlot = slot;
            _hand = hand;
 
         }
 
        public void Abort()
        {
            Notify("Abort");
        }


        public RState Monitor()
        {
            Runner.Run(RoutineStep.CheckBeforeGoto, BeforeGoto, NullFun,        _gotoTimeout * 1000)
                .Run(RoutineStep.RobotGoto,         RobotGoto,  CheckRobotGoto, _gotoTimeout * 1000)
                .End(RoutineStep.End,               NullFun,    _delay_1s);
            if(Runner.Status == RState.End)
                Notify($"Finish, robot move to {_source} slot {_sourceSlot + 1}, by {_hand}");
            return Runner.Status;
        }

        public bool BeforeGoto()
        {
            Notify("Check robot goto motion is enabled");
       
            string reason = string.Empty;
        
            return true;
        }
        public bool RobotGoto()
        {
            Notify("robot execute goto command");

            string reason;
            if (!_robotModule.RobotDevice.Goto(_source, _hand, _sourceSlot, out reason))
            {
                Stop(reason);
                return false;
            }
            return true;
        }

        bool CheckRobotGoto()
        {
            return !_robotModule.RobotDevice.IsError && _robotModule.RobotDevice.IsIdle;
        } 
    }
}