1. GTS回原点修改，避免来回 2. 添加单键测量结果补偿 3. 部分条件限制，日志修改 4. 来料检debug，添加退出条件判断

patrick.xu

2021-08-13 2edc3c54ba7a42d5e9bb9120b9881d418e6bd408

 src/Bro.Device.GTSCard/GTSCardDriver.cs

@@ -99,6 +99,9 @@
        {
            AllAxisOn();

            MonitorPosition();
            MonitorAxisStatus();

            base.Start();
        }

@@ -309,7 +312,6 @@
            }
        }


        public override bool AllAxisOn()
        {
            List<Task<bool>> taskList = new List<Task<bool>>(); ;
@@ -384,7 +386,7 @@
                responseMessage.Result = resultList.All(u => u == true);
                if (!responseMessage.Result)
                {
                    responseMessage.Message = $"点位运动异常";
                    responseMessage.Message = $"点位运动异常,运动结果：{string.Join(" ", resultList.Select(u => u ? "1" : "0"))}";
                }
            }
            return responseMessage;
@@ -397,24 +399,28 @@
        /// <returns></returns>
        private bool SetAxisParam(MovingOption optionPara)
        {
            List<short> resultCode = new List<short>() { 0 };
            List<short> resultCode = new List<short>();
            GTSCardAPI.TTrapPrm trapprm = new GTSCardAPI.TTrapPrm();
            short axisIndex = short.Parse(optionPara.AxisIndexStr);
            resultCode.Add(GTSCardAPI.GT_PrfTrap((short)IConfig.CardNum, axisIndex));
            resultCode.Add(GTSCardAPI.GT_GetTrapPrm((short)IConfig.CardNum, axisIndex, out trapprm));
            trapprm.smoothTime = 1;

            if (optionPara.VelocityPara.Acc != 0)
            if (optionPara.VelocityPara.Acc != 0 || optionPara.VelocityPara.Dec != 0)
            {
                trapprm.acc = optionPara.VelocityPara.Acc;
            }
                resultCode.Add(GTSCardAPI.GT_GetTrapPrm((short)IConfig.CardNum, axisIndex, out trapprm));
                trapprm.smoothTime = 1;

            if (optionPara.VelocityPara.Dec != 0)
            {
                trapprm.dec = optionPara.VelocityPara.Dec;
            }
                if (optionPara.VelocityPara.Acc != 0)
                {
                    trapprm.acc = optionPara.VelocityPara.Acc;
                }

            resultCode.Add(GTSCardAPI.GT_SetTrapPrm((short)IConfig.CardNum, axisIndex, ref trapprm));
                if (optionPara.VelocityPara.Dec != 0)
                {
                    trapprm.dec = optionPara.VelocityPara.Dec;
                }

                resultCode.Add(GTSCardAPI.GT_SetTrapPrm((short)IConfig.CardNum, axisIndex, ref trapprm));
            }

            if (optionPara.VelocityPara.Velocity != 0)
            {
@@ -424,9 +430,10 @@
            var resultOK = resultCode.All(u => u == (short)GTSRetCode.GRCRunOK);
            if (!resultOK)
            {
                throw new ProcessException("轴" + optionPara.AxisIndex + "设置参数异常，错误码：" + string.Join(",", resultCode));
                //throw new ProcessException("轴" + optionPara.AxisIndex + "设置参数异常，错误码：" + string.Join(",", resultCode));
                LogAsync(DateTime.Now, $"轴{optionPara.AxisIndex}设置参数异常，错误码：{string.Join(",", resultCode)}", "");
            }
            return resultOK;
            return true;
        }

        TaskFactory taskFactory = new TaskFactory(TaskCreationOptions.LongRunning, TaskContinuationOptions.LongRunning);
@@ -523,7 +530,7 @@
        /// <returns></returns>
        public double GetPosition(int axisNum)
        {
            lock (moveLock)
            //lock (moveLock)
            {
                double position = 0;
                double pPos = 0;
@@ -643,7 +650,9 @@
                if (CurrentState != EnumHelper.DeviceState.DSOpen)
                {
                    LogAsync(DateTime.Now, "非正常状态异常", "轴" + optionPara.AxisIndex + "试图在非正常状态运动");
                    throw new ProcessException("轴" + optionPara.AxisIndex + "试图在非正常状态运动", null);

                    return false;
                    //throw new ProcessException("轴" + optionPara.AxisIndex + "试图在非正常状态运动", null);
                }

                LogAsync(DateTime.Now, "轴" + optionPara.AxisIndex + "开始运动", "目标坐标：" + optionPara.Destination);
@@ -722,8 +731,11 @@
                if (CurrentState != EnumHelper.DeviceState.DSOpen)
                {
                    LogAsync(DateTime.Now, "非正常状态异常", "轴" + optionPara.AxisIndex + "试图在非正常状态运动");
                    throw new ProcessException("轴" + optionPara.AxisIndex + "试图在非正常状态运动", null);

                    return false;
                    //throw new ProcessException("轴" + optionPara.AxisIndex + "试图在非正常状态运动", null);
                }

                LogAsync(DateTime.Now, "轴" + optionPara.AxisIndex + "开始运动", "目标坐标：" + optionPara.Destination);
                short ret = 0;
                bool isSuccessSetAxisParam = false;
@@ -757,7 +769,14 @@
                    repeatTime--;
                }

                return (ret == (short)GTSRetCode.GRCRunOK) && isStop;
                if (repeatTime <= 0)
                {
                    LogAsync(DateTime.Now, "轴" + optionPara.AxisIndex + "APS_absolute_move未停止", "");
                    return false;
                }

                //return (ret == (short)GTSRetCode.GRCRunOK) && isStop;
                return true;
            }
            catch (Exception ex)
            {
@@ -780,7 +799,8 @@
                bool isStop = false;
                if (option == 1)
                {
                    StateChange(EnumHelper.DeviceState.DSExcept);
                    //StateChange(EnumHelper.DeviceState.DSExcept);
                    LogAsync(DateTime.Now, "急停停止", "");
                }
                var ret = GTSCardAPI.GT_Stop((short)IConfig.CardNum, 1 << (axisNum - 1), option);
                if (ret != (short)GTSRetCode.GRCRunOK)
@@ -888,6 +908,8 @@
        {
            try
            {
                Thread.CurrentThread.Priority = ThreadPriority.AboveNormal;

                var goHomePara = IConfig.AxisSettings.FirstOrDefault(u => u.AxisIndex == movingOption.AxisIndex).GoHomePara;
                bool homeDirection = goHomePara.IsHomeDirPositive;
                bool isRightLimitReached = false;
@@ -904,34 +926,56 @@
                // 设置点位模式运动参数
                sRtn = GTSCardAPI.GT_SetTrapPrm((short)IConfig.CardNum, (short)movingOption.AxisIndex, ref trapPrm);
                // 设置点位模式目标速度，即回原点速度
                sRtn = GTSCardAPI.GT_SetVel((short)IConfig.CardNum, (short)movingOption.AxisIndex, goHomePara.HighVelocity);
                sRtn = GTSCardAPI.GT_SetVel((short)IConfig.CardNum, (short)movingOption.AxisIndex, goHomePara.HomeVelocity);

                do
                {
                    PositionReset(movingOption.AxisIndex, 1);
                    ClearStatus(movingOption.AxisIndex, 1);

                    //搜索距离 阶段1
                    // 启动Home捕获
                    sRtn = GTSCardAPI.GT_SetCaptureMode((short)IConfig.CardNum, (short)movingOption.AxisIndex, GTSCardAPI.CAPTURE_HOME);
                    //LogAsync(DateTime.Now, $"{Name}位置置零", "");

                    if (goHomePara.IsCaptureMode)
                    {
                        //搜索距离 阶段1
                        // 启动Home捕获
                        sRtn = GTSCardAPI.GT_SetCaptureMode((short)IConfig.CardNum, (short)movingOption.AxisIndex, GTSCardAPI.CAPTURE_HOME);
                    }

                    // 设置点位模式目标位置，即原点搜索距离
                    sRtn = GTSCardAPI.GT_SetPos((short)IConfig.CardNum, (short)movingOption.AxisIndex, homeDirection ? 99999999 : -99999999);
                    sRtn = GTSCardAPI.GT_SetPos((short)IConfig.CardNum, (short)movingOption.AxisIndex, homeDirection ? 999999999 : -999999999);
                    // 启动运动
                    sRtn = GTSCardAPI.GT_Update((short)IConfig.CardNum, 1 << (movingOption.AxisIndex - 1));

                    int repeatTime = goHomePara.GoHomeTimeOut * 1000;

                    bool isStop = false;
                    int pos;
                    int pos = 0;
                    uint clk;//时钟参数

                    int checkInterval = IConfig.MonitorInterval;
                    if (IConfig.MonitorInterval <= 0)
                    {
                        checkInterval = 10;
                    }

                    do
                    {
                        Thread.Sleep(IConfig.MonitorInterval * 5);
                        // 读取捕获状态
                        GTSCardAPI.GT_GetCaptureStatus((short)IConfig.CardNum, (short)movingOption.AxisIndex, out capture, out pos, 1, out clk);
                        Thread.Sleep(checkInterval);

                        if (goHomePara.IsCaptureMode)
                        {
                            // 读取捕获状态
                            GTSCardAPI.GT_GetCaptureStatus((short)IConfig.CardNum, (short)movingOption.AxisIndex, out capture, out pos, 1, out clk);
                        }
                        else
                        {
                            GTSCardAPI.GT_GetDi((short)IConfig.CardNum, GTSCardAPI.MC_HOME, out int pValue);
                            capture = (short)((pValue & (1 << (movingOption.AxisIndex - 1))) == 0 ? 1 : 0);
                            //LogAsync(DateTime.Now, $"原点状态{pValue}，轴{movingOption.AxisIndex}原点{capture}", "");
                        }
                        isStop = IsStop((short)movingOption.AxisIndex);
                        repeatTime -= IConfig.MonitorInterval * 5;
                        repeatTime -= checkInterval;
                    } while (!(isStop || capture == 1 || repeatTime <= 0));

                    if (repeatTime <= 0)
@@ -942,25 +986,40 @@

                    var axisStatus = AxisStatusList.FirstOrDefault(u => u.AxisIndex == movingOption.AxisIndex);

                    //if (isStop)
                    //{
                    //    LogAsync(DateTime.Now, $"轴{movingOption.AxisIndex}复位中停止", $"Capture状态{capture}");
                    //}

                    if (isStop && capture != 1)
                    {
                        if (((axisStatus.AxisStatus & 0x20) != 0) || ((axisStatus.AxisStatus & 0x40) != 0))
                        {
                            //movingOption.GoHomePara.HomeDir = (short)(movingOption.GoHomePara.HomeDir == 1 ? -1 : 1);
                            capture = 0;

                            //正限位
                            if ((axisStatus.AxisStatus & 0x20) != 0 && !goHomePara.IsCaptureDirPositive)
                            if ((axisStatus.AxisStatus & 0x20) != 0)
                            {
                                isRightLimitReached = true;
                                homeDirection = false;

                                if (!goHomePara.IsCaptureDirPositive)
                                {
                                    isRightLimitReached = true;
                                }
                            }

                            //负限位
                            if ((axisStatus.AxisStatus & 0x40) != 0 && goHomePara.IsCaptureDirPositive)
                            if ((axisStatus.AxisStatus & 0x40) != 0)
                            {
                                isRightLimitReached = true;
                                homeDirection = true;

                                if (goHomePara.IsCaptureDirPositive)
                                {
                                    isRightLimitReached = true;
                                }
                            }

                            homeDirection = !homeDirection;
                            LogAsync(DateTime.Now, $"轴{movingOption.AxisIndex}极限位置换向", "");
                        }

                        ClearStatus(movingOption.AxisIndex, 1);
@@ -995,17 +1054,25 @@
                            repeatTime--;
                        } while (!isStop && repeatTime > 0);
                        PositionReset(movingOption.AxisIndex, 1);
                        LogAsync(DateTime.Now, $"轴{movingOption.AxisIndex}复位完成，位置清零", "");

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

                } while (!(capture == 1 && isRightLimitReached));

                return false;
            }
            catch (Exception ex)
            {
                LogAsync(DateTime.Now, $"{Name}回原点异常", ex.GetExceptionMessage());
                AllMoveStop(true);
                OnExceptionOccured?.Invoke(DateTime.Now, ex);
                return false;
            }
            finally
            { 
            
            }
        }

@@ -1076,7 +1143,7 @@
        /// <returns></returns>
        public override int GetAxisStatus(int axisNum)
        {
            lock (moveLock)
            //lock (moveLock)
            {
                int sts = 0;
                uint pclock = 0;
@@ -1099,7 +1166,7 @@
        /// <returns></returns>
        public override bool PositionReset(int startAxisIndex, int count)
        {
            lock (moveLock)
            //lock (moveLock)
            {
                var rtn = GTSCardAPI.GT_ZeroPos((short)IConfig.CardNum, (short)startAxisIndex, (short)count);
                return rtn == (short)GTSRetCode.GRCRunOK;
@@ -1130,7 +1197,6 @@

            return axisMovingStatusesList;
        }


        public List<IOItem> GetMonitorValues()
        {
@@ -1164,6 +1230,82 @@
            return result;
        }

        SpinWait _positionWait = new SpinWait();
        private async void MonitorPosition()
        {
            await Task.Run(() =>
            {
                while (CurrentState != EnumHelper.DeviceState.DSClose && CurrentState != EnumHelper.DeviceState.DSExcept && CurrentState != EnumHelper.DeviceState.DSUninit)
                {
                    try
                    {
                        if (!IConfig.IsEnableMonitor)
                            return;

                        AxisStatusList.ForEach(a =>
                        {
                            int curPosition = (int)GetPrfPosition(a.AxisIndex);

                            if (a.CurPosition != curPosition)
                            {
                                a.CurPosition = curPosition;
                                AxisPositionChanged(a.AxisIndex, curPosition);
                            }
                        });

                        Task.Delay(100).Wait();

                        //_positionWait.SpinOnce();
                    }
                    catch (Exception ex)
                    {
                        if (CurrentState == DeviceState.DSOpen)
                        {
                            LogAsync(DateTime.Now, $"{this.Name}监听轴信息异常", ex.GetExceptionMessage());
                        }
                    }
                }
            });
        }

        SpinWait _statusWait = new SpinWait();
        private async void MonitorAxisStatus()
        {
            await Task.Run(() =>
            {
                while (CurrentState != EnumHelper.DeviceState.DSClose && CurrentState != EnumHelper.DeviceState.DSExcept && CurrentState != EnumHelper.DeviceState.DSUninit)
                {
                    try
                    {
                        if (!IConfig.IsEnableMonitor)
                            return;

                        AxisStatusList.ForEach(a =>
                        {
                            int curStatus = GetAxisStatus(a.AxisIndex);

                            if (a.AxisStatus != curStatus)
                            {
                                int temp = a.AxisStatus;
                                a.AxisStatus = curStatus;
                                AxisStatusChanged(a.AxisIndex, temp, curStatus);
                            }
                        });

                        Thread.Sleep(10);
                        //Task.Delay(10).Wait();
                    }
                    catch (Exception ex)
                    {
                        if (CurrentState == DeviceState.DSOpen)
                        {
                            LogAsync(DateTime.Now, $"{this.Name}监听轴信息异常", ex.GetExceptionMessage());
                        }
                    }
                }
            });
        }

        public async override void Monitor()
        {
            await Task.Run(() =>
@@ -1176,9 +1318,9 @@
                            return;

                        var newValues = GetMonitorValues();
                        var newAxisMovingStatus = GetAxisMovingStatus();
                        //var newAxisMovingStatus = GetAxisMovingStatus();

                        if (newValues == null || newValues.Count == 0 || newAxisMovingStatus == null || newAxisMovingStatus.Count == 0)
                        if (newValues == null || newValues.Count == 0)
                            continue;

                        //Stopwatch sw = new Stopwatch();
@@ -1192,14 +1334,14 @@
                            MonitorCheckAndInvoke(tempNew, tempOld);
                        }

                        if (AxisStatusList.Count == newAxisMovingStatus.Count)
                        {
                            var tempNew = new List<AxisMovingStatus>(newAxisMovingStatus);//clone
                            var tempOld = new List<AxisMovingStatus>(AxisStatusList);
                            AxisStatusCheck(tempNew, tempOld);
                        }
                        //if (AxisStatusList.Count == newAxisMovingStatus.Count)
                        //{
                        //    var tempNew = new List<AxisMovingStatus>(newAxisMovingStatus);//clone
                        //    var tempOld = new List<AxisMovingStatus>(AxisStatusList);
                        //    AxisStatusCheck(tempNew, tempOld);
                        //}

                        AxisStatusList = new List<AxisMovingStatus>(newAxisMovingStatus);
                        //AxisStatusList = new List<AxisMovingStatus>(newAxisMovingStatus);
                        MonitorValues = new List<IOItem>(newValues);
                        //sw.Stop();