回原点

wells.liu

2020-07-11 8477176a329c0530c4873f57f401a2a22b002760

 src/Bro.Device.GTSCard/GTSCardDriver.cs

@@ -425,7 +425,8 @@
                                 break;
                             case MotionMode.FindOri:
                                 {
                                     isSuccessAndStop = GoHome(optionPara);
                                     //isSuccessAndStop = GoHome(optionPara);  
                                     isSuccessAndStop = P2PGoHome(optionPara);
                                 }
                                 break;
                             case MotionMode.Jog:
@@ -463,7 +464,7 @@
                throw new Exception("轴" + axisNum + "获取规划位置异常，错误码：" + ret);
            }
            //var AxisRatio = IConfig.AxisSettings.FirstOrDefault(u => u.AxisIndex == axisNum) == null ? 1 : IConfig.AxisSettings.FirstOrDefault(u => u.AxisIndex == axisNum).AxisRatio;
            position = prfpos ;
            position = prfpos;
            return position;
        }

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

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

@@ -746,8 +747,8 @@
                int repeatTime = 100;
                do
                {
                    isStop = IsStop((short)axisNum);
                    Thread.Sleep(10);
                    isStop = IsStop((short)axisNum);
                    repeatTime--;
                } while (!isStop && repeatTime > 0);

@@ -786,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;
@@ -794,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);  //启动回零
@@ -826,6 +833,79 @@
        }

        /// <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;
                do
                {
                    Thread.Sleep(20);
                    isStop = IsStop((short)movingOption.AxisIndex);
                    repeatTime--;
                } while (!isStop && repeatTime > 0);

                if (isStop)
                {
                    short capture;
                    int pos;
                    uint clk;//时钟参数
                    // 读取捕获状态
                    GTSCardAPI.GT_GetCaptureStatus((short)IConfig.CardNum, (short)movingOption.AxisIndex, out capture, out pos, 1, out clk);
                    //已经捕获到Home才可以回零 阶段2
                    if (capture == 1)
                    {
                        // 运动到"捕获位置+偏移量"
                        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);
                    return (sRtn == (short)GTSRetCode.GRCRunOK) && isStop;
                }
                return false;
            }
            catch (Exception ex)
            {
                AllMoveStop(true);
                OnExceptionOccured?.Invoke(DateTime.Now, ex);
                return false;
            }
        }

        /// <summary>
        /// 读取IO输入
        /// </summary>
        /// <param name="cardNum">卡号</param>