Merge branch 'master' of http://gitblit.broconcentric.com:8088/r/M071

wells.liu

2020-07-11 eb4b6e7a257aaae8f65f770928a7d9b317347a39

 src/Bro.Device.GTSCard/GTSCardDriver.cs

@@ -96,6 +96,8 @@
        protected override void Start()
        {
            AllAxisOn();

            base.Start();
        }

        protected override void Stop()
@@ -267,7 +269,7 @@
            ClearStatus(1, IConfig.AxisSettings.FindAll(u => u.IsAxisEnabled).Count);
            if (res != (short)GTSRetCode.GRCRunOK)
            {
                throw new Exception("板卡载入配置文件异常，错误码：" + res);
                throw new ProcessException("板卡载入配置文件异常，错误码：" + res);
            }
        }

@@ -341,8 +343,8 @@
                {
                    var task = SingleAxisMoving(movingOp);
                    taskList.Add(task);
                    task.Start();
                }

                Task.WaitAll(taskList.ToArray());
                responseMessage.Result = taskList.All(u => u.GetAwaiter().GetResult());
                if (!responseMessage.Result)
@@ -375,74 +377,76 @@
            var resultOK = resultCode.All(u => u == (short)GTSRetCode.GRCRunOK);
            if (!resultOK)
            {
                throw new Exception("轴" + optionPara.AxisIndex + "设置参数异常，错误码：" + string.Join(",", resultCode));
                throw new ProcessException("轴" + optionPara.AxisIndex + "设置参数异常，错误码：" + string.Join(",", resultCode));
            }
            return resultOK;
        }

        TaskFactory taskFactory = new TaskFactory(TaskCreationOptions.LongRunning, TaskContinuationOptions.LongRunning);
        /// <summary>
        /// 单个轴 运动(点到点 jog 回零...）
        /// </summary>
        /// <param name="optionPara">运动参数对象</param>
        public override async Task<bool> SingleAxisMoving(MovingOption optionPara)
        public override Task<bool> SingleAxisMoving(MovingOption optionPara)
        {
            return await Task.Run(() =>
            {
                axisImmediatePauseHandleDict[optionPara.AxisIndex].WaitOne();
                bool isSuccessAndStop = false;
                try
                {
                    if (IConfig.AxisSettings.FirstOrDefault(a => a.AxisIndex == optionPara.AxisIndex)?.IsAxisEnabled ?? false)
                    {
                        string _position = "";
                        string motionType = optionPara.MoveMode == EnumHelper.MotionMode.Normal ? (optionPara.IsAbsolute ? "Abs" : "Rel") : optionPara.MoveMode.ToString();
            return new Task<bool>(() =>
             {
                 axisImmediatePauseHandleDict[optionPara.AxisIndex].WaitOne();
                 bool isSuccessAndStop = false;
                 try
                 {
                     if (IConfig.AxisSettings.FirstOrDefault(a => a.AxisIndex == optionPara.AxisIndex)?.IsAxisEnabled ?? false)
                     {
                         string _position = "";
                         string motionType = optionPara.MoveMode == EnumHelper.MotionMode.Normal ? (optionPara.IsAbsolute ? "Abs" : "Rel") : optionPara.MoveMode.ToString();

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

                        switch (optionPara.MoveMode)
                        {
                            case MotionMode.Normal:
                                {
                                    if (_isResetting)
                                    {
                                        LogAsync(DateTime.Now, "复位中启动运动异常", optionPara.AxisIndex + "启动运动异常");
                                        return false;
                                    }
                         switch (optionPara.MoveMode)
                         {
                             case MotionMode.Normal:
                                 {
                                     if (_isResetting)
                                     {
                                         LogAsync(DateTime.Now, "复位中启动运动异常", optionPara.AxisIndex + "启动运动异常");
                                         return false;
                                     }

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

                                }
                                break;
                            case MotionMode.FindOri:
                                {
                                    isSuccessAndStop = GoHome(optionPara);
                                }
                                break;
                            case MotionMode.Jog:
                                {
                                    isSuccessAndStop = JogMove(optionPara);
                                }
                                break;
                        }
                        _position += $"{GetPosition(optionPara.AxisIndex)},";
                        _position += $"{DateTime.Now:yyyy-MM-dd HH:mm:ss.fff}";
                        LogAsync(DateTime.Now, "", _position);
                    }
                }
                catch (Exception ex)
                {
                    isSuccessAndStop = false;
                    LogAsync(DateTime.Now, $"轴{optionPara.AxisIndex}运动异常", ex.GetExceptionMessage());
                }
                return isSuccessAndStop;
            });
                                 }
                                 break;
                             case MotionMode.FindOri:
                                 {
                                     //isSuccessAndStop = GoHome(optionPara);  
                                     isSuccessAndStop = P2PGoHome(optionPara);
                                 }
                                 break;
                             case MotionMode.Jog:
                                 {
                                     isSuccessAndStop = JogMove(optionPara);
                                 }
                                 break;
                         }
                         _position += $"{GetPosition(optionPara.AxisIndex)},";
                         _position += $"{DateTime.Now:yyyy-MM-dd HH:mm:ss.fff}";
                         LogAsync(DateTime.Now, "", _position);
                     }
                 }
                 catch (Exception ex)
                 {
                     isSuccessAndStop = false;
                     LogAsync(DateTime.Now, $"轴{optionPara.AxisIndex}运动异常", ex.GetExceptionMessage());
                 }
                 return isSuccessAndStop;
             });
        }

        /// <summary>
@@ -457,10 +461,10 @@
            var ret = GTSCardAPI.GT_GetPrfPos((short)IConfig.CardNum, (short)axisNum, out prfpos, 1, out pclock);
            if (ret != (short)GTSRetCode.GRCRunOK)
            {
                throw new Exception("轴" + axisNum + "获取规划位置异常，错误码：" + ret);
                throw new ProcessException("轴" + axisNum + "获取规划位置异常，错误码：" + ret);
            }
            var AxisRatio = IConfig.AxisSettings.FirstOrDefault(u => u.AxisIndex == axisNum) == null ? 1 : IConfig.AxisSettings.FirstOrDefault(u => u.AxisIndex == axisNum).AxisRatio;
            position = prfpos / AxisRatio;
            //var AxisRatio = IConfig.AxisSettings.FirstOrDefault(u => u.AxisIndex == axisNum) == null ? 1 : IConfig.AxisSettings.FirstOrDefault(u => u.AxisIndex == axisNum).AxisRatio;
            position = prfpos;
            return position;
        }

@@ -474,14 +478,14 @@
            lock (moveLock)
            {
                double position = 0;
                int pPos = 0;
                var ret = GTSCardAPI.GT_GetPos((short)IConfig.CardNum, (short)axisNum, out pPos);
                double pPos = 0;
                var ret = GTSCardAPI.GT_GetPrfPos((short)IConfig.CardNum, (short)axisNum, out pPos, 1, out uint pclock);
                if (ret != (short)GTSRetCode.GRCRunOK)
                {
                    throw new Exception("轴" + axisNum + "获取目标位置异常，错误码：" + ret);
                    throw new ProcessException("轴" + axisNum + "获取目标位置异常，错误码：" + ret);
                }
                var AxisRatio = IConfig.AxisSettings.FirstOrDefault(u => u.AxisIndex == axisNum) == null ? 1 : IConfig.AxisSettings.FirstOrDefault(u => u.AxisIndex == axisNum).AxisRatio;
                position = pPos / AxisRatio;
                //var AxisRatio = IConfig.AxisSettings.FirstOrDefault(u => u.AxisIndex == axisNum) == null ? 1 : IConfig.AxisSettings.FirstOrDefault(u => u.AxisIndex == axisNum).AxisRatio;
                position = pPos;
                return position;
            }
        }
@@ -498,7 +502,7 @@
            var ret = GTSCardAPI.GT_GetPrfVel((short)IConfig.CardNum, (short)axisNum, out prfVel, 1, out pclock);
            if (ret != (short)GTSRetCode.GRCRunOK)
            {
                throw new Exception("轴" + axisNum + "获取规划速度异常，错误码：" + ret);
                throw new ProcessException("轴" + axisNum + "获取规划速度异常，错误码：" + ret);
            }
            return prfVel;
        }
@@ -514,7 +518,7 @@
            var ret = GTSCardAPI.GT_GetVel((short)IConfig.CardNum, (short)axisNum, out vel);
            if (ret != (short)GTSRetCode.GRCRunOK)
            {
                throw new Exception("轴" + axisNum + "获取当前速度异常，错误码：" + ret);
                throw new ProcessException("轴" + axisNum + "获取当前速度异常，错误码：" + ret);
            }
            return vel;
        }
@@ -572,7 +576,7 @@
                if (_isResetting)
                {
                    LogAsync(DateTime.Now, "复位过程异常", "轴" + optionPara.AxisIndex + "试图在复位过程中运动");
                    throw new Exception("轴" + optionPara.AxisIndex + "试图在复位过程中运动");
                    throw new ProcessException("轴" + optionPara.AxisIndex + "试图在复位过程中运动");
                }

                int repeatTime = 30;
@@ -591,7 +595,7 @@
                if (CurrentState != EnumHelper.DeviceState.DSOpen)
                {
                    LogAsync(DateTime.Now, "非正常状态异常", "轴" + optionPara.AxisIndex + "试图在非正常状态运动");
                    throw new Exception("轴" + optionPara.AxisIndex + "试图在非正常状态运动", null);
                    throw new ProcessException("轴" + optionPara.AxisIndex + "试图在非正常状态运动", null);
                }

                LogAsync(DateTime.Now, "轴" + optionPara.AxisIndex + "开始运动", "目标坐标：" + optionPara.Destination);
@@ -605,7 +609,7 @@
                {
                    //设置 运动参数
                    isSuccessSetAxisParam = SetAxisParam(optionPara);
                    ret = GTSCardAPI.GT_SetPrfPos((short)IConfig.CardNum, (short)optionPara.AxisIndex, (int)(dPosition * AxisRatio));// 设置规划位置
                    ret = GTSCardAPI.GT_SetPos((short)IConfig.CardNum, (short)optionPara.AxisIndex, (int)(dPosition));// 设置目的位置
                    ret = GTSCardAPI.GT_Update((short)IConfig.CardNum, 1 << (optionPara.AxisIndex - 1));//更新运动

                    if (ret != (short)GTSRetCode.GRCRunOK)
@@ -625,8 +629,8 @@
                repeatTime = 1000;
                do
                {
                    isStop = IsStop((short)optionPara.AxisIndex);
                    Thread.Sleep(50);
                    isStop = IsStop((short)optionPara.AxisIndex);
                    repeatTime--;
                } while (!isStop && repeatTime > 0);

@@ -653,7 +657,7 @@
                if (_isResetting)
                {
                    LogAsync(DateTime.Now, "复位过程异常", "轴" + optionPara.AxisIndex + "试图在复位过程中运动");
                    throw new Exception("轴" + optionPara.AxisIndex + "试图在复位过程中运动");
                    throw new ProcessException("轴" + optionPara.AxisIndex + "试图在复位过程中运动");
                }
                int repeatTime = 30;
                while (CurrentState != EnumHelper.DeviceState.DSOpen && repeatTime > 0)
@@ -670,7 +674,7 @@
                if (CurrentState != EnumHelper.DeviceState.DSOpen)
                {
                    LogAsync(DateTime.Now, "非正常状态异常", "轴" + optionPara.AxisIndex + "试图在非正常状态运动");
                    throw new Exception("轴" + optionPara.AxisIndex + "试图在非正常状态运动", null);
                    throw new ProcessException("轴" + optionPara.AxisIndex + "试图在非正常状态运动", null);
                }
                LogAsync(DateTime.Now, "轴" + optionPara.AxisIndex + "开始运动", "目标坐标：" + optionPara.Destination);
                short ret = 0;
@@ -681,7 +685,7 @@
                {
                    //设置 运动参数
                    isSuccessSetAxisParam = SetAxisParam(optionPara);
                    ret = GTSCardAPI.GT_SetPrfPos((short)IConfig.CardNum, (short)optionPara.AxisIndex, (int)(optionPara.Destination * AxisRatio));// 设置规划位置
                    ret = GTSCardAPI.GT_SetPos((short)IConfig.CardNum, (short)optionPara.AxisIndex, (int)(optionPara.Destination));// 设置目标位置
                    ret = GTSCardAPI.GT_Update((short)IConfig.CardNum, 1 << (optionPara.AxisIndex - 1));//更新运动

                    if (ret != (short)GTSRetCode.GRCRunOK)
@@ -700,8 +704,8 @@
                repeatTime = 1000;
                while (!isStop && repeatTime > 0)
                {
                    isStop = IsStop((short)optionPara.AxisIndex);
                    Thread.Sleep(50);
                    isStop = IsStop((short)optionPara.AxisIndex);
                    repeatTime--;
                }

@@ -734,7 +738,7 @@
                if (ret != (short)GTSRetCode.GRCRunOK)
                {
                    LogAsync(DateTime.Now, "轴" + axisNum + "运动停止异常", "错误码：" + ret);
                    throw new Exception("轴" + axisNum + "运动停止异常，错误码：" + ret);
                    throw new ProcessException("轴" + axisNum + "运动停止异常，错误码：" + ret);
                }
                else
                {
@@ -743,8 +747,8 @@
                int repeatTime = 100;
                do
                {
                    isStop = IsStop((short)axisNum);
                    Thread.Sleep(10);
                    isStop = IsStop((short)axisNum);
                    repeatTime--;
                } while (!isStop && repeatTime > 0);

@@ -783,7 +787,13 @@
                PositionReset(movingOption.AxisIndex, 1);
                GTSCardAPI.THomePrm thomeprm;
                GTSCardAPI.THomeStatus homests;
                short rtn = GTSCardAPI.GT_GetHomePrm((short)IConfig.CardNum, (short)movingOption.AxisIndex, out thomeprm);
                // 启动Home捕获
                short rtn = GTSCardAPI.GT_SetCaptureMode((short)IConfig.CardNum, (short)movingOption.AxisIndex, GTSCardAPI.CAPTURE_HOME);
                // 切换到点位运动模式
                rtn = GTSCardAPI.GT_PrfTrap((short)IConfig.CardNum, (short)movingOption.AxisIndex);
                // 读取点位模式运动参数
                rtn = GTSCardAPI.GT_GetHomePrm((short)IConfig.CardNum, (short)movingOption.AxisIndex, out thomeprm);

                thomeprm.mode = movingOption.GoHomePara.HomeMode;//回零方式
                thomeprm.moveDir = movingOption.GoHomePara.HomeDir;//回零方向
                thomeprm.edge = movingOption.GoHomePara.Edge;
@@ -791,7 +801,7 @@
                thomeprm.velLow = movingOption.GoHomePara.LowVelocity;
                thomeprm.acc = movingOption.VelocityPara.Acc;
                thomeprm.dec = movingOption.VelocityPara.Dec;
                thomeprm.searchHomeDistance = movingOption.GoHomePara.SearchHomeDistance;//搜搜距离
                thomeprm.searchHomeDistance = movingOption.GoHomePara.SearchHomeDistance;//搜索距离
                thomeprm.homeOffset = movingOption.GoHomePara.HomeOffset;  //偏移距离
                thomeprm.escapeStep = movingOption.GoHomePara.EscapeStep;
                rtn = GTSCardAPI.GT_GoHome((short)IConfig.CardNum, (short)movingOption.AxisIndex, ref thomeprm);  //启动回零
@@ -813,6 +823,93 @@
                } while (!isStop && repeatTime > 0);

                return isStop;
            }
            catch (Exception ex)
            {
                AllMoveStop(true);
                OnExceptionOccured?.Invoke(DateTime.Now, ex);
                return false;
            }
        }

        /// <summary>
        /// P2P方式回原点
        /// </summary>
        /// <param name="movingOption">运动参数</param>
        public bool P2PGoHome(MovingOption movingOption)
        {
            try
            {
                PositionReset(movingOption.AxisIndex, 1);
                ClearStatus(movingOption.AxisIndex, 1);
                GTSCardAPI.TTrapPrm trapPrm;
                bool isStop = false;
                //搜索距离 阶段1
                // 启动Home捕获
                short sRtn = GTSCardAPI.GT_SetCaptureMode((short)IConfig.CardNum, (short)movingOption.AxisIndex, GTSCardAPI.CAPTURE_HOME);
                // 切换到点位运动模式
                sRtn = GTSCardAPI.GT_PrfTrap((short)IConfig.CardNum, (short)movingOption.AxisIndex);
                // 读取点位模式运动参数
                sRtn = GTSCardAPI.GT_GetTrapPrm((short)IConfig.CardNum, (short)movingOption.AxisIndex, out trapPrm);
                trapPrm.acc = movingOption.VelocityPara.Acc;
                trapPrm.dec = movingOption.VelocityPara.Dec;
                // 设置点位模式运动参数
                sRtn = GTSCardAPI.GT_SetTrapPrm((short)IConfig.CardNum, (short)movingOption.AxisIndex, ref trapPrm);
                // 设置点位模式目标速度，即回原点速度
                sRtn = GTSCardAPI.GT_SetVel((short)IConfig.CardNum, (short)movingOption.AxisIndex, movingOption.GoHomePara.HighVelocity);
                // 设置点位模式目标位置，即原点搜索距离
                sRtn = GTSCardAPI.GT_SetPos((short)IConfig.CardNum, (short)movingOption.AxisIndex, movingOption.GoHomePara.SearchHomeDistance);
                // 启动运动
                sRtn = GTSCardAPI.GT_Update((short)IConfig.CardNum, 1 << (movingOption.AxisIndex - 1));

                int repeatTime = 1000;
                short capture;
                int pos;
                uint clk;//时钟参数
                do
                {
                    Thread.Sleep(20);
                    // 读取捕获状态
                    GTSCardAPI.GT_GetCaptureStatus((short)IConfig.CardNum, (short)movingOption.AxisIndex, out capture, out pos, 1, out clk);
                    isStop = IsStop((short)movingOption.AxisIndex);
                    repeatTime--;
                } while (!(isStop || capture == 1 || repeatTime <= 0));

                if (repeatTime <= 0)
                {
                    MoveStop((short)movingOption.AxisIndex, 0);
                    throw new ProcessException($"运动轴{movingOption.AxisIndex} ，回原点超时异常");
                }
                if (isStop && capture != 1)
                {

                    ClearStatus((short)movingOption.AxisIndex, 1);
                    throw new ProcessException($"运动轴{movingOption.AxisIndex} ，回原点超时异常");
                }

                if (capture == 1)
                {
                    //先stop
                    MoveStop((short)movingOption.AxisIndex, 0);
                    ClearStatus((short)movingOption.AxisIndex, 1);
                    //已经捕获到Home才可以回零 阶段2
                    // 运动到"捕获位置+偏移量"
                    sRtn = GTSCardAPI.GT_SetPos((short)IConfig.CardNum, (short)movingOption.AxisIndex, pos + movingOption.GoHomePara.HomeOffset);
                    // 在运动状态下更新目标位置
                    sRtn = GTSCardAPI.GT_Update((short)IConfig.CardNum, 1 << (movingOption.AxisIndex - 1));
                    isStop = false;

                    repeatTime = 1000;
                    do
                    {
                        Thread.Sleep(20);
                        isStop = IsStop((short)movingOption.AxisIndex);
                        repeatTime--;
                    } while (!isStop && repeatTime > 0);
                    PositionReset(movingOption.AxisIndex, 1);
                    return (sRtn == (short)GTSRetCode.GRCRunOK) && isStop;
                }
                return false;
            }
            catch (Exception ex)
            {
@@ -854,10 +951,10 @@
        /// 按位设置数字 IO 输出状态
        /// </summary>
        /// <param name="index">输出口,返回1-16</param>
        /// <param name="value">false表示输出，true表示关闭</param>
        /// <param name="value">false表示关，true表示开，板卡要设置取反</param>
        public override void WriteOutput(short index, IOValue value)
        {
            short outNum = (short)(index % 100);
            short outNum = (short)(index % 100 + 1);
            if ((int)value <= 1)
            {
                GTSCardAPI.GT_SetDoBit((short)IConfig.CardNum, GTSCardAPI.MC_GPO, outNum, (short)value);
@@ -931,10 +1028,10 @@
                AxisMovingStatus axisMovingStatus = new AxisMovingStatus();
                axisMovingStatus.AxisIndex = axisConfig.AxisIndex;
                axisMovingStatus.AxisName = axisConfig.AxisName;
                axisMovingStatus.CurPosition = Convert.ToInt32(GetPosition(axisMovingStatus.AxisIndex));
                //axisMovingStatus.CurPosition = Convert.ToInt32(GetPosition(axisMovingStatus.AxisIndex));
                axisMovingStatus.Destination = Convert.ToInt32(GetPrfPosition(axisMovingStatus.AxisIndex));
                axisMovingStatus.CurVelocity = GetVelocity(axisMovingStatus.AxisIndex);
                axisMovingStatus.PrfVelocity = GetPrfVelocity(axisMovingStatus.AxisIndex);
                //axisMovingStatus.CurVelocity = GetVelocity(axisMovingStatus.AxisIndex);
                //axisMovingStatus.PrfVelocity = GetPrfVelocity(axisMovingStatus.AxisIndex);
                axisMovingStatus.AxisStatus = GetAxisStatus(axisMovingStatus.AxisIndex);

                axisMovingStatusesList.Add(axisMovingStatus);
@@ -956,7 +1053,7 @@
            GTSCardAPI.GT_GetDo((short)IConfig.CardNum, GTSCardAPI.MC_GPO, out outValue);

            //解析结果
            for (var index = 1; index <= 16; index++)
            for (var index = 0; index < 16; index++)
            {
                IOItem inItem = new IOItem()
                {