FurnaceELK/Furnace/FurnaceRT/Equipments/Boats/BoatHome.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.Sorter.Common;
using MECF.Framework.Common.Device.Bases;
using MECF.Framework.Common.Equipment;
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.IoAutoShutter;
using FurnaceRT.Devices;
using static Aitex.Core.RT.Device.Unit.IoBoat;

namespace FurnaceRT.Equipments.Boats
{
    public class BoatHome : ModuleRoutine, IRoutine
    {
        enum RoutineStep
        {
            AutoShutterOpen,
            AutoShutterClose,
            CheckPrepareMove,
            SetReset,
            SetServoOn,
            BoatZAxisHome,
            BoatRAxisHome,
            Delay1,
            SetBoatRAxisMoveStop,
        }

        private BoatModule _boatModule;
        private int _zAxisTimeout = 0;
        private int _rAxisTimeout = 0;
        private int _shutterTimeout = 0;

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

            Name = "Boat Home";
        }

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

            _zAxisTimeout = SC.GetValue<int>($"{Module}.BoatElevatorServo.MotionTimeout");
            _rAxisTimeout = SC.GetValue<int>($"{Module}.BoatRotationServo.MotionTimeout");
            _shutterTimeout = SC.GetValue<int>($"{Module}.AutoShutter.MotionTimeout");
            _boatModule.AutoShutterMoveFailedForInterlock.RetryMessage = (int)BoatModule.MSG.Home;
            _boatModule.AutoShutterOpenTimeOut.RetryMessage = (int)BoatModule.MSG.Home;
            _boatModule.AutoShutterCloseTimeOut.RetryMessage = (int)BoatModule.MSG.Home;
            _boatModule.BoatZAxisHomeFailed.RetryMessage = (int)BoatModule.MSG.Home;
            _boatModule.BoatZAxisHomeTimeout.RetryMessage = (int)BoatModule.MSG.Home;
            _boatModule.BoatRAxisHomeFailed.RetryMessage = (int)BoatModule.MSG.Home;
            _boatModule.BoatRAxisHomeTimeout.RetryMessage = (int)BoatModule.MSG.Home;

            Notify($"Start");

            return Result.RUN;
        }

        public void Abort()
        {
            _boatModule.BoatZAxisStop();
            _boatModule.BoatRAxisStop();
        }

        public override Result Monitor()
        {
            try
            {
                PauseRountine(_boatModule.RAxisDevice.IsPause);
                if (_boatModule.RAxisDevice.IsPause)
                    return Result.RUN;

                AutoShutterOpen((int)RoutineStep.AutoShutterOpen, true, _shutterTimeout);

                CheckPrepareMove((int)RoutineStep.CheckPrepareMove, 2);
                
                SetBoatZAxisMove((int)RoutineStep.BoatZAxisHome, BoatPosition.HomePosition, (int)_zAxisTimeout);

                SetBoatRAxisMoveStop((int)RoutineStep.SetBoatRAxisMoveStop);

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

                SetBoatRAxisHome((int)RoutineStep.BoatRAxisHome, _rAxisTimeout);

                AutoShutterOpen((int)RoutineStep.AutoShutterClose, false, _shutterTimeout);
            }
            catch (RoutineBreakException)
            {
                return Result.RUN;
            }
            catch (RoutineFaildException ex)
            {
                return Result.FAIL;
            }

            Notify("Finished");
            return Result.DONE;
        }
        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 _boatModule.RAxisDevice.IsHomeDone&& _boatModule.RAxisDevice.IsMoving && _boatModule.RAxisDevice.IsReady;

            }, 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 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 SetBoatZAxisMove(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 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 AutoShutterOpen(int id, bool isOpen, int timeout)
        //{
        //    Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
        //    {
        //        Notify($"Auto shutter {(isOpen ? "open" : "close")}");
        //        string reason;
        //        if (!_boatModule.AutoShutterDevice.SetOpen(isOpen, out reason))
        //        {
        //            _boatModule.AutoShutterMoveFailedForInterlock.Set();
        //            return false;
        //        }
        //        return true;
        //    }, () =>
        //    {

        //        return isOpen ? _boatModule.AutoShutterDevice.OpenCloseStatus == ASOpenCloseStatus.Open : _boatModule.AutoShutterDevice.OpenCloseStatus == ASOpenCloseStatus.Close;
        //        //return true;

        //    }, timeout * 1000);

        //    if (ret.Item1)
        //    {
        //        if (ret.Item2 == Result.FAIL)
        //        {
        //            throw (new RoutineFaildException());
        //        }
        //        else if (ret.Item2 == Result.TIMEOUT) //timeout
        //        {
        //            if (isOpen)
        //            {
        //                //autoShutter.AutoShutterOpenTimeOut.Description = $"can not complete in {timeout} seconds";
        //                _boatModule.AutoShutterOpenTimeOut.Set($"can not complete in {timeout} seconds");
        //            }
        //            else
        //            {
        //                //autoShutter.AutoShutterCloseTimeOut.Description = $"can not complete in {timeout} seconds";
        //                _boatModule.AutoShutterCloseTimeOut.Set($"can not complete in {timeout} seconds");
        //            }
        //            throw (new RoutineFaildException());
        //        }
        //        else
        //            throw (new RoutineBreakException());
        //    }
        //}

        //private void AutoShutterUp(int id, bool isUp, int timeout)
        //{
        //    Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
        //    {
        //        Notify($"Auto shutter {(isUp ? "up" : "down")}");
        //        string reason;
        //        if (!_boatModule.AutoShutterDevice.SetUp(isUp, out reason))
        //        {
        //            _boatModule.AutoShutterMoveFailedForInterlock.Set();
        //            return false;
        //        }
        //        return true;
        //    }, () =>
        //    {

        //        return isUp ? _boatModule.AutoShutterDevice.UpDownStatus == ASUpDownStatus.Up : _boatModule.AutoShutterDevice.UpDownStatus == ASUpDownStatus.Down;
        //        //return true;

        //    }, timeout * 1000);

        //    if (ret.Item1)
        //    {
        //        if (ret.Item2 == Result.FAIL)
        //        {
        //            throw (new RoutineFaildException());
        //        }
        //        else if (ret.Item2 == Result.TIMEOUT) //timeout
        //        {
        //            if (isUp)
        //            {
        //                //autoShutter.AutoShutterUpTimeOut.Description = $"can not complete in {timeout} seconds";
        //                _boatModule.AutoShutterUpTimeOut.Set($"can not complete in {timeout} seconds");
        //            }
        //            else
        //            {
        //                //autoShutter.AutoShutterDownTimeOut.Description = $"can not complete in {timeout} seconds";
        //                _boatModule.AutoShutterDownTimeOut.Set($"can not complete in {timeout} seconds");
        //            }
        //            throw (new RoutineFaildException());
        //        }
        //        else
        //            throw (new RoutineBreakException());
        //    }
        //}

        //private void SetReset(int id)
        //{
        //    Tuple<bool, Result> ret = Execute(id, () =>
        //    {
        //        Notify($"Set boat reset");

        //        _boatModule.BoatDevice.SetReset();

        //        return true;
        //    });

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

        //private void SetServoOn(int id)
        //{
        //    Tuple<bool, Result> ret = Execute(id, () =>
        //    {
        //        Notify($"Set boat servo on");

        //        _boatModule.BoatDevice.SetServoOn();

        //        return true;
        //    });

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

        //private void SetZAxisBoatSpeed(int id, float speed)
        //{
        //    Tuple<bool, Result> ret = Execute(id, () =>
        //    {
        //        Notify($"Set ZAxis boat speed={speed}");

        //        _boatModule.BoatDevice.SetZAxisSpeed(speed);

        //        return true;
        //    });

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

        //private void BoatZAxisMoveTo(int id, BoatPosition targetPosition, int timeout)
        //{
        //    Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
        //    {
        //        Notify($"Boat ZAxis home");
        //        string reason;
        //        if (!_boatModule.BoatDevice.ZAxisMoveTo(targetPosition, out reason))
        //        {
        //            //_boatModule.BoatDevice.BoatZAxisMoveFailedForInterlock.Description = reason;
        //            _boatModule.BoatDevice.BoatZAxisHomeFailed.Set(reason);
        //            return false;
        //        }
        //        return true;
        //    }, () =>
        //    {

        //        return _boatModule.BoatDevice.BoatCurrentPosition == targetPosition && !_boatModule.BoatDevice.IsZAxisMoving;

        //    }, timeout * 1000);

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

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

        //        if (!_boatModule.BoatDevice.RAxisRotate(rotationMode, out string reason))
        //        {
        //            _boatModule.BoatDevice.BoatRAxisHomeFailed.Set();
        //        }

        //        return true;
        //    }, () =>
        //    {

        //        return _boatModule.BoatDevice.IsRAxisAtHome && !_boatModule.BoatDevice.IsRAxisRotating;

        //    }, timeout * 1000);

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

        //private void SetBoatRAxisSpeed(int id, float speed)
        //{
        //    Tuple<bool, Result> ret = Execute(id, () =>
        //    {
        //        Notify($"Set RAxis boat speed={speed}");

        //        _boatModule.SetRAxisSpeed(speed);

        //        return true;
        //    });

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

        //private void SetBoatRAxisInterval(int id, float interval)
        //{
        //    Tuple<bool, Result> ret = Execute(id, () =>
        //    {
        //        Notify($"Set boat r axis interval={interval}");

        //        _boatModule.SetRAxisIntervalPosition(interval);

        //        return true;
        //    });

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