机器世界_轴回原点

上代码

 class BackToOrigin//返回原点
    {
        short Rtn;
        short Axis;
        int Search_Home = -200000,Home_Offset=-2000;
        gts.mc.TTrapPrm trap;//定义点位运动结构体变量

        public void StartOriginCapture() 
        {
            short Pcapture;
            int Psts, Pos;
            uint Pclock;
            double PrfPos, EncPos;

            Rtn = gts.mc.GT_SetCaptureMode(Axis,gts.mc.CAPTURE_HOME);//启用Home捕获
            HaveError.GetError("Home捕获启用失败！","原点消息",Rtn);
            Rtn = gts.mc.GT_PrfTrap(Axis);
            HaveError.GetError("切换点位运动模式失败！", "原点消息", Rtn);
            Rtn = gts.mc.GT_GetTrapPrm(Axis, out trap);
            HaveError.GetError("获取点位运动参数失败！", "原点消息", Rtn);

            trap.acc = 0.25;//加速度
            trap.dec = 0.25;//减速度

            Rtn=gts.mc.GT_SetTrapPrm(Axis, ref trap);//设置点位运动参数
            HaveError.GetError("设置点位运动参数失败！", "原点消息", Rtn);
            Rtn = gts.mc.GT_SetVel(Axis, 10);
            HaveError.GetError("设置点位运动速度失败！", "原点消息", Rtn);
            Rtn = gts.mc.GT_SetPos(Axis,Search_Home);
            HaveError.GetError("设置目标位置失败！", "原点消息", Rtn);
            Rtn = gts.mc.GT_Update((1<<(Axis-1)));
            HaveError.GetError("启动运动失败！", "原点消息", Rtn);

            do
            {
                Rtn = gts.mc.GT_GetSts(Axis, out Psts, 1, out Pclock);//读取轴状态
                Rtn = gts.mc.GT_GetCaptureStatus(Axis, out Pcapture, out Pos, 1, out Pclock);//读取捕获状态
                Rtn = gts.mc.GT_GetPrfPos(Axis, out PrfPos, 1, out Pclock);//读取规划位置
                Rtn = gts.mc.GT_GetEncPos(Axis, out EncPos, 1, out Pclock);//读取编码器位置
                if (0 == (Psts & 0x400))//如果运动停止，报错
                {
                    HaveError.GetError("回原点意外停止！", "原点消息", 1);
                    return;
                }
            } while (0 == Pcapture);

            Rtn = gts.mc.GT_SetPos(Axis, Pos + Home_Offset);//原点补偿
            HaveError.GetError("原点补偿失败！", "原点消息", Rtn);
            Rtn = gts.mc.GT_Update(1 << (Axis - 1));
            HaveError.GetError("更新失败！", "原点消息", Rtn);

            do
            {
                Rtn=gts.mc.GT_GetSts(Axis,out Psts,1,out Pclock);
                Rtn=gts.mc.GT_GetPrfPos(Axis,out PrfPos,1,out Pclock);
                Rtn=gts.mc.GT_GetEncPos(Axis,out EncPos,1,out Pclock);
            }while((Psts&0x400)!=0);

            if (PrfPos != Pos + Home_Offset) 
            {
                HaveError.GetError("原点位置偏差！", "原点消息", 1);
                return;
            }

            HaveError.GetError("回原点成功！", "原点消息", 1);
            Rtn = gts.mc.GT_ZeroPos(Axis,1);//设置机械原点
            HaveError.GetError("设置机械原点！", "原点消息", 1);
            Rtn = gts.mc.GT_AxisOff(Axis);//关闭伺服
            HaveError.GetError("关闭伺服失败！", "原点消息", 1);

        }  
    }