From 0d97500a3aac13b642fc93fae2e5dd01e1086b21 Mon Sep 17 00:00:00 2001
From: wells.liu <wells.liu@broconcentric.com>
Date: 星期一, 29 六月 2020 12:31:12 +0800
Subject: [PATCH] 暂存 固高板卡

---
 src/Bro.Device.GTSCard/GTSCardDriver.cs | 1037 ++++++++++++++++++++++++++++++++++++++++++++++++++++++---
 1 files changed, 973 insertions(+), 64 deletions(-)

diff --git a/src/Bro.Device.GTSCard/GTSCardDriver.cs b/src/Bro.Device.GTSCard/GTSCardDriver.cs
index a51429e..03b4c15 100644
--- a/src/Bro.Device.GTSCard/GTSCardDriver.cs
+++ b/src/Bro.Device.GTSCard/GTSCardDriver.cs
@@ -1,27 +1,101 @@
 锘縰sing Bro.Common.Base;
+using Bro.Common.Helper;
 using Bro.Common.Interface;
 using Bro.Common.Model;
 using System;
+using System.Collections.Concurrent;
 using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using System.Drawing;
 using System.Linq;
 using System.Text;
+using System.Threading;
 using System.Threading.Tasks;
 
 namespace Bro.Device.GTSCard
 {
-    public class GTSCardDriver : DeviceBase,IMonitor, IMotion
+    [Device("GTSCard", "鍥洪珮鏉垮崱", EnumHelper.DeviceAttributeType.Device)]
+    public class GTSCardDriver : DeviceBase, IMonitor, IMotion
     {
         public event Action<DateTime, string, IDevice, MonitorSet> OnMonitorInvoke;
         public event Action<DateTime, IDevice, WarningSet> OnMonitorAlarm;
+
+        public delegate bool OnAxisStartToCheckDelegate(int axisIndex, int startPosition, int endPosition);
+        // 寮傚父浜嬩欢
+        public Action<Exception> OnExceptionRaised;
+
+        public GTSCardInitialConfig IConfig
+        {
+            get
+            {
+                return InitialConfig as GTSCardInitialConfig;
+            }
+        }
+
+        static Dictionary<int, object> axisMoveLockDict = new Dictionary<int, object>();
+
+        /// <summary>
+        /// 杞磋繍鍔ㄥ紑濮嬫椂鐨勬娴嬶紝true:鏈夊啿绐� 涓嶅彲缁х画鎵ц false锛氭棤鍐茬獊,鍙户缁墽琛�
+        /// </summary>
+        public event OnAxisStartToCheckDelegate OnAxisStartToCheckConfliction;
+        /// <summary>
+        /// 鏆傚仠锛堢嚎绋嬪悓姝ヤ簨浠讹級
+        /// </summary>
+        Dictionary<int, ManualResetEvent> axisImmediatePauseHandleDict = new Dictionary<int, ManualResetEvent>();
+        Dictionary<int, CancellationTokenSource> axisMoveCancelDict = new Dictionary<int, CancellationTokenSource>();
+        /// <summary>
+        /// 杩愯杩囩▼涓殑绾跨▼绛夊緟
+        /// </summary>
+        private Dictionary<int, ManualResetEvent> runningEventDic = new Dictionary<int, ManualResetEvent>();
+        private Dictionary<int, AutoResetEvent> axisMovingHandleDict = new Dictionary<int, AutoResetEvent>();
+        private ConcurrentDictionary<int, int> axisDestination = new ConcurrentDictionary<int, int>();
+
+        private ObservableCollection<int> _commandAxisList = new ObservableCollection<int>();
+        public Action<bool> CommandAxisCountChangedAction = null;
+        private Dictionary<int, VelocityPara> velIndexDict = new Dictionary<int, VelocityPara>();
+        ManualResetEvent _pauseHandle = new ManualResetEvent(true);
+
+        static object lockObj = new object();
+        static object _commandAxisLock = new object();
+        /// <summary>
+        /// 鏄惁澶嶄綅鏍囧織
+        /// </summary>
+        bool _isResetting = false;
+
+        public void SetResetFlag(bool isReset)
+        {
+            _isResetting = isReset;
+        }
 
         public List<AxisInfo> GetCurrentAxisInfo(params string[] axisName)
         {
             throw new NotImplementedException();
         }
 
+        #region DeviceBase
+
         protected override void Init()
         {
-            throw new NotImplementedException();
+            InitialMotionCard((short)IConfig.CardNum, IConfig.InitialConfigFilePath);
+
+            axisMoveLockDict = IConfig.AxisSettings.ToDictionary(a => a.AxisIndex, a => new object());
+            runningEventDic = IConfig.AxisSettings.ToDictionary(a => a.AxisIndex, a => new ManualResetEvent(false));
+            axisMovingHandleDict = IConfig.AxisSettings.ToDictionary(a => a.AxisIndex, a => new AutoResetEvent(true));
+            axisImmediatePauseHandleDict = IConfig.AxisSettings.ToDictionary(a => a.AxisIndex, a => new ManualResetEvent(true));
+            axisMoveCancelDict = IConfig.AxisSettings.ToDictionary(a => a.AxisIndex, a => new CancellationTokenSource());
+
+            axisMoveCancelDict.Values.ToList().ForEach(c =>
+            {
+                c = new CancellationTokenSource();
+            });
+
+            _commandAxisList.CollectionChanged -= CommandAxisList_CollectionChanged;
+            _commandAxisList.CollectionChanged += CommandAxisList_CollectionChanged;
+        }
+
+        private void CommandAxisList_CollectionChanged(object sender, System.Collections.Specialized.NotifyCollectionChangedEventArgs e)
+        {
+            CommandAxisCountChangedAction?.Invoke(_commandAxisList.Count > 0);
         }
 
         protected override void Pause()
@@ -43,43 +117,874 @@
         {
             throw new NotImplementedException();
         }
+        #endregion
 
-        /// <summary>
-        /// 鐐逛綅鍒扮偣浣� 杩愬姩
-        /// </summary>
-        /// <param name="cardNum">鍗″彿</param>
-        /// <param name="axisNum">杞村彿</param>
-        /// <param name="prfPosition">瑙勫垝浣嶇疆,鍗曚綅姣背</param>
-        /// <param name="prfVelocity">瑙勫垝閫熷害,鍗曚綅绫虫瘡绉�</param>
-        public void P2P(short cardNum, short axisNum, int prfPosition, int prfVelocity)
+        #region GTSCard
+
+        public void ClearPosition(short cardNum, short axisNum)
         {
-            GTSCardAPI.TTrapPrm trapprm;
-            GTSCardAPI.GT_PrfTrap(cardNum, axisNum);
-            GTSCardAPI.GT_GetTrapPrm(cardNum, axisNum, out trapprm);
-            trapprm.acc = GTSCardParameter.P2PAcc;
-            trapprm.dec = GTSCardParameter.P2PDec;
-            trapprm.smoothTime = 1;
-            GTSCardAPI.GT_SetTrapPrm(cardNum, axisNum, ref trapprm);
-            GTSCardAPI.GT_SetPos(cardNum, axisNum, prfPosition * GTSCardParameter.Dangliang);
-            GTSCardAPI.GT_SetVel(cardNum, axisNum, prfVelocity * GTSCardParameter.Dangliang);
-            GTSCardAPI.GT_Update(cardNum, 1 << (axisNum - 1));
+            int ret = GTSCardAPI.GT_SetPos(cardNum, axisNum, 0);
         }
 
         /// <summary>
-        /// Jog杩愬姩
+        /// Load Motion Card parameter from file
         /// </summary>
-        /// <param name="cardNum"></param>
-        /// <param name="axisNum"></param>
-        /// <param name="velocity">瑙勫垝閫熷害锛屽崟浣嶇背姣忕</param>
-        public void Jog(short cardNum, short axisNum, double velocity)
+        /// <param name="fileName">Invalid Parameter</param>
+        /// <returns></returns>
+        public void InitialMotionCard(short cardNum, string fileName)
+        {
+            var res = GTSCardAPI.GT_LoadConfig(cardNum, fileName);
+            if (res != GTSCardAPI.ResultSuccess)
+            {
+                throw new Exception("鏉垮崱杞藉叆閰嶇疆鏂囦欢寮傚父锛岄敊璇爜锛�" + res);
+            }
+
+        }
+
+        /// <summary>
+        /// 鍗曚釜杞� 鐐逛綅鍒扮偣浣嶈繍鍔紙寮傛锛�
+        /// </summary>
+        /// <param name="item">杩愬姩瀵硅薄</param>
+        /// <returns></returns>
+        public async Task SingleAxisMovingAsync(MovingOption item)
+        {
+            await Task.Run(() =>
+            {
+                SingleAxisMoving(item);
+            });
+        }
+
+        /// <summary>
+        /// 鍗曚釜杞� 鐐逛綅鍒扮偣浣嶈繍鍔紙寮傛锛�
+        /// </summary>
+        /// <param name="item">杩愬姩瀵硅薄</param>
+        public void SingleAxisMoving(MovingOption item)
+        {
+            if (IConfig.AxisSettings.FirstOrDefault(a => a.AxisIndex == item.AxisIndex)?.IsAxisEnabled ?? false)
+            {
+                axisImmediatePauseHandleDict[item.AxisIndex].WaitOne();
+                VelocityPara vel = new VelocityPara();
+                if (item.VelocityPara.Velocity != 0)
+                {
+                    velIndexDict[item.AxisIndex] = vel = item.VelocityPara;
+                }
+                else
+                {
+                    vel = velIndexDict[item.AxisIndex];
+                }
+
+                string _position = "";
+                string motionType = item.MoveMode == EnumHelper.MotorMoveMode.Normal ? (item.IsAbsolute ? "Abs" : "Rel") : item.MoveMode.ToString();
+
+                _position = $"{DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss.fff")},{item.AxisIndex},{motionType},{GetCmdOrPosition(item.AxisIndex, 0).ToString()},{GetCmdOrPosition(item.AxisIndex, 1).ToString()},{item.Destination},";
+
+                lock (axisMoveLockDict[item.AxisIndex])
+                {
+                    Task.Run(() =>
+                    {
+                        lock (_commandAxisLock)
+                        {
+                            try
+                            {
+                                if (!_commandAxisList.Contains(item.AxisIndex))
+                                {
+                                    _commandAxisList.Add(item.AxisIndex);
+                                }
+                            }
+                            catch (Exception)
+                            {
+                            }
+                        }
+                    });
+
+                    switch (item.MoveMode)
+                    {
+                        case EnumHelper.MotorMoveMode.Normal:
+                            {
+                                if (_isResetting)
+                                {
+                                    LogAsync(DateTime.Now, "澶嶄綅涓惎鍔ㄨ繍鍔ㄥ紓甯�", item.AxisIndex + "鍚姩杩愬姩寮傚父");
+                                    return;
+                                }
+
+                                SetAxisParam(item.AxisIndex, vel);
+                                if (item.IsAbsolute)
+                                {
+                                    MoveAbs(item.AxisIndex, item.Destination, (int)(vel.Velocity * IConfig.AxisVelocityRatio));
+                                }
+                                else
+                                {
+                                    MoveRel(item.AxisIndex, item.Destination, (int)(vel.Velocity * IConfig.AxisVelocityRatio));
+                                }
+                            }
+                            break;
+                        case EnumHelper.MotorMoveMode.FindOri:
+                            {
+                                AxisSetting setting = IConfig.AxisSettings.FirstOrDefault(u => u.AxisIndex == item.AxisIndex);
+                                StartHoming(item.AxisIndex, setting.HomeMode, setting.IsHomePositive ? 1 : 0, item.VelocityPara.Dec, item.VelocityPara.Acc, item.VelocityPara.Velocity);
+                            }
+                            break;
+                    }
+
+                    Task.Run(() =>
+                    {
+                        lock (_commandAxisLock)
+                        {
+                            try
+                            {
+                                _commandAxisList.Remove(item.AxisIndex);
+                            }
+                            catch (Exception)
+                            {
+                            }
+                        }
+                    });
+                }
+
+                _position += $"{GetCmdOrPosition(item.AxisIndex, 0).ToString()},";
+                _position += $"{DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss.fff")}";
+                LogAsync(DateTime.Now, "", _position);
+            }
+        }
+
+        public int GetCmdOrPosition(int axisNum, int flag = 0)
+        {
+            int position = 0;
+
+            if (flag == 0)
+            {
+                GetPosition(axisNum, ref position);
+            }
+            else
+            {
+                GetCmdPosition(axisNum, ref position);
+            }
+
+            return position;
+        }
+
+        /// <summary>
+        ///Get single Axis Feedback position 
+        /// </summary>
+        /// <param name="axisNum">Axis number</param>
+        /// <param name="nPosition">Feedback/Encorde position </param>
+        /// <returns></returns>
+        public void GetPosition(int axisNum, ref int nPosition)
+        {
+            lock (lockObj)
+            {
+                double prfpos = 0; uint pclock = 0;
+                var ret = GTSCardAPI.GT_GetPrfPos((short)IConfig.CardNum, (short)axisNum, out prfpos, 1, out pclock);
+                if (ret != GTSCardAPI.ResultSuccess)
+                {
+                    throw new Exception("杞�" + axisNum + "鑾峰彇褰撳墠浣嶇疆寮傚父锛岄敊璇爜锛�" + ret);
+                }
+                nPosition = prfpos / IConfig.AxisVelocityRatio;
+            }
+        }
+
+        public int GetPosition(int axisNum)
+        {
+            int position = 0;
+
+            if (!IConfig.AxisSettings.FirstOrDefault(u => u.AxisIndex == axisNum).IsUseCmmdPosition)
+            {
+                GetPosition(axisNum, ref position);
+            }
+            else
+            {
+                GetCmdPosition(axisNum, ref position);
+            }
+
+            return position;
+        }
+
+        /// <summary>
+        ///Get single Axis Command position 
+        /// </summary>
+        /// <param name="axisNum">Axis number</param>
+        /// <param name="nPosition">Command position </param>
+        /// <returns></returns>
+        public void GetCmdPosition(int axisNum, ref int nPosition)
+        {
+            var ret = GTSCardAPI.APS_get_command(axisNum, ref nPosition);
+            if (ret != (Int32)GTSCardParameter.ResultSuccess)
+            {
+                throw new Exception("杞�" + axisNum + "鑾峰彇褰撳墠浣嶇疆寮傚父锛岄敊璇爜锛�" + ret);
+            }
+            nPosition = prfpos / IConfig.AxisVelocityRatio;
+        }
+
+        /// <summary>
+        /// Set AxisParam
+        /// </summary>
+        /// <param name="axisNo"></param>
+        /// <param name="param"></param>
+        /// <returns></returns>
+        public void SetAxisParam(int axisNum, VelocityPara param)
+        {
+            int ret = 0;
+            GTSCardAPI.TTrapPrm trapprm;
+            GTSCardAPI.GT_PrfTrap((short)IConfig.CardNum, (short)axisNum);
+            GTSCardAPI.GT_GetTrapPrm((short)IConfig.CardNum, (short)axisNum, out trapprm);
+            trapprm.smoothTime = 1;
+            if (param.Acc != 0)
+            {
+                trapprm.acc = param.Acc;
+            }
+            if (param.Dec != 0)
+            {
+                trapprm.dec = param.Dec;
+            }
+            ret += GTSCardAPI.GT_SetTrapPrm((short)IConfig.CardNum, (short)axisNum, ref trapprm);
+            ret += GTSCardAPI.GT_SetVel((short)IConfig.CardNum, (short)axisNum, param.Velocity * IConfig.AxisVelocityRatio);
+
+            if (ret != (int)APS_Define.ResultSuccess)
+            {
+                throw new Exception("杞�" + axisNum + "璁剧疆鍙傛暟寮傚父锛岄敊璇爜锛�" + ret);
+            }
+        }
+
+        /// <summary>
+        /// Set Single Axis Do Jog Move  
+        /// </summary>
+        /// <param name="axisNum">AxisNo</param>
+        /// <param name="nDirection">Motion Direction 0: Negative, 1: Positive</param>
+        /// <param name="nMaxVel">max velocity</param>
+        /// <returns></returns>
+        public bool StartJog(int axisNum, int nDirection, int velocity)
         {
             GTSCardAPI.TJogPrm jogprm = new GTSCardAPI.TJogPrm();
-            short rtn = GTSCardAPI.GT_PrfJog(cardNum, axisNum);
+            short rtn = GTSCardAPI.GT_PrfJog((short)IConfig.CardNum, (short)axisNum);
             jogprm.acc = 1;
             jogprm.dec = 1;
-            GTSCardAPI.GT_SetJogPrm(cardNum, axisNum, ref jogprm);//璁剧疆jog杩愬姩鍙傛暟
-            GTSCardAPI.GT_SetVel(cardNum, axisNum, velocity);//璁剧疆鐩爣閫熷害
-            GTSCardAPI.GT_Update(cardNum, 1 << (axisNum - 1));//鏇存柊杞磋繍鍔�
+            GTSCardAPI.GT_SetJogPrm((short)IConfig.CardNum, (short)axisNum, ref jogprm);//璁剧疆jog杩愬姩鍙傛暟
+            GTSCardAPI.GT_SetVel((short)IConfig.CardNum, (short)axisNum, velocity);//璁剧疆鐩爣閫熷害
+            int ret = GTSCardAPI.GT_Update((short)IConfig.CardNum, 1 << (axisNum - 1));//鏇存柊杞磋繍鍔�
+
+            if (ret != (int)APS_Define.ResultSuccess)
+            {
+                return false;
+            }
+            return true;
+        }
+
+        /// <summary>
+        /// Set Single Axis Do stop Jog Move  
+        /// </summary>
+        /// <param name="axisNum">AxisNo</param>
+        /// <returns></returns>
+        public bool StopJog(int axisNum)
+        {
+            MoveStop();
+            return IsStop((short)IConfig.CardNum, (short)axisNum);
+        }
+
+        /// <summary>
+        /// Set Single Axis Do Rel Move  
+        /// </summary>
+        /// <param name="axisNum">AxisNo</param>
+        /// <param name="nDistance">run distance</param>
+        /// <returns></returns>
+        public void MoveRel(int axisNum, int nDistance, int nMaxVel)
+        {
+            try
+            {
+                if (CurrentState == EnumHelper.DeviceState.DSExcept)
+                {
+                    LogAsync(DateTime.Now, "鏉垮崱寮傚父鐘舵��", "杞�" + axisNum + "璇曞浘寮傚父鐘舵�佽繍鍔�");
+                    return;
+                }
+
+                if (CurrentState != EnumHelper.DeviceState.DSOpen)
+                {
+                    return;
+                }
+
+                int currentPosition = GetPosition(axisNum);
+
+                if (OnAxisStartToCheckConfliction != null && OnAxisStartToCheckConfliction.Invoke(axisNum, currentPosition, currentPosition + nDistance))
+                {
+                    return;
+                }
+
+                int ret = GTSCardAPI.GT_SetPos((short)IConfig.CardNum, (short)axisNum, nDistance * IConfig.AxisVelocityRatio);
+
+                ret += GTSCardAPI.GT_Update((short)IConfig.CardNum, 1 << (axisNum - 1));
+                if (ret != (Int32)APS_Define.ResultSuccess)
+                {
+                    throw new Exception("杞�" + axisNum + "鍚姩鐩稿杩愬姩寮傚父锛岄敊璇爜锛�" + ret);
+                }
+
+                RunFinish(axisNum, false);
+            }
+            catch (Exception ex)
+            {
+                MoveStop();
+                OnExceptionRaised?.Invoke(ex);
+            }
+        }
+
+        /// <summary>
+        ///  Set Single Axis Do Absolute Move  
+        /// </summary>
+        /// <param name="axisNum">AxisNo</param>
+        /// <param name="nDistance">run distance</param>
+        /// <param name="nMaxVel">max velocity</param>
+        /// <returns></returns>
+        public void MoveAbs(int axisNum, int nPosition, int nMaxVel)
+        {
+            try
+            {
+            ReMove:
+                MoveAbsAsync(axisNum, nPosition);
+
+                var runFinish = Task.Run(() => RunFinish(axisNum), axisMoveCancelDict[axisNum].Token);
+                try
+                {
+                    runFinish.Wait(axisMoveCancelDict[axisNum].Token);
+                }
+                catch (OperationCanceledException ex)
+                {
+                    goto ReMove;
+                }
+
+                //// 鍒犻櫎璁板綍
+                //if (currentCommandDic.ContainsKey(axisNum))
+                //{
+                //    currentCommandDic.TryRemove(axisNum, out int[] temp);
+                //}
+            }
+            catch (Exception ex)
+            {
+                MoveStop();
+                OnExceptionRaised?.Invoke(ex);
+            }
+        }
+
+        static object moveLock = new object();
+
+        /// <summary>
+        /// 缁濆杩愬姩锛堝紓姝ワ級
+        /// </summary>
+        /// <param name="axisNum"></param>
+        /// <param name="nPosition"></param>
+        public void MoveAbsAsync(int axisNum, int nPosition)
+        {
+            try
+            {
+                lock (moveLock)
+                {
+                    axisImmediatePauseHandleDict[axisNum].WaitOne();
+                    _pauseHandle.WaitOne();
+
+                    int currentPosition = GetPosition(axisNum);
+                    if (OnAxisStartToCheckConfliction != null && OnAxisStartToCheckConfliction.Invoke(axisNum, currentPosition, nPosition))
+                    {
+                        return;
+                    }
+
+                    if (_isResetting)
+                    {
+                        LogAsync(DateTime.Now, "澶嶄綅杩囩▼寮傚父", "杞�" + axisNum + "璇曞浘鍦ㄥ浣嶈繃绋嬩腑杩愬姩");
+                        throw new Exception("杞�" + axisNum + "璇曞浘鍦ㄥ浣嶈繃绋嬩腑杩愬姩");
+                    }
+
+                    int repeatTime = 30;
+                    while (CurrentState != EnumHelper.DeviceState.DSOpen && repeatTime > 0)
+                    {
+                        Thread.Sleep(10);
+                        repeatTime--;
+                    }
+
+                    if (CurrentState == EnumHelper.DeviceState.DSExcept)
+                    {
+                        LogAsync(DateTime.Now, "鏉垮崱寮傚父鐘舵��", "杞�" + axisNum + "璇曞浘寮傚父鐘舵�佽繍鍔�");
+                        return;
+                    }
+
+                    if (CurrentState != EnumHelper.DeviceState.DSOpen)
+                    {
+                        LogAsync(DateTime.Now, "闈炴甯哥姸鎬佸紓甯�", "杞�" + axisNum + "璇曞浘鍦ㄩ潪姝e父鐘舵�佽繍鍔�");
+                        throw new Exception("杞�" + axisNum + "璇曞浘鍦ㄩ潪姝e父鐘舵�佽繍鍔�", null);
+                    }
+
+                    int ret = 0;
+                    repeatTime = 50;
+                    do
+                    {
+                        LogAsync(DateTime.Now, "杞�" + axisNum + "鍚姩杩愬姩", "鐩爣鍧愭爣锛�" + nPosition);
+                        ret = GTSCardAPI.GT_SetPos((short)IConfig.CardNum, (short)axisNum, nPosition * IConfig.AxisVelocityRatio);
+
+                        ret += GTSCardAPI.GT_Update((short)IConfig.CardNum, 1 << (axisNum - 1));
+                        if (ret != (Int32)APS_Define.ResultSuccess)
+                        {
+                            LogAsync(DateTime.Now, "杞�" + axisNum + "APS_absolute_move寮傚父", "杩斿洖鍊硷細" + ret + "锛涢噸璇曟鏁帮細" + repeatTime);
+                            Thread.Sleep(50);
+                        }
+                        repeatTime--;
+                    } while (ret != (Int32)APS_Define.ResultSuccess && repeatTime > 0);
+
+                    if (ret != (Int32)APS_Define.ResultSuccess)
+                    {
+                        LogAsync(DateTime.Now, "杞�" + axisNum + "鍚姩缁濆杩愬姩寮傚父", "閿欒鐮侊細" + ret);
+                        throw new Exception("杞�" + axisNum + "鍚姩缁濆杩愬姩寮傚父锛岄敊璇爜锛�" + ret);
+                    }
+                }
+            }
+            catch (Exception ex)
+            {
+                MoveStop();
+                OnExceptionRaised?.Invoke(ex);
+            }
+        }
+
+        Dictionary<int, AxisMovingStay> _axisStayDict = new Dictionary<int, AxisMovingStay>();
+
+        private async void MoveAbsStay(int axisNum)
+        {
+            await Task.Run(() =>
+            {
+                while (CurrentState != EnumHelper.DeviceState.DSClose)
+                {
+                    if (!_axisStayDict.ContainsKey(axisNum))
+                    {
+                        _axisStayDict[axisNum] = new AxisMovingStay();
+                    }
+
+                    _axisStayDict[axisNum].MoveHandle.WaitOne();
+                    MoveAbsAsync(axisNum, _axisStayDict[axisNum].Position);
+                    _axisStayDict[axisNum].MoveSendHandle.Set();
+                }
+            });
+        }
+
+        /// <summary>
+        /// 鍔ㄤ綔缁撴潫
+        /// </summary>
+        /// <param name="axisNum"></param>
+        /// <returns></returns>
+        public void RunFinish(int axisNum, bool isAbs = true)
+        {
+            MOTION_IO_STATUS MotionIoSts = new MOTION_IO_STATUS();
+            MOTION_BOOL_STATUS MotionSts = new MOTION_BOOL_STATUS();
+            var axisSetting = IConfig.AxisSettings.FirstOrDefault(a => a.AxisIndex == axisNum);
+            int startTime = System.Environment.TickCount;
+
+            //MDN淇″彿
+            if (axisSetting.IsUseMDNStopSignal)
+            {
+                while (GetMotionIoStatus(axisNum, ref MotionIoSts) && GetMotionStatus(axisNum, ref MotionSts))
+                {
+                    if (MotionIoSts.EMG)
+                    {
+                        LogAsync(DateTime.Now, "杞�" + axisNum + "鎬ュ仠", "");
+                        throw new Exception("杞�" + axisNum + "鎬ュ仠");
+                    }
+
+                    if (MotionSts.MDN)
+                    {
+                        int stopCode = -1;
+                        APS168.APS_get_stop_code(axisNum, ref stopCode);
+
+                        LogAsync(DateTime.Now, "杞�" + axisNum + "StopCode锛�", stopCode.ToString());
+
+                        //0 姝e父鍋滄 4 姝f瀬闄� 5 璐熸瀬闄�
+                        if (new List<int>() { 0, 4, 5 }.Contains(stopCode))
+                        {
+                            if (new List<int>() { 4, 5 }.Contains(stopCode) && axisSetting.IsUseWarning)
+                            {
+                                MoveStop(true);
+                                OnExceptionRaised?.Invoke(new Exception("杞�" + axisNum + "杩愬姩鑷虫瀬闄愪綅缃紝stopCode锛�" + stopCode.ToString(), null));
+                                return;
+                            }
+                            if (IConfig.ActionAfterDelay > 0)
+                            {
+                                Thread.Sleep(IConfig.ActionAfterDelay);
+                            }
+
+                            int feedBack = 0;
+                            GetPosition(axisNum, ref feedBack);
+                            LogAsync(DateTime.Now, "杞�" + axisNum + "杩愬姩缁撴潫", "褰撳墠浣嶇疆锛�" + feedBack);
+                            break;
+                        }
+                    }
+
+                }
+            }
+            else    //INP淇″彿
+            {
+                while (GetMotionIoStatus(axisNum, ref MotionIoSts) && GetMotionStatus(axisNum, ref MotionSts))
+                {
+                    if (MotionIoSts.EMG)
+                    {
+                        LogAsync(DateTime.Now, "杞�" + axisNum + "鎬ュ仠", "");
+                        throw new Exception("杞�" + axisNum + "鎬ュ仠");
+                    }
+
+
+                    if (MotionSts.MDN && MotionIoSts.INP)
+                    {
+                        int stopCode = -1;
+                        APS168.APS_get_stop_code(axisNum, ref stopCode);
+
+                        LogAsync(DateTime.Now, "杞�" + axisNum + "StopCode锛�", stopCode.ToString());
+
+                        //0 姝e父鍋滄 4 姝f瀬闄� 5 璐熸瀬闄�
+                        if (new List<int>() { 0, 4, 5 }.Contains(stopCode))
+                        {
+                            if (new List<int>() { 4, 5 }.Contains(stopCode) && axisSetting.IsUseWarning)
+                            {
+                                MoveStop(true);
+                                OnExceptionRaised?.Invoke(new AMPRunException("杞�" + axisNum + "杩愬姩鑷虫瀬闄愪綅缃紝stopCode锛�" + stopCode.ToString(), null));
+                                return;
+                            }
+
+                            if (IConfig.ActionAfterDelay > 0)
+                            {
+                                Thread.Sleep(IConfig.ActionAfterDelay);
+                            }
+
+                            int feedBack = 0;
+                            GetPosition(axisNum, ref feedBack);
+                            LogAsync(DateTime.Now, "杞�" + axisNum + "杩愬姩缁撴潫", "褰撳墠浣嶇疆锛�" + feedBack);
+                            break;
+                        }
+                    }
+
+                }
+            }
+        }
+
+        /// <summary>
+        /// Stop single axis move 
+        /// </summary>
+        /// <param name="axisNum">axisNo</param>
+        /// <param name="option">0琛ㄧず骞虫粦鍋滄锛�1琛ㄧず绱ф�ュ仠姝�</param>
+        /// <returns></returns>
+        public void MoveStop(int axisNum, int option)
+        {
+            int ret = GTSCardAPI.GT_Stop((short)IConfig.CardNum, 1 << (axisNum - 1), option);
+            if (ret != (Int32)APS_Define.ResultSuccess)
+            {
+                LogAsync(DateTime.Now, "杞�" + axisNum + "杩愬姩鍋滄寮傚父", "閿欒鐮侊細" + ret);
+                throw new Exception("杞�" + axisNum + "杩愬姩鍋滄寮傚父锛岄敊璇爜锛�" + ret);
+            }
+            else
+            {
+                LogAsync(DateTime.Now, "杞�" + axisNum + "杩愬姩鍋滄", "");
+            }
+        }
+
+        static object motionIOLock = new object();
+        /// <summary>
+        /// Get single Axis  Motion_Io status and motion status
+        /// </summary>
+        /// <param name="nAxis"></param>
+        /// <param name="nMotionSts"></param>
+        /// <returns></returns>
+        public bool GetMotionStatus(int nAxis, ref MOTION_BOOL_STATUS strcMotionSts)
+        {
+            lock (motionIOLock)
+            {
+                int nMotionSts = 0;
+                nMotionSts = APS168.APS_motion_status(nAxis);
+                if (nMotionSts < 0)
+                {
+                    return false;
+                }
+                strcMotionSts.ASTP = Convert.ToBoolean(nMotionSts & (1 << (int)MOTION_STATUS.ASTP));
+                strcMotionSts.HMV = Convert.ToBoolean(nMotionSts & (1 << (int)MOTION_STATUS.HMV));
+                strcMotionSts.MDN = Convert.ToBoolean(nMotionSts & (1 << (int)MOTION_STATUS.MDN));
+                strcMotionSts.DIR = Convert.ToBoolean(nMotionSts & (1 << (int)MOTION_STATUS.DIR));
+            }
+            return true;
+        }
+
+        /// <summary>
+        /// Get single Axis  Motion_Io status and motion status
+        /// </summary>
+        /// <param name="nAxis"></param>
+        /// <param name="nMotionIoSts"></param>
+        /// <returns></returns>
+        public bool GetMotionIoStatus(int nAxis, ref MOTION_IO_STATUS strcMotionIoSts)
+        {
+            lock (motionIOLock)
+            {
+                //Thread.Sleep(15);
+                int nMotionIoSts = 0;
+                nMotionIoSts = APS168.APS_motion_io_status(nAxis);
+                if (nMotionIoSts < 0)
+                {
+                    return false;
+                }
+
+                bool isAlarm = Convert.ToBoolean(nMotionIoSts & (1 << (int)MOTION_IOSTATUS.ALM));
+                if (strcMotionIoSts.ALM != isAlarm)
+                {
+                    strcMotionIoSts.ALM = isAlarm;
+                    OnMotionAlarm?.Invoke(nAxis, isAlarm);
+                }
+
+                strcMotionIoSts.PEL = Convert.ToBoolean(nMotionIoSts & (1 << (int)MOTION_IOSTATUS.PEL));
+                strcMotionIoSts.MEL = Convert.ToBoolean(nMotionIoSts & (1 << (int)MOTION_IOSTATUS.MEL));
+                strcMotionIoSts.ORG = Convert.ToBoolean(nMotionIoSts & (1 << (int)MOTION_IOSTATUS.ORG));
+                strcMotionIoSts.EMG = Convert.ToBoolean(nMotionIoSts & (1 << (int)MOTION_IOSTATUS.EMG));
+                strcMotionIoSts.INP = Convert.ToBoolean(nMotionIoSts & (1 << (int)MOTION_IOSTATUS.INP));
+                strcMotionIoSts.SVON = Convert.ToBoolean(nMotionIoSts & (1 << (int)MOTION_IOSTATUS.SVON));
+            }
+            return true;
+        }
+
+        public void MoveStop(bool emergencyStop = false)
+        {
+            int option = 0;
+            if (emergencyStop)
+            {
+                option = 1;
+                StateChange(EnumHelper.DeviceState.DSExcept);
+            }
+
+            IConfig.AxisSettings.Where(a => a.IsAxisEnabled).ToList().ForEach(axisNum =>
+            {
+                MoveStop(axisNum.AxisIndex, option);
+            });
+        }
+
+        /// <summary>
+        /// 鍥炲師鐐�
+        /// </summary>
+        /// <param name="cardn">鍗″彿</param>
+        /// <param name="axisn">杞村彿</param>
+        /// <param name="homests">杞村洖鍘熺偣鐘舵��</param>
+        public bool GoHome(short cardn, short axisn, short home_mode, short home_dir, int homeoffset)
+        {
+            try
+            {
+                GTSCardAPI.GT_ZeroPos(cardn, axisn, 1);
+                GTSCardAPI.THomePrm thomeprm;
+                GTSCardAPI.THomeStatus homests;
+                short rtn = GTSCardAPI.GT_GetHomePrm(cardn, axisn, out thomeprm);
+                thomeprm.mode = home_mode;//鍥為浂鏂瑰紡
+                thomeprm.moveDir = home_dir;//鍥為浂鏂瑰悜
+                thomeprm.edge = 0;
+                thomeprm.velHigh = 50;
+                thomeprm.velLow = 50;
+                thomeprm.acc = 50;
+                thomeprm.dec = 50;
+                thomeprm.searchHomeDistance = 9999999;//鎼滄悳璺濈
+                thomeprm.homeOffset = 0;  //鍋忕Щ璺濈
+                thomeprm.escapeStep = 1000;
+                rtn = GTSCardAPI.GT_GoHome(cardn, axisn, ref thomeprm);  //鍚姩鍥為浂
+
+                while (true)
+                {
+                    Thread.Sleep(5);
+                    GTSCardAPI.GT_GetHomeStatus(cardn, axisn, out homests);
+                    if (homests.run == 0)
+                    {
+                        if (homests.error == 0)
+                        {
+                            Thread.Sleep(200);
+                            GTSCardAPI.GT_ZeroPos(cardn, axisn, 1);
+                        }
+                        return true;
+                    }
+                }
+            }
+            catch (Exception ex)
+            {
+                MoveStop();
+                OnExceptionRaised?.Invoke(ex);
+                return false;
+            }
+        }
+
+        /// <summary>
+        /// 鍥炲師鐐�
+        /// </summary>
+        /// <param name="axisId"></param>
+        public void StartHoming(int axisId)
+        {
+            try
+            {
+                //servo on
+                APS168.APS_set_servo_on(axisId, 1);
+                Thread.Sleep(500); // Wait stable.
+
+                // 2. Start home move
+                APS168.APS_home_move(axisId);
+
+                WaitHomeFinish(axisId);
+
+                axisDestination[axisId] = 0;
+            }
+            catch (Exception ex)
+            {
+                MoveStop();
+                OnExceptionRaised?.Invoke(ex);
+            }
+        }
+
+        /// <summary>
+        /// 鍥炲師鐐�
+        /// </summary>
+        /// <param name="axisId"></param>
+        /// <param name="homeMode"></param>
+        /// <param name="homeDir"></param>
+        /// <param name="praCurve"></param>
+        /// <param name="praAcc"></param>
+        /// <param name="praVm"></param>
+        public void StartHoming(int axisId, int homeMode, int homeDir, double praCurve, double praAcc, double praVm)
+        {
+            // 1. Select home mode and config home parameters 
+            APS168.APS_set_axis_param(axisId, (Int32)APS_Define.PRA_HOME_MODE, homeMode); // Set home mode
+            APS168.APS_set_axis_param(axisId, (Int32)APS_Define.PRA_HOME_DIR, homeDir); // Set home direction
+            APS168.APS_set_axis_param_f(axisId, (Int32)APS_Define.PRA_HOME_CURVE, praCurve); // Set acceleration paten (T-curve)
+            APS168.APS_set_axis_param_f(axisId, (Int32)APS_Define.PRA_HOME_ACC, praAcc); // Set homing acceleration rate
+            APS168.APS_set_axis_param_f(axisId, (Int32)APS_Define.PRA_HOME_VM, praVm); // Set homing maximum velocity.
+            APS168.APS_set_axis_param_f(axisId, (Int32)APS_Define.PRA_HOME_VO, praVm / 5); // Set homing VO speed
+            APS168.APS_set_axis_param_f(axisId, (Int32)APS_Define.PRA_HOME_EZA, 0); // Set EZ signal alignment (yes or no)
+            APS168.APS_set_axis_param_f(axisId, (Int32)APS_Define.PRA_HOME_SHIFT, 0); // Set home position shfit distance. 
+            APS168.APS_set_axis_param_f(axisId, (Int32)APS_Define.PRA_HOME_POS, 0); // Set final home position.
+
+            StartHoming(axisId);
+        }
+
+        /// <summary>
+        /// 鍥炲師鐐圭粨鏉�
+        /// </summary>
+        /// <param name="axisNum"></param>
+        /// <returns></returns>
+        public void WaitHomeFinish(int axisNum)
+        {
+            MOTION_IO_STATUS MotionIoSts = new MOTION_IO_STATUS();
+            MOTION_BOOL_STATUS MotionSts = new MOTION_BOOL_STATUS();
+            var axisSetting = IConfig.AxisSettings.FirstOrDefault(a => a.AxisIndex == axisNum);
+            int startTime = System.Environment.TickCount;
+            while ((GetMotionIoStatus(axisNum, ref MotionIoSts)) && (GetMotionStatus(axisNum, ref MotionSts)))
+            {
+                if (MotionIoSts.EMG)
+                {
+                    LogAsync(DateTime.Now, "杞村浣嶄腑" + axisNum + "鎬ュ仠", "");
+                    throw new Exception("杞村浣嶄腑" + axisNum + "鎬ュ仠");
+                }
+
+                if (axisSetting.HomeMode == 0)
+                {
+                    if (!MotionSts.HMV && MotionIoSts.ORG)
+                        break;
+                }
+                else
+                {
+                    if (!MotionSts.HMV)
+                        break;
+                }
+
+                if (axisSetting.TimeOutHome < System.Environment.TickCount - startTime)
+                {
+                    LogAsync(DateTime.Now, "杞村浣嶄腑" + axisNum + "鍥炲師鐐硅秴鏃�", "");
+                    MoveStop(axisNum);
+                    throw new Exception("杞村浣嶄腑" + axisNum + "鍥炲師鐐硅秴鏃�");
+                }
+            }
+
+            //ClearPosition(axisNum);
+        }
+
+        static object _ioLock = new object();
+        /// <summary>
+        /// 璁剧疆IO鍗¤緭鍑�
+        /// </summary>
+        /// <param name="DINo">IO绔彛搴忓彿</param>
+        /// <param name="Value">璁剧疆鍊�(true:楂樼數骞筹紝 false:浣庣數骞�)</param>
+        /// <returns></returns>
+        public bool SetOutput(int DINo, bool Value)
+        {
+            if (OnCheckOutputAllowed?.Invoke(DINo, Value, new Dictionary<int, int>(axisDestination.ToDictionary(u => u.Key, u => u.Value))) ?? false)
+            {
+                OnExceptionRaised?.Invoke(new AMPRunException(DINo + "杈撳嚭" + Value.ToString() + "涓嶈鍏佽", null));
+                return false;
+            }
+
+            int ret = -1;
+            lock (_ioLock)
+            {
+                ret = APS168.APS_write_d_channel_output(0, 0, DINo, Value ? 1 : 0);
+            }
+            if (ret < 0)
+            {
+                return false;
+            }
+            return true;
+        }
+
+        /// <summary>
+        /// 鑾峰彇杈撳叆淇″彿
+        /// </summary>
+        /// <param name="stateType"></param>
+        /// <returns></returns>
+        public bool[] GetInputState()
+        {
+            int diVaule = 0;
+
+            lock (_ioLock)
+            {
+                APS168.APS_read_d_input(0, 0, ref diVaule);
+            }
+            return GetBoolSig(diVaule);
+            //return new bool[16];
+        }
+
+        /// <summary>
+        /// 鑾峰彇杈撳嚭淇″彿
+        /// </summary>
+        /// <param name="stateType"></param>
+        /// <returns></returns>
+        public bool[] GetOutputState()
+        {
+            int doVaule = 0;
+
+            lock (_ioLock)
+            {
+                APS168.APS_read_d_output(0, 0, ref doVaule);
+            }
+
+            return GetBoolSig(doVaule);
+
+        }
+
+        /// <summary>
+        /// 灏嗘棤绗﹀彿鏁村瀷杞崲涓築ool鍨嬫暟缁�
+        /// </summary>
+        /// <param name="sigStr"></param>
+        /// <returns></returns>
+        private bool[] GetBoolSig(int sigStr)
+        {
+            bool[] state = new bool[32];
+
+            for (int i = 0; i < 32; i++)
+            {
+                state[i] = (sigStr & (1 << i)) != 0;
+            }
+            return state;
+        }
+
+        /// <summary>
+        /// 灏哹ool鏁扮粍杞崲鎴愭棤绗﹀彿鏁村瀷
+        /// </summary>
+        /// <param name="sigArray"></param>
+        /// <returns></returns>
+        private int GetintSig(bool[] sigArray)
+        {
+            int state = -1;
+
+            for (int i = 31; i > -1; i--)
+            {
+                state = (state << 1) + (sigArray[i] ? 1 : 0);
+            }
+
+            return state;
         }
 
         /// <summary>
@@ -99,36 +1004,45 @@
         /// <summary>
         /// 杈撳嚭
         /// </summary>
-        /// <param name="cardNumo">鍗″彿</param>
+        /// <param name="cardNum">鍗″彿</param>
         /// <param name="index">杈撳嚭鍙�,杩斿洖1-16</param>
-        /// <param name="value">0琛ㄧず杈撳嚭锛�1琛ㄧず鍏抽棴</param>
-        public void WriteOut(short cardNumo, short index, bool value)
+        /// <param name="value">false琛ㄧず杈撳嚭锛宼rue琛ㄧず鍏抽棴</param>
+        public void WriteOut(short cardNum, short index, bool value)
         {
             short outNum = (short)(index % 100 + 1);
-            switch (value)
+            if (value)
             {
-                case true:
-                    {
-                        GTSCardAPI.GT_SetDoBit(cardNumo, GTSCardAPI.MC_GPO, outNum, 0);//鎸変綅杈撳嚭锛�0琛ㄧず杈撳嚭锛�1琛ㄧず鍏抽棴
-                    }
-                    break;
-                case false:
-                    {
-                        GTSCardAPI.GT_SetDoBit(cardNumo, GTSCardAPI.MC_GPO, outNum, 1);//鎸変綅杈撳嚭锛�0琛ㄧず杈撳嚭锛�1琛ㄧず鍏抽棴
-                    }
-                    break;
+                GTSCardAPI.GT_SetDoBit(cardNum, GTSCardAPI.MC_GPO, outNum, 0);
+            }
+            else
+            {
+                GTSCardAPI.GT_SetDoBit(cardNum, GTSCardAPI.MC_GPO, outNum, 1);
             }
         }
 
         /// <summary>
         /// 鍋滄 鏌愪釜杞�
         /// </summary>
-        /// <param name="cardNum"></param>
-        /// <param name="axisNum"></param>
+        /// <param name="cardNum">鍗″彿</param>
+        /// <param name="axisNum">杞村彿</param>
         /// <param name="option">0琛ㄧず骞虫粦鍋滄锛�1琛ㄧず绱ф�ュ仠姝�</param>
         public void Stop(short cardNum, short axisNum, short option)
         {
             GTSCardAPI.GT_Stop(cardNum, 1 << (axisNum - 1), option);
+        }
+
+        /// <summary>
+        /// 鍋滄 鏌愪釜杞达紙寮傛锛�
+        /// </summary>
+        /// <param name="cardNum">鍗″彿</param>
+        /// <param name="axisNum">杞村彿</param>
+        /// <param name="option">0琛ㄧず骞虫粦鍋滄锛�1琛ㄧず绱ф�ュ仠姝�</param>
+        public async Task StopAsync(short cardNum, short axisNum, short option)
+        {
+            await Task.Run(() =>
+           {
+               GTSCardAPI.GT_Stop(cardNum, 1 << (axisNum - 1), option);
+           });
         }
 
         /// <summary>
@@ -137,21 +1051,15 @@
         /// <param name="cardNum"></param>
         /// <param name="axisNum">杞村彿</param>
         /// <param name="value">鍋滄鏂瑰紡锛宖alse琛ㄧず骞虫粦鍋滄锛宼rue琛ㄧず绱ф�ュ仠姝�</param>
-        public void Stop(short cardNum, short axisNum, bool value)
+        public void Stop(short cardNum, short axisNum, bool emergencyStop)
         {
-            switch (value)
+            if (emergencyStop)
             {
-                case false:
-                    {
-                        GTSCardAPI.GT_Stop(cardNum, 1 << (axisNum - 1), 0);
-                    }
-                    break;
-                case true:
-                    {
-                        GTSCardAPI.GT_Stop(cardNum, 1 << (axisNum - 1), 1 << (axisNum - 1));
-
-                    }
-                    break;
+                GTSCardAPI.GT_Stop(cardNum, 1 << (axisNum - 1), 1 << (axisNum - 1));
+            }
+            else
+            {
+                GTSCardAPI.GT_Stop(cardNum, 1 << (axisNum - 1), 0);
             }
         }
 
@@ -190,21 +1098,21 @@
         /// <summary>
         /// 璇诲彇IO杈撳嚭鐘舵��
         /// </summary>
-        /// <param name="cardNumo"></param>
+        /// <param name="cardNum"></param>
         /// <param name="index"></param>
         /// <returns></returns>
-        public bool GetDoSts(short cardNumo, short index)
+        public bool GetDoSts(short cardNum, short index)
         {
             short outNum = 0;
             int outSts;
             outNum = (short)(index % 100);
-            GTSCardAPI.GT_GetDo(cardNumo, GTSCardAPI.MC_GPO, out outSts);
+            GTSCardAPI.GT_GetDo(cardNum, GTSCardAPI.MC_GPO, out outSts);
             if ((outSts & (1 << outNum)) == 0) return true;
             else return false;
         }
 
-        static object lockObj = new object();
-      
+
+
 
         /// <summary>
         /// 璇诲彇褰撳墠鍊�
@@ -218,7 +1126,7 @@
             {
                 double prfpos = 0; uint pclock = 0;
                 GTSCardAPI.GT_GetPrfPos(cardNum, axisNum, out prfpos, 1, out pclock);
-                return prfpos / GTSCardParameter.Dangliang;
+                return prfpos / IConfig.AxisVelocityRatio;
             }
         }
 
@@ -239,6 +1147,7 @@
                 else return false;              //杩愯涓繑鍥瀎alse
             }
         }
+        #endregion
 
         public void Monitor()
         {

--
Gitblit v1.8.0