固高驱动

wells.liu

2020-06-30 9e1247ca8890d904b0ebf0ee1565839e32166ec3

 src/Bro.Device.GTSCard/GTSCardDriver.cs

@@ -33,29 +33,6 @@
            }
        }

        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 moveLock = new object();
        /// <summary>
        /// 是否复位标志
@@ -79,7 +56,6 @@
            InitialMotionCard();
        }


        protected override void Pause()
        {
            throw new NotImplementedException();
@@ -92,12 +68,13 @@

        protected override void Start()
        {
            throw new NotImplementedException();
            AllAxisOn();
        }

        protected override void Stop()
        {
            throw new NotImplementedException();
            AllMoveStop();
            AllAxisOff();
        }
        #endregion

@@ -119,6 +96,60 @@
            }
        }

        public bool AllAxisOn()
        {
            List<Task<bool>> taskList = new List<Task<bool>>(); ;
            // 如果是多个轴的运动 等每个轴开启
            IConfig.AxisSettings.Where(a => a.IsAxisEnabled).ToList().ForEach(axisNum =>
            {
                var task = AxisOnAsync((short)IConfig.CardNum, (short)axisNum.AxisIndex);
                taskList.Add(task);
            });
            Task.WaitAll(taskList.ToArray());
            var resultOK = taskList.All(u => u.GetAwaiter().GetResult());
            return resultOK;
        }

        public bool AllAxisOff()
        {
            List<Task<bool>> taskList = new List<Task<bool>>(); ;
            // 如果是多个轴的运动 等每个轴关闭
            IConfig.AxisSettings.Where(a => a.IsAxisEnabled).ToList().ForEach(axisNum =>
            {
                var task = AxisOffAsync((short)IConfig.CardNum, (short)axisNum.AxisIndex);
                taskList.Add(task);
            });
            Task.WaitAll(taskList.ToArray());
            var resultOK = taskList.All(u => u.GetAwaiter().GetResult());
            return resultOK;
        }

        /// <summary>
        /// 单个轴开启
        /// </summary>
        /// <returns></returns>
        public async Task<bool> AxisOnAsync(short cardNum, short axisNum)
        {
            return await Task.Run(() =>
            {
                var ret = GTSCardAPI.GT_AxisOn(cardNum, axisNum);
                return ret == (short)GTSRetCode.GRCRunOK;
            });
        }

        /// <summary>
        /// 单个轴关闭
        /// </summary>
        /// <returns></returns>
        public async Task<bool> AxisOffAsync(short cardNum, short axisNum)
        {
            return await Task.Run(() =>
            {
                var ret = GTSCardAPI.GT_AxisOff(cardNum, axisNum);
                return ret == (short)GTSRetCode.GRCRunOK;
            });
        }

        /// <summary>
        /// 点位到点位运动
        /// </summary>
@@ -128,22 +159,33 @@
        {
            bool resultOK = false;
            var gtsOperationConfig = opConfig as GTSCardOperationConfig;
            List<Task<bool>> taskList = new List<Task<bool>>();
            //TaskFactory factory = new TaskFactory(TaskCreationOptions.LongRunning, TaskContinuationOptions.None);
            // 如果是多个轴的运动 等每个轴运动结束
            foreach (var movingOp in gtsOperationConfig.MovingOps)
            {
                resultOK = SingleAxisMoving(movingOp);
                //var task = factory.StartNew<bool>((op) =>
                //{
                //    return SingleAxisMoving(op as MovingOption);
                //}, movingOp);
                var task = SingleAxisMoving(movingOp);
                taskList.Add(task);
            }
            Task.WaitAll(taskList.ToArray());
            resultOK = taskList.All(u => u.GetAwaiter().GetResult());

            return resultOK;
        }

        /// <summary>
        /// Set AxisParam
        /// 点到点运动设置参数
        /// </summary>
        /// <param name="optionPara">运动参数对象</param>
        /// <returns></returns>
        private bool SetAxisParam(MovingOption optionPara)
        {
            List<short> resultCode = new List<short>() { 0 };
            GTSCardAPI.TTrapPrm trapprm;
            GTSCardAPI.TTrapPrm trapprm = new GTSCardAPI.TTrapPrm();
            resultCode.Add(GTSCardAPI.GT_PrfTrap((short)IConfig.CardNum, (short)optionPara.AxisIndex));
            resultCode.Add(GTSCardAPI.GT_GetTrapPrm((short)IConfig.CardNum, (short)optionPara.AxisIndex, out trapprm));
            trapprm.smoothTime = 1;
@@ -162,54 +204,59 @@
        }

        /// <summary>
        /// 单个轴 点位到点位运动
        /// 单个轴 运动(点到点 jog 回零...）
        /// </summary>
        /// <param name="optionPara">运动参数对象</param>
        public bool SingleAxisMoving(MovingOption optionPara)
        public async Task<bool> SingleAxisMoving(MovingOption optionPara)
        {
            bool isSuccessAndStop = false;
            if (IConfig.AxisSettings.FirstOrDefault(a => a.AxisIndex == optionPara.AxisIndex)?.IsAxisEnabled ?? false)
            return await Task.Run(() =>
            {
                string _position = "";
                string motionType = optionPara.MoveMode == EnumHelper.MotorMoveMode.Normal ? (optionPara.IsAbsolute ? "Abs" : "Rel") : optionPara.MoveMode.ToString();

                _position = $"{DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss.fff")},{optionPara.AxisIndex},{motionType},{GetPosition(optionPara.AxisIndex).ToString()},{GetPrfPosition(optionPara.AxisIndex).ToString()},{optionPara.Destination},";

                switch (optionPara.MoveMode)
                bool isSuccessAndStop = false;
                if (IConfig.AxisSettings.FirstOrDefault(a => a.AxisIndex == optionPara.AxisIndex)?.IsAxisEnabled ?? false)
                {
                    case MotorMoveMode.Normal:
                        {
                            if (_isResetting)
                    string _position = "";
                    string motionType = optionPara.MoveMode == EnumHelper.MotorMoveMode.Normal ? (optionPara.IsAbsolute ? "Abs" : "Rel") : optionPara.MoveMode.ToString();

                    _position = $"{DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss.fff")},{optionPara.AxisIndex},{motionType},{GetPosition(optionPara.AxisIndex).ToString()},{GetPrfPosition(optionPara.AxisIndex).ToString()},{optionPara.Destination},";

                    switch (optionPara.MoveMode)
                    {
                        case MotorMoveMode.Normal:
                            {
                                LogAsync(DateTime.Now, "复位中启动运动异常", optionPara.AxisIndex + "启动运动异常");
                                return false;
                            }
                            //设置 运动参数
                            var isSuccess = SetAxisParam(optionPara);
                            if (isSuccess)
                            {
                                if (_isResetting)
                                {
                                    LogAsync(DateTime.Now, "复位中启动运动异常", optionPara.AxisIndex + "启动运动异常");
                                    return false;
                                }

                                if (optionPara.IsAbsolute)
                                {
                                    isSuccessAndStop = MoveAbs(optionPara);
                                    isSuccessAndStop = P2PMoveAbs(optionPara);
                                }
                                else
                                {
                                    isSuccessAndStop = MoveRel(optionPara);
                                    isSuccessAndStop = P2PMoveRel(optionPara);
                                }

                            }
                        }
                        break;
                    case MotorMoveMode.FindOri:
                        {
                            isSuccessAndStop = GoHome(optionPara);
                        }
                        break;
                            break;
                        case MotorMoveMode.FindOri:
                            {
                                isSuccessAndStop = GoHome(optionPara);
                            }
                            break;
                        case MotorMoveMode.Jog:
                            {
                                isSuccessAndStop = JogMove(optionPara);
                            }
                            break;
                    }
                    _position += $"{GetPosition(optionPara.AxisIndex)},";
                    _position += $"{DateTime.Now:yyyy-MM-dd HH:mm:ss.fff}";
                    LogAsync(DateTime.Now, "", _position);
                }
                _position += $"{GetPosition(optionPara.AxisIndex)},";
                _position += $"{DateTime.Now:yyyy-MM-dd HH:mm:ss.fff}";
                LogAsync(DateTime.Now, "", _position);
            }
            return isSuccessAndStop;
                return isSuccessAndStop;
            });
        }

        /// <summary>
@@ -262,21 +309,38 @@
        /// <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)
        public bool JogMove(MovingOption optionPara)
        {
            GTSCardAPI.TJogPrm jogprm = new GTSCardAPI.TJogPrm();
            short rtn = GTSCardAPI.GT_PrfJog((short)IConfig.CardNum, (short)axisNum);
            jogprm.acc = 1;
            jogprm.dec = 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)GTSRetCode.GRCRunOK)
            try
            {
                GTSCardAPI.TJogPrm jogprm = new GTSCardAPI.TJogPrm();
                short ret = 0;
                int repeatTime = 100;
                do
                {
                    ret = GTSCardAPI.GT_PrfJog((short)IConfig.CardNum, (short)optionPara.AxisIndex);
                    jogprm.acc = optionPara.VelocityPara.Acc;
                    jogprm.dec = optionPara.VelocityPara.Dec;
                    ret = GTSCardAPI.GT_SetJogPrm((short)IConfig.CardNum, (short)optionPara.AxisIndex, ref jogprm);//设置jog运动参数
                    ret = GTSCardAPI.GT_SetVel((short)IConfig.CardNum, (short)optionPara.AxisIndex, optionPara.VelocityPara.Velocity);//设置目标速度
                    ret = GTSCardAPI.GT_Update((short)IConfig.CardNum, 1 << (optionPara.AxisIndex - 1));//更新轴运动

                    if (ret != (short)GTSRetCode.GRCRunOK)
                    {
                        LogAsync(DateTime.Now, "轴" + optionPara.AxisIndex + "APS_absolute_move异常", "错误码：" + ret + ";" + "重试次数：" + repeatTime);
                        Thread.Sleep(50);
                    }
                    repeatTime--;
                } while (ret != (short)GTSRetCode.GRCRunOK && repeatTime > 0);
                return (ret == (short)GTSRetCode.GRCRunOK);
            }
            catch (Exception ex)
            {
                AllMoveStop(true);
                OnExceptionRaised?.Invoke(ex);
                return false;
            }
            return true;

        }

        /// <summary>
@@ -307,7 +371,7 @@
        /// <param name="axisNum">AxisNo</param>
        /// <param name="nDistance">run distance</param>
        /// <returns></returns>
        public bool MoveRel(MovingOption optionPara)
        public bool P2PMoveRel(MovingOption optionPara)
        {
            try
            {
@@ -338,11 +402,14 @@

                LogAsync(DateTime.Now, "轴" + optionPara.AxisIndex + "开始运动", "目标坐标：" + optionPara.Destination);
                short ret = 0;
                repeatTime = 1000;
                repeatTime = 100;
                bool isSuccessSetAxisParam = false;
                int currentPosition = (int)GetPosition(optionPara.AxisIndex);
                int dPosition = optionPara.Destination + currentPosition;
                do
                {
                    //设置 运动参数
                    isSuccessSetAxisParam = SetAxisParam(optionPara);
                    ret = GTSCardAPI.GT_SetPrfPos((short)IConfig.CardNum, (short)optionPara.AxisIndex, (int)(dPosition * IConfig.AxisVelocityRatio));// 设置规划位置
                    ret = GTSCardAPI.GT_Update((short)IConfig.CardNum, 1 << (optionPara.AxisIndex - 1));//更新运动

@@ -352,7 +419,7 @@
                        Thread.Sleep(50);
                    }
                    repeatTime--;
                } while (ret != (short)GTSRetCode.GRCRunOK && repeatTime > 0);
                } while (ret != (short)GTSRetCode.GRCRunOK && !isSuccessSetAxisParam && repeatTime > 0);

                //运动开始后 检查运动是否停止
                bool isStop = false;
@@ -378,7 +445,7 @@
        ///  绝对位置运动
        /// </summary>
        /// <param name="optionPara">运动参数对象</param>
        public bool MoveAbs(MovingOption optionPara)
        public bool P2PMoveAbs(MovingOption optionPara)
        {
            try
            {
@@ -406,9 +473,12 @@
                }
                LogAsync(DateTime.Now, "轴" + optionPara.AxisIndex + "开始运动", "目标坐标：" + optionPara.Destination);
                short ret = 0;
                repeatTime = 1000;
                repeatTime = 100;
                bool isSuccessSetAxisParam = false;
                do
                {
                    //设置 运动参数
                    isSuccessSetAxisParam = SetAxisParam(optionPara);
                    ret = GTSCardAPI.GT_SetPrfPos((short)IConfig.CardNum, (short)optionPara.AxisIndex, (int)(optionPara.Destination * IConfig.AxisVelocityRatio));// 设置规划位置
                    ret = GTSCardAPI.GT_Update((short)IConfig.CardNum, 1 << (optionPara.AxisIndex - 1));//更新运动

@@ -418,7 +488,7 @@
                        Thread.Sleep(50);
                    }
                    repeatTime--;
                } while (ret != (short)GTSRetCode.GRCRunOK && repeatTime > 0);
                } while (ret != (short)GTSRetCode.GRCRunOK && !isSuccessSetAxisParam && repeatTime > 0);

                bool isStop = false;
                repeatTime = 1000;
@@ -445,36 +515,53 @@
        /// <param name="axisNum">axisNo</param>
        /// <param name="option">0表示平滑停止，1表示紧急停止</param>
        /// <returns></returns>
        public void MoveStop(int axisNum, int option)
        public async Task<bool> MoveStop(int axisNum, int option)
        {
            if (option == 1)
            return await Task.Run(() =>
            {
                StateChange(EnumHelper.DeviceState.DSExcept);
            }
            var ret = GTSCardAPI.GT_Stop((short)IConfig.CardNum, 1 << (axisNum - 1), option);
            if (ret != (short)GTSRetCode.GRCRunOK)
            {
                LogAsync(DateTime.Now, "轴" + axisNum + "运动停止异常", "错误码：" + ret);
                throw new Exception("轴" + axisNum + "运动停止异常，错误码：" + ret);
            }
            else
            {
                LogAsync(DateTime.Now, "轴" + axisNum + "运动停止", "");
            }
                bool isStop = false;
                if (option == 1)
                {
                    StateChange(EnumHelper.DeviceState.DSExcept);
                }
                var ret = GTSCardAPI.GT_Stop((short)IConfig.CardNum, 1 << (axisNum - 1), option);
                if (ret != (short)GTSRetCode.GRCRunOK)
                {
                    LogAsync(DateTime.Now, "轴" + axisNum + "运动停止异常", "错误码：" + ret);
                    throw new Exception("轴" + axisNum + "运动停止异常，错误码：" + ret);
                }
                else
                {
                    LogAsync(DateTime.Now, "轴" + axisNum + "运动停止", "");
                }
                int repeatTime = 100;
                do
                {
                    isStop = IsStop((short)IConfig.CardNum, (short)axisNum);
                    Thread.Sleep(10);
                    repeatTime--;
                } while (!isStop && repeatTime > 0);

                return (ret == (short)GTSRetCode.GRCRunOK) && isStop;
            });
        }

        /// <summary>
        /// 所有开启的轴关闭
        /// 所有开启的轴停止
        /// </summary>
        /// <param name="emergencyStop"></param>
        public void AllMoveStop(bool emergencyStop = false)
        {
            int option = emergencyStop ? 1 : 0;

            List<Task<bool>> taskList = new List<Task<bool>>(); ;
            // 如果是多个轴的运动 等每个轴运动结束
            IConfig.AxisSettings.Where(a => a.IsAxisEnabled).ToList().ForEach(axisNum =>
            {
                MoveStop(axisNum.AxisIndex, option);
                var task = MoveStop(axisNum.AxisIndex, option);
                taskList.Add(task);
            });
            Task.WaitAll(taskList.ToArray());
            var resultOK = taskList.All(u => u.GetAwaiter().GetResult());
        }

        /// <summary>
@@ -642,9 +729,45 @@

        public void ResetAlarm()
        {
            throw new NotImplementedException();
        }
            int axis_sts;
            uint clk;
            var axisSettings = IConfig.AxisSettings.FindAll(u => u.IsAxisEnabled);
            GTSCardAPI.GT_ClrSts((short)IConfig.CardNum, 1, (short)axisSettings.Count);
            foreach (var axisSetting in axisSettings)
            {
                GTSCardAPI.GT_GetSts((short)IConfig.CardNum, (short)axisSetting.AxisIndex, out axis_sts, 1, out clk);
                if ((axis_sts & 0x200) == 0)
                {
                    var rst = GTSCardAPI.GT_AxisOn((short)IConfig.CardNum, (short)axisSetting.AxisIndex);
                }
                // 位置请零
                GTSCardAPI.GT_ZeroPos((short)IConfig.CardNum, 1, (short)axisSettings.Count);
                // 正极限报警
                if ((axis_sts & 0x20) != 0)
                {
                    // 负向移动
                    MovingOption movingOption = new MovingOption();
                    movingOption.AxisIndex = (short)axisSetting.AxisIndex;
                    movingOption.Destination = -50; // 负向移动
                    movingOption.VelocityPara.Velocity = 50;
                    P2PMoveAbs(movingOption);
                }

                // 负极限报警
                if ((axis_sts & 0x40) != 0)
                {
                    // 正向移动
                    MovingOption movingOption = new MovingOption();
                    movingOption.AxisIndex = (short)axisSetting.AxisIndex;
                    movingOption.Destination = 50; // 负向移动
                    movingOption.VelocityPara.Velocity = 50;
                    P2PMoveAbs(movingOption);
                }
            }

            // 清除状态
            GTSCardAPI.GT_ClrSts((short)IConfig.CardNum, 1, (short)IConfig.AxisSettings.FindAll(u => u.IsAxisEnabled).Count);
        }

    }
}