OHV/Dev/OHV/VehicleControlSystem/ControlLayer/Clamp.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Threading.Tasks;
using FluentResults;
using GSG.NET.Concurrent;
using GSG.NET.Logging;
using OHV.Common.Events;
using OHV.Common.Shareds;
using OHV.SqliteDAL;
using Prism.Events;
using VehicleControlSystem.ControlLayer.Axis;
using VehicleControlSystem.ControlLayer.Lib.EziPlusE;

namespace VehicleControlSystem.ControlLayer
{
    public class Clamp : ControlObjectBase
    {
        SqliteManager sql = null;
        List<EzAxis> axes = new List<EzAxis>();
        static Logger logger = Logger.GetLogger();

        IEventAggregator eventAggregator;
        TaskCancel taskCancel = new TaskCancel();

        public Clamp(SqliteManager sqliteManager, IEventAggregator ea)
        {
            this.sql = sqliteManager;
            this.eventAggregator = ea;
        }

        public void Init()
        {
            this.CreateAxis();

            this.eventAggregator.GetEvent<AxisControlPubSubEvent>().Unsubscribe(ReceivedMessageEvent);
            this.eventAggregator.GetEvent<AxisControlPubSubEvent>().Subscribe(ReceivedMessageEvent);
        }

        void ReceivedMessageEvent(AxisControlEventArgs args)
        {
            if (args.Dir == AxisControlEventArgs.eEventDir.ToBack)
            {
                switch (args.Kind)
                {
                    case AxisControlEventArgs.eAxisControlKind.Stop:
                        break;
                    case AxisControlEventArgs.eAxisControlKind.EStop:
                        break;
                    case AxisControlEventArgs.eAxisControlKind.Move:
                        break;
                    case AxisControlEventArgs.eAxisControlKind.Jog:
                        this.ReqJog(args);
                        break;
                    case AxisControlEventArgs.eAxisControlKind.CurrentPosition:
                        break;
                    case AxisControlEventArgs.eAxisControlKind.ServoOn:
                        this.ReqServoOn( args );
                        break;
                    case AxisControlEventArgs.eAxisControlKind.ServoOff:
                        this.ReqServoOff( args );
                        break;
                    case AxisControlEventArgs.eAxisControlKind.ReqCurrentPosition:
                        this.ReqStartCurrentPosition();
                        break;
                    case AxisControlEventArgs.eAxisControlKind.ReqStopCurrentPosition:
                        this.taskCancel.Cancel();
                        this.taskCancel.WaitAll();
                        break;
                    case AxisControlEventArgs.eAxisControlKind.OriginReturn:
                        this.ReqOriginReturn(args);
                        break;
                    case AxisControlEventArgs.eAxisControlKind.FaultReset:
                        this.ReqFaultReset( args );
                        break;
                    default:
                        break;
                }
            }
        }

        void PublishEvent(AxisControlEventArgs args)
        {
            this.eventAggregator.GetEvent<AxisControlPubSubEvent>().Publish(args);
        }

        #region Requst Method
        private void ReqFaultReset(AxisControlEventArgs args)
        {
            var axis = this.axes.Where(x => x.Config.AxisName.Equals(args.AxisName)).FirstOrDefault();
            int resultNo = axis.ResetAmpFault();

            var msg = new AxisControlEventArgs
            {
                AxisName = args.AxisName,
                Kind = AxisControlEventArgs.eAxisControlKind.FaultReset,
            };
            msg.Result = Results.Ok("Servo On");
            this.PublishEvent(msg);
        }

        private void ReqServoOn(AxisControlEventArgs args)
        {
            var axis = this.axes.Where(x => x.Config.AxisName.Equals(args.AxisName)).FirstOrDefault();
            axis.ServoOn();

            var msg = new AxisControlEventArgs
            {
                AxisName = args.AxisName,
                Kind = AxisControlEventArgs.eAxisControlKind.ServoOn,
            };
            msg.Result = Results.Ok("Servo On");
            this.PublishEvent(msg);
        }

        private void ReqServoOff(AxisControlEventArgs args)
        {
            var axis = this.axes.Where(x => x.Config.AxisName.Equals(args.AxisName)).FirstOrDefault();
            axis.ServoOff();

            var msg = new AxisControlEventArgs
            {
                AxisName = args.AxisName,
                Kind = AxisControlEventArgs.eAxisControlKind.ServoOff,
            };
            msg.Result = Results.Ok("Servo Off");
            this.PublishEvent(msg);
        }

        void ReqStartCurrentPosition()
        {
            var task = Task.Factory.StartNew(() =>
            {
                double[] beforePosition = new double[this.axes.Count];
                beforePosition.Initialize();

                while (!this.taskCancel.Canceled)
                {
                    LockUtils.Wait(500);

                    int index = 0;
                    this.axes.ForEach(a =>
                    {
                        if (beforePosition[index] != a.CurrentPosition)
                        {
                            beforePosition[index] = a.CurrentPosition;

                            var msg = new AxisControlEventArgs
                            {
                                AxisName = a.Config.AxisName,
                                Dir = AxisControlEventArgs.eEventDir.ToFront,
                                Kind = AxisControlEventArgs.eAxisControlKind.CurrentPosition,
                                CurrentPosition = a.CurrentPosition,
                            };
                            this.PublishEvent(msg);
                        }
                        index++;
                    });
                }
            });

            this.taskCancel.Add(task);
        }

        void ReqJog(AxisControlEventArgs arg)
        {
            if (arg.JogDir == AxisControlEventArgs.eJogMoveDir.Positive)
                this.JogMove(arg.AxisName, E_JogMoveDir.Positive, 0);
            else
                this.JogMove(arg.AxisName, E_JogMoveDir.Negative, 0);
        }

        void ReqOriginReturn(AxisControlEventArgs arg)
        {
            int result = 0;
            if (arg.AxisName.Equals(ConstString.AXIS_CARRIER_LOCK_LEFT))
                result = this.LeftOriginReturn();
            else
                result = this.RightOriginReturn();

            var msg = new AxisControlEventArgs
            {
                AxisName = arg.AxisName,
                Kind = AxisControlEventArgs.eAxisControlKind.OriginReturn,
            };
            if (result != EziMOTIONPlusELib.FMM_OK)
                msg.Result = Results.Fail<int>("Origin Fail").WithValue(result);
            else
                msg.Result = Results.Ok();

            this.PublishEvent(msg);
        }
        #endregion

        void CreateAxis()
        {
            sql.AxisConfigDAL.All.ForEach(config =>
            {
                Axis.AxisConfig c = new Axis.AxisConfig
                {
                    AxisName = config.Name,
                    AxisNo = config.Id,
                    Scale = config.Scale,
                    Address = config.Address,
                };
                axes.Add(new EzAxis(c));
            });

            this.axes.ForEach(axis =>
            {
                var data = sql.AxisVelocityDataDAL.GetK(axis.Config.AxisName);
                Axis.AxisData axisData = new AxisData
                {
                    AxisNo = data.id,
                    AxisName = data.AxisName,
                    AutoRunVelocity = data.AutoRunVelocity,
                    AutoRunAccelerate = data.AutoRunAccelerate,
                    AutoRunDecelerate = data.AutoRunDecelerate,
                    ManualRunVelocity = data.ManualRunVelocity,
                    JogFast = data.JogFast,
                    JogSlow = data.JogSlow,
                    Tolerance = data.Tolerance,
                };

                axis.Initialize(axisData);
            });
        }

        public int LeftOriginReturn()
        {
            var servo = this.axes.Where(s => s.Config.AxisName.Equals(ConstString.AXIS_CARRIER_LOCK_LEFT)).Single();
            return servo.OriginReturn(true);
        }

        public int RightOriginReturn()
        {
            var servo = this.axes.Where(s => s.Config.AxisName.Equals(ConstString.AXIS_CARRIER_LOCK_RIGHT)).Single();
            return servo.OriginReturn(true);
        }

        public int AllOriginReturn()
        {
            int result = 0;
            result = LeftOriginReturn();
            if (result != EziMOTIONPlusELib.FMM_OK)
                return 7;

            result = RightOriginReturn();
            if (result != EziMOTIONPlusELib.FMM_OK)
                return 8;

            return result;
        }

        public int IsAllAxisOriginReturn()
        {
            int result = 0;
            this.axes.ForEach(x =>
            {
                if (!x.IsOriginReturn())
                {
                    if (x.Config.AxisName.Equals(ConstString.AXIS_CARRIER_LOCK_LEFT))
                        result = 5;
                    else
                        result = 6;
                    return;
                }
            });

            return result;
        }

        #region Data
        public void SavePositionData(string pos, double left, double right)
        {
            var dataList = sql.AxisPositionDataDAL.GetKFromPostion(pos);
            var sqlLeft = dataList.Where(x => x.AxisName.Equals(ConstString.AXIS_CARRIER_LOCK_LEFT)).Single();
            var sqlRight = dataList.Where(x => x.AxisName.Equals(ConstString.AXIS_CARRIER_LOCK_RIGHT)).Single();

            sqlLeft.Value = left;
            sql.AxisPositionDataDAL.Update(sqlLeft);

            sqlRight.Value = right;
            sql.AxisPositionDataDAL.Update(sqlRight);
        }
        #endregion

        public int JogMove(string axisName, E_JogMoveDir dir, int velocity)
        {
            logger.D($"JogMove - Axis[{axisName}] / Dir[{dir}] / vel[{velocity}]");

            var axis = this.axes.Where(x => x.Config.AxisName.Equals(axisName)).Single();
            return axis.JogMove(dir, velocity);
        }

        public int MoveToLockPosition(string axisName)
        {
            var servo = this.axes.Where(x => x.Config.AxisName.Equals(axisName)).Single();
            var axisPositionDatas = sql.AxisPositionDataDAL.GetKFromName(axisName).ToList();
            var data = axisPositionDatas.Where(x => x.Name.Equals(ConstString.TEACH_POSITION_LOCK)).Single();

            int result = servo.Move(data.Value);

            return result;
        }

        public int MoveToUnlockPosition(string axisName)
        {
            var servo = this.axes.Where(x => x.Config.AxisName.Equals(axisName)).Single();
            var axisPositionDatas = sql.AxisPositionDataDAL.GetKFromName(axisName).ToList();
            var data = axisPositionDatas.Where(x => x.Name.Equals(ConstString.TEACH_POSITION_UNLOCK)).Single();

            int result = servo.Move(data.Value);

            return result;
        }

        /// <summary>
        /// 두개를 동시에 Lock
        /// </summary>
        /// <returns></returns>
        public int Lock_Sync()
        {
            int result = 0;

            result = this.AllOriginReturn();
            if (result != 0)
                return result;

            var leftPositionDatas = sql.AxisPositionDataDAL.GetKFromName(ConstString.AXIS_CARRIER_LOCK_LEFT).ToList();
            var leftData = leftPositionDatas.Where(x => x.Name.Equals(ConstString.TEACH_POSITION_LOCK)).Single();
            var left = this.axes.Where(x => x.Config.AxisName.Equals(ConstString.AXIS_CARRIER_LOCK_LEFT)).Single();

            var rightPositionDatas = sql.AxisPositionDataDAL.GetKFromName(ConstString.AXIS_CARRIER_LOCK_RIGHT).ToList();
            var rightData = leftPositionDatas.Where(x => x.Name.Equals(ConstString.TEACH_POSITION_LOCK)).Single();
            var right = this.axes.Where(x => x.Config.AxisName.Equals(ConstString.AXIS_CARRIER_LOCK_RIGHT)).Single();

            result = left.StartMove(leftData.Value, 0);
            if (result != EziMOTIONPlusELib.FMM_OK)
                return result;

            result = right.StartMove(rightData.Value, 0);
            if (result != EziMOTIONPlusELib.FMM_OK)
                return result;

            result = left.Wait4Done();
            if (result != EziMOTIONPlusELib.FMM_OK)
                return result;

            result = right.Wait4Done();
            if (result != EziMOTIONPlusELib.FMM_OK)
                return result;


            return EziMOTIONPlusELib.FMM_OK;
        }

        /// <summary>
        /// 두개의 Axis 를 동시에 Unlock
        /// </summary>
        /// <returns></returns>
        public int Unlock_Sync()
        {
            int result = 0;

            result = this.AllOriginReturn();
            if (result != 0)
                return result;

            var leftPositionDatas = sql.AxisPositionDataDAL.GetKFromName(ConstString.AXIS_CARRIER_LOCK_LEFT).ToList();
            var leftData = leftPositionDatas.Where(x => x.Name.Equals(ConstString.TEACH_POSITION_UNLOCK)).Single();
            var left = this.axes.Where(x => x.Config.AxisName.Equals(ConstString.AXIS_CARRIER_LOCK_LEFT)).Single();

            var rightPositionDatas = sql.AxisPositionDataDAL.GetKFromName(ConstString.AXIS_CARRIER_LOCK_RIGHT).ToList();
            var rightData = leftPositionDatas.Where(x => x.Name.Equals(ConstString.TEACH_POSITION_UNLOCK)).Single();
            var right = this.axes.Where(x => x.Config.AxisName.Equals(ConstString.AXIS_CARRIER_LOCK_RIGHT)).Single();

            result = left.StartMove(leftData.Value, 0);
            if (result != EziMOTIONPlusELib.FMM_OK)
                return result;

            result = right.StartMove(rightData.Value, 0);
            if (result != EziMOTIONPlusELib.FMM_OK)
                return result;

            result = left.Wait4Done();
            if (result != EziMOTIONPlusELib.FMM_OK)
                return result;

            result = right.Wait4Done();
            if (result != EziMOTIONPlusELib.FMM_OK)
                return result;


            return EziMOTIONPlusELib.FMM_OK;
        }
    }
}