Sic02-new/Modules/SicPM/Routines/PMServoUpRoutine.cs

using Aitex.Core.RT.Device;
using Aitex.Core.RT.Event;
using Aitex.Core.RT.Routine;
using Aitex.Core.RT.SCCore;
using MECF.Framework.Common.Equipment;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Servo.NAIS;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using static SicPM.PMModule;

namespace SicPM.Routines
{
    class PMServoUpRoutine : PMBaseRoutine
    {
        enum RoutineStep
        {
            SetServoOn,
            SetUpLimit,

            SetBlockOn,

            Delay,

            WaitStatusCorrect,

            SetBlockOff,
            SetBlockOff0
        }



        private int _timeout;

        private NAISServo servo;

        public PMServoUpRoutine(ModuleName module, PMModule pm) : base(module, pm)
        {
            Module = module.ToString();
            Name = "Servo Up";
            servo = DEVICE.GetDevice<NAISServo>($"{Module}.NAISServo");

            _timeout = SC.GetValue<int>($"NAISServo.UpTimeout");
        }


        public void Init()
        {
            //_useSettingValue = false;
        }
        public void Init(float basePressure, int ventDelayTime)
        {

        }


        public override Result Start(params object[] objs)
        {
            if (PMDevice.CheckServoAlarm())
            {
                EV.PostWarningLog(Module, "can not up,confinementring is error.");
                return Result.FAIL;
            }
            if (PMDevice.CheckServoIsBusy())
            {
                EV.PostWarningLog(Module, "can not vent,confinementring is busy.");
                return Result.FAIL;
            }
            //if (PMDevice.ServoState == ServoStates.Up && PMDevice.ConfinementRing.RingUpFaceback)
            //{
            //    return Result.DONE;
            //}
            Reset();

            Notify("Start");
            return Result.RUN;
        }


        public override Result Monitor()
        {
            try
            {
                if (SC.GetValue<bool>($"System.IsSimulatorMode"))
                {
                    PMDevice.ServoState = ServoStates.Up;
                    //PMDevice.ConfinementRing.RingUpSetpoint = true;
                    //PMDevice.ConfinementRing.RingDownSetpoint = false;
                    if (servo != null)
                    {
                        servo.IsStbOff = true;
                    }
                    Notify($"SimulatorMode Servo Down.");
                    return Result.DONE;
                }


                ServoBlockOff((int)RoutineStep.SetBlockOff0, PMDevice, _timeout);

                ServoUpLimit((int)RoutineStep.SetUpLimit, PMDevice, _timeout);

                ServoBlockOn((int)RoutineStep.SetBlockOn, PMDevice, _timeout);

                TimeDelay((int)RoutineStep.Delay, 2);

                WaitStatusCorrect((int)RoutineStep.WaitStatusCorrect, PMDevice, _timeout);

                //ServoBlockOff((int)RoutineStep.SetBlockOff, PMDevice, _timeout);
            }
            catch (RoutineBreakException)
            {
                return Result.RUN;
            }
            catch (RoutineFaildException)
            {
                return Result.FAIL;
            }

            Notify("Finished");

            return Result.DONE;
        }

        private void ServoUpLimit(int id, PMModuleBase pm, int timeout)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Run {pm.Name} set Block No High.");

                //if (!servo.SetBlockNoHigh())
                //{
                //    Stop(reason);
                //    return false;
                //}
                servo.SetBlockUp();
                return true;
            }, () =>
            {
                if (servo.AlarmStatus)
                {
                    Stop($"{pm.Name} Servo in error State.");
                    return null;
                }
                if (!servo.IsBusy)
                    return true;

                return false;

            }, timeout * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    Stop($"{pm.Name} prepare vent timeout, over {timeout} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        private void SetServoOn(int id, PMModuleBase pm, int timeout)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Run {pm.Name} set Servo On.");
                servo.SetServoOn();
                return true;
            }, () =>
            {
                if (servo.AlarmStatus)
                {
                    Stop($"{pm.Name} Servo in error State");
                    return null;
                }
                if (!servo.IsBusy)
                    return true;

                return false;

            }, timeout * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    Stop($"{pm.Name} set Servo On timeout, over {timeout} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        private void ServoBlockOn(int id, PMModuleBase pm, int timeout)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Run {pm.Name} set Stb On.");

                //if (!servo.SetBlockNoHigh())
                //{
                //    Stop(reason);
                //    return false;
                //}
                servo.SetStbOn();
                return true;
            }, () =>
            {
                if (servo.AlarmStatus)
                {
                    Stop($"{pm.Name} Servo in error State.");
                    return null;
                }
                if (!servo.IsBusy)
                {
                    PMDevice.ServoState = ServoStates.Uping;
                    return true;
                }

                return false;

            }, timeout * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    Stop($"{pm.Name} prepare vent timeout, over {timeout} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }
        private void ServoBlockOff(int id, PMModuleBase pm, int timeout)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Run {pm.Name} set Stb Off.");

                //if (!servo.SetBlockNoHigh())
                //{
                //    Stop(reason);
                //    return false;
                //}
                servo.SetStbOff();
                return true;
            }, () =>
            {
                if (servo.AlarmStatus)
                {
                    Stop($"{pm.Name} Servo in error State.");
                    return null;
                }
                if (!servo.IsBusy)
                    return true;

                return false;

            }, timeout * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    Stop($"{pm.Name} prepare vent timeout, over {timeout} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }
        private void WaitStatusCorrect(int id, PMModule pm, int timeout)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Run {pm.Name} Wait, Wait Status Correct");
                servo.Query();
                return true;
            }, () =>
            {
                if (servo.AlarmStatus)
                {
                    Stop($"{pm.Name} Servo in error State error");
                    return null;
                }
                if (!servo.IsBusy)
                {
                    //if ((!servo.MotorBusy && servo.PositionComplete) && pm.ConfinementRing.IsUp)
                    if (servo.IsArrived && pm.ConfinementRing.IsUp)
                    {
                        PMDevice.ServoState = ServoStates.Up;
                        return true;
                    }
                    else
                    {
                        servo.QueryArrival();
                    }
                }
                return false;

            }, timeout * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    Stop($"{pm.Name} vent timeout, over {timeout} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        public override void Abort()
        {
            if (!PMDevice.StopVent(out string reason))
            {
                EV.PostWarningLog(Module, reason);
            }

            Stop("aborted");
        }
    }
}