FurnaceELK/Furnace/FurnaceRT/Equipments/Boats/BoatMoveCycleTest.cs

using Aitex.Core.RT.Device;
using Aitex.Core.RT.Device.Unit;
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.Alarms;
using MECF.Framework.Common.Device.Bases;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.SubstrateTrackings;
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robot;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using FurnaceRT.Equipments.Systems;
using static Aitex.Core.RT.Device.Unit.IoBoat;
using System.Diagnostics;
using FurnaceRT.Devices;

namespace FurnaceRT.Equipments.Boats
{
    public class BoatMoveCycleTest : ModuleRoutine, IRoutine
    {
        enum RoutineStep
        {
            SetBoatSpeed,
            SetBoatDirection,
            BoatRAxisHome,
            BoatRAxisMove,
            SetBoatRAxisMoveStop,
            SetBoatInterval,
            CheckPrepareMove,
            AutoShutterOpen,
            Loop,
            EndLoop,
            Delay1,
            Delay2,
            Delay3,
            ZMoveToStart,
            ZMoveToEnd,
        }

        private BoatRotationMode _targetMode;
        private float _interval;
        private float _rotateTime;
        private BoatModule _boatModule;
        private int _timeout = 0;
        private int _shutterTimeout = 0;
        private bool _isRotate;
        private BoatRotationDirection _direction;
        private Stopwatch _timer = new Stopwatch();
        private int _count;
        private BoatPosition _positionStart;
        private BoatPosition _positionEnd;

        public BoatMoveCycleTest(BoatModule boatModule)
        {
            _boatModule = boatModule;
            Module = boatModule.Module;

            Name = "MoveTest";
        }
        public void Init(string positionStart = "HomePosition", string positionEnd = "CapPosition", string direction = "CW")
        {
            _positionStart = (BoatPosition)Enum.Parse(typeof(BoatPosition), positionStart);
            _positionEnd = (BoatPosition)Enum.Parse(typeof(BoatPosition), positionEnd);
            _direction = (BoatRotationDirection)Enum.Parse(typeof(BoatRotationDirection), direction);
        }

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

            _isRotate = _targetMode != BoatRotationMode.Stop;

            _timeout = SC.GetValue<int>($"{Module}.MotionTimeout");
            _count = SC.GetValue<int>($"Boat.CycleCount");
            _interval = (float)SC.GetValue<double>($"Boat.IntervalTime");
            _rotateTime = (float)SC.GetValue<double>($"Boat.BoatRotationServo.RotateTime");
            _shutterTimeout = SC.GetValue<int>($"{Module}.AutoShutter.MotionTimeout");
            string reason = string.Empty;
            if (!_boatModule.CheckPrepareMove(out reason, false))
            {
                _boatModule.BoatZAxisMoveFailedForInterlock.Set(reason);
                return Result.FAIL;
            }
            Notify($"Start");

            return Result.RUN;
        }

        public void Abort()
        {
            _boatModule.ZAxisDevice.ServoStop();
            _boatModule.RAxisDevice.ServoStop();
        }

        public override Result Monitor()
        {
            try
            {
                PauseRountine(_boatModule.RAxisDevice.IsPause);
                if (_boatModule.RAxisDevice.IsPause)
                    return Result.RUN;
                AutoShutterOpen((int)RoutineStep.AutoShutterOpen, true, 30);

                SetBoatRAxisMove((int)RoutineStep.BoatRAxisMove, _direction, (int)_rotateTime);

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

                CheckPrepareMove((int)RoutineStep.CheckPrepareMove, 2);

                SetBoatZAxisMoveStart((int)RoutineStep.ZMoveToStart, _positionStart, (int)_timeout);

                //SetBoatRAxisMoveStop((int)RoutineStep.SetBoatRAxisMoveStop);

                Delay((int)RoutineStep.Delay1, 2);
               
                Delay((int)RoutineStep.Delay2, _interval);

                //SetBoatRAxisHome((int)RoutineStep.BoatRAxisHome, _timeout);

                SetBoatZAxisMoveEnd((int)RoutineStep.ZMoveToEnd, _positionEnd, (int)_timeout);

                Delay((int)RoutineStep.Delay3, _interval);

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

            _boatModule.RAxisDevice.ServoStop();
            Notify("Finished");
            return Result.DONE;
        }
        private void AutoShutterOpen(int id, bool isOpen, int timeout)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"shutter {(isOpen ? "open" : "close")}");
                string reason;
                if (!_boatModule.ShutterDevice.SetOpen(isOpen, out reason))
                {
                    _boatModule.ShutterDevice.AutoShutterMoveFailedForInterlock.Set();
                    return false;
                }
                return true;
            }, () =>
            {

                return isOpen ? _boatModule.ShutterDevice.OpenCloseStatus == DeviceStatus.Open : _boatModule.ShutterDevice.OpenCloseStatus == DeviceStatus.Close;

            }, timeout * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    if (isOpen)
                    {
                        _boatModule.ShutterDevice.AutoShutterOpenTimeOut.Set($"can not complete in {timeout} seconds");
                    }
                    else
                    {
                        _boatModule.ShutterDevice.AutoShutterCloseTimeOut.Set($"can not complete in {timeout} seconds");
                    }
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }
        private void CheckPrepareMove(int id, int timeout)
        {
            var reason = "";
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Check boat move enable");
                return true;
            }, () =>
            {
                return _boatModule.CheckPrepareMove(out reason);
            }, timeout * 2 * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    _boatModule.BoatZAxisMoveFailedForInterlock.Set($"{reason} is not OK, can not complete in {timeout} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }
        private void SetBoatZAxisMoveStart(int id, BoatPosition position, int timeout)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Boat ZAxis movet to {position}");
                string reason;
                if (!_boatModule.ZAxisDevice.SetServoMoveTo(position.ToString(), out reason))
                {
                    //_boatModule.BoatDevice.BoatRAxisMoveFailedForInterlock.Description = reason;
                    _boatModule.BoatZAxisMoveFailedForInterlock.Set(reason);
                }
                return true;
            }, () =>
            {
                if (_boatModule.ZAxisDevice.IsError)
                    return null;
                return _boatModule.ZAxisDevice.CheckServoAtPosition(position.ToString());
            }, timeout * 2 * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    _boatModule.ZAxisDevice.ServoStop();
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    _boatModule.ZAxisDevice.ServoStop();
                    _boatModule.BoatZAxisMoveTimeOut.Set($"can not complete in {timeout} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }
        private void SetBoatZAxisMoveEnd(int id, BoatPosition position, int timeout)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Boat ZAxis movet to {position}");
                string reason;
                if (!_boatModule.ZAxisDevice.SetServoMoveTo(position.ToString(), out reason))
                {
                    //_boatModule.BoatDevice.BoatRAxisMoveFailedForInterlock.Description = reason;
                    _boatModule.BoatZAxisMoveFailedForInterlock.Set(reason);
                }
                return true;
            }, () =>
            {
                if (_boatModule.ZAxisDevice.IsError)
                    return null;
                return _boatModule.ZAxisDevice.CheckServoAtPosition(position.ToString());
            }, timeout * 2 * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    _boatModule.ZAxisDevice.ServoStop();
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    _boatModule.ZAxisDevice.ServoStop();
                    _boatModule.BoatZAxisMoveTimeOut.Set($"can not complete in {timeout} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        private void SetBoatRAxisMoveStop(int id)
        {
            Tuple<bool, Result> ret = Execute(id, () =>
            {
                Notify($"Set RAxis boat stop");

                _boatModule.RAxisDevice.ServoStop();

                return true;
            });

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

        private void SetBoatRAxisHome(int id, int timeout)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Boat RAxis home");

                if (!_boatModule.RAxisDevice.SetServoHome())
                {
                    _boatModule.BoatRAxisHomeFailed.Set();
                }

                return true;
            }, () =>
            {
                if (_boatModule.RAxisDevice.IsError)
                    return null;
                return true;

            }, timeout * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    _boatModule.RAxisDevice.ServoStop();
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    _boatModule.RAxisDevice.ServoStop();
                    _boatModule.BoatRAxisHomeTimeout.Set($"can not complete in {timeout} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }

        private void SetBoatRAxisMove(int id, BoatRotationDirection direction, int timeout)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Boat RAxis {direction}");
                _timer.Restart();
                string reason;
                if (!_boatModule.RAxisDevice.SetServoMoveTo(direction.ToString(), out reason))
                {
                    //_boatModule.BoatDevice.BoatRAxisMoveFailedForInterlock.Description = reason;
                    _boatModule.BoatRAxisMoveFailedForInterlock.Set(reason);
                }
                return true;
            }, () =>
            {
                if (_boatModule.RAxisDevice.IsError)
                    return null;
                return _boatModule.RAxisDevice.IsMoving;
            }, timeout * 2 * 1000);

            if (ret.Item1)
            {
                if (ret.Item2 == Result.FAIL)
                {
                    _boatModule.RAxisDevice.ServoStop();
                    throw (new RoutineFaildException());
                }
                else if (ret.Item2 == Result.TIMEOUT) //timeout
                {
                    _boatModule.RAxisDevice.ServoStop();
                    _boatModule.BoatRAxisMoveTimeOut.Set($"can not complete in {timeout} seconds");
                    throw (new RoutineFaildException());
                }
                else
                    throw (new RoutineBreakException());
            }
        }
    }
}