FurnaceELK/Furnace/FurnaceRT/Equipments/Boats/BoatRAxisMove.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;

namespace FurnaceRT.Equipments.Boats
{
    public class BoatRAxisMove : ModuleRoutine, IRoutine
    {
        enum RoutineStep
        {
            SetBoatSpeed,
            SetBoatDirection,
            BoatRAxisHome,
            BoatRAxisMove,
            SetBoatInterval,
        }

        private BoatRotationMode _targetMode;
        private float _speed;
        private float _interval;
        private BoatModule _boatModule;
        private int _timeout = 0;
        private bool _isRotate;
        private BoatRotationDirection _direction;

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

            Name = "RAxisMove";
        }

        public void Init(string rotationMode, string speed, string interval, string direction = "CW")
        {
            _targetMode = (BoatRotationMode)Enum.Parse(typeof(BoatRotationMode), rotationMode);
            float.TryParse(speed, out _speed);
            float.TryParse(interval, out _interval);
            _direction = (BoatRotationDirection)Enum.Parse(typeof(BoatRotationDirection), direction);

            var para = new List<object> { _targetMode, speed, interval };
            _boatModule.BoatRAxisMoveFailedForInterlock.RetryMessage = (int)BoatModule.MSG.RAxisMoveRetry;
            _boatModule.BoatRAxisMoveFailedForInterlock.RetryMessageParas = para.ToArray();
            _boatModule.BoatRAxisMoveTimeOut.RetryMessage = (int)BoatModule.MSG.RAxisMoveRetry;
            _boatModule.BoatRAxisMoveTimeOut.RetryMessageParas = para.ToArray();
        }

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

            if (_targetMode == BoatRotationMode.None)
                return Result.DONE;

            _isRotate = _targetMode != BoatRotationMode.Stop;

            _timeout = SC.GetValue<int>($"{Module}.BoatRotationServo.MotionTimeout");

            Notify($"Start");

            return Result.RUN;
        }

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

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

                if (_boatModule.RAxisDevice.IsError)
                    return Result.FAIL;

                SetBoatRAxisMove((int)RoutineStep.SetBoatDirection, _direction, _speed, _timeout);
            }
            catch (RoutineBreakException)
            {
                return Result.RUN;
            }
            catch (RoutineFaildException ex)
            {
                return Result.FAIL;
            }

            Notify("Finished");
            return Result.DONE;
        }

        private void SetBoatRAxisMove(int id, BoatRotationDirection direction, float speed, int timeout)
        {
            Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
            {
                Notify($"Boat RAxis {direction}");
                string reason;
                if (!_boatModule.RAxisDevice.SetServoMoveTo(direction.ToString(), out reason, speed))
                {
                    //_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());
            }
        }
    }
}