非標框架（6）-軸類

using System;
using System.Threading;
using LaserWelder.Base;
using LaserWelder.Util;

namespace LaserWelder.Motion
{
    /// <summary>
    /// 軸
    /// </summary>
    public partial class Axis : EntityBase<AxisKey>
    {
        /// <summary>
        /// 基於軸參數的軸構造函數
        /// </summary>
        /// <param name="prm"></param>
        public Axis(AxisPrm prm)
        {
            Prm = prm;
            Key = prm.Key;
            switch (Prm.Type)
            {
                case AxisType.Reverse:
                    ReverseAxis = AxisMgr.Instance.FindBy(Prm.ReverseAxisKey);
                    break;
                case AxisType.Cylinder_TwoPos:
                    break;
            }
            if (Prm.Type == AxisType.Common || Prm.Type == AxisType.Master || Prm.Type == AxisType.Reverse)
            {
                Card.Executer.SetScale(prm.Code, prm.PulseEquiv, Card.Key);
            }
        }
        /// <summary>
        /// 參數
        /// </summary>
        public AxisPrm Prm { get; set; }
        /// <summary>
        /// 軸需要用到的控制卡
        /// </summary>
        public Card Card
        {
            get
            {
                return CardMgr.Instance[Prm.CardNum];
            }
        }
        /// <summary>
        /// Reverse軸所關聯的其它軸
        /// </summary>
        public Axis ReverseAxis { get; private set; }
        private bool isHomed;
        public bool IsHomed
        {
            get
            {
                if (Prm.Type == AxisType.Reverse)
                {
                    return ReverseAxis.IsHomed;
                }
                return isHomed;
            }
        }

        /// <summary>
        /// 等待軸使能成功
        /// </summary>
        /// <param name="maxDelay">最大等待時間/ms</param>
        /// <param name="code">軸編號</param>
        private bool waitAxisOn(int maxDelay, short code)
        {
            Result rtn;
            bool isServo;
            int num = maxDelay / 5; //允許的最大循環次數
            int i = 0; //已循環的次數
            while (true)
            {
                rtn = Card.Executer.GetIsServo(code, out isServo, Card.Key);
                if (!rtn.IsOk)
                {
                    return false;
                }
                else if (isServo)
                {
                    return true;
                }
                if (i > num)
                    break;
                i++;
                Thread.Sleep(3);
            }
            return false;
        }
        private bool isServo(short code)
        {
            bool isServo;
            Card.Executer.GetIsServo(code, out isServo, Card.Key);
            return isServo;
        }
        public bool Alarm
        {
            get
            {
                bool isAlarm;
                Result rst;
                if (Prm.Type == AxisType.Master)
                {
                    rst = Card.Executer.GetIsAlarm(Prm.FollowAxisCode, out isAlarm, Card.Key);
                    if (!rst.IsOk || !isAlarm)
                    {
                        return false;
                    }
                }
                rst = Card.Executer.GetIsAlarm(Prm.Code, out isAlarm, Card.Key);
                if (!rst.IsOk || !isAlarm)
                {
                    return false;
                }
                else
                {
                    return true;
                }
            }
            set
            {
                if (!value)
                {
                    if (Prm.Type == AxisType.Master)
                    {
                        Card.Executer.ClearAlarm(Prm.FollowAxisCode, Card.Key);
                    }
                    Card.Executer.ClearAlarm(Prm.Code, Card.Key);
                }
            }
        }
        /// <summary>
        /// 獲得絕對座標
        /// </summary>
        public double Position
        {
            get
            {
                switch (Prm.Type)
                {
                    case AxisType.Reverse:
                        return ReverseAxis.Position * (-1);
                    case AxisType.Cylinder_TwoPos:
                        return 0;
                }
                double pos = 0;
                int enc, ele;
                Card.Executer.GetEncPos(Prm.Code, out pos, Card.Key);
                Card.Executer.GetGear(Prm.Code, out enc, out ele, Card.Key);
                return pos * ele / enc / Prm.PulseEquiv;
            }
        }
        public bool PosHardLimit
        {
            get
            {
                int status;
                Card.Executer.GetLmtsSts(Prm.Code, Prm.PosLimitNum, out status, Card.Key);
                if (status == 0)
                {
                    return false;
                }
                else
                {
                    return true;
                }
            }
        }
        public bool NegHardLimit
        {
            get
            {
                int status;
                Card.Executer.GetLmtsSts(Prm.Code, Prm.NegLimitNum, out status, Card.Key);
                if (status == 0)
                {
                    return false;
                }
                else
                {
                    return true;
                }
            }
        }
        public Result SetSoftLimitOn()
        {
            //Prm.IsSoftLimit = true;
            return Card.Executer.SetSoftLimit(Prm.Code, 1, Prm.SoftPosLimit, Prm.SoftNegLimit, Card.Key);
        }
        public Result SetSoftLimitOff()
        {
            //Prm.IsSoftLimit = false;
            return Card.Executer.SetSoftLimit(Prm.Code, 0, Prm.SoftPosLimit, Prm.SoftNegLimit, Card.Key);
        }
        public Result GetSoftLimit(out int enable, out int positive, out int negative)
        {
            int mode;
            return Card.Executer.GetSoftLimit(Prm.Code, out enable, out positive, out negative, out mode, Card.Key);
        }
        /// <summary>
        /// 設置或獲得上電狀態
        /// </summary>
        public bool Servo
        {
            get
            {
                switch (Prm.Type)
                {
                    case AxisType.Common:
                    case AxisType.Master:
                        bool isServo;
                        Card.Executer.GetIsServo(Prm.Code, out isServo, Card.Key);
                        return isServo;
                    case AxisType.Reverse:
                        return ReverseAxis.Servo;
                }
                return true;
            }
            set
            {
                switch (Prm.Type)
                {
                    case AxisType.Common:
                    case AxisType.Master:
                        if (value) //上電
                        {
                            switch (Prm.Type)
                            {
                                case AxisType.Common:
                                    Card.Executer.AxisOn(Prm.Code, Card.Key);
                                    waitAxisOn(500, Prm.Code);
                                    break;
                                case AxisType.Master:

                                    break;
                            }
                            Thread.Sleep(200);
                        }
                        else //斷電
                        {
                            switch (Prm.Type)
                            {
                                case AxisType.Common:
                                    Card.Executer.AxisOff(Prm.Code, Card.Key);
                                    break;
                                case AxisType.Master:
                                    Card.Executer.SetFollowAxis(0, Prm.FollowAxisCode, Card.Key);
                                    Thread.Sleep(500);
                                    Card.Executer.AxisOff(Prm.Code, Card.Key);
                                    Card.Executer.AxisOff(Prm.FollowAxisCode, Card.Key);
                                    break;
                            }
                        }
                        break;
                    case AxisType.Reverse:
                        ReverseAxis.Servo = value;
                        break;
                }
            }
        }
        /// <summary>
        /// 清零
        /// </summary>
        /// <returns></returns>
        public Result Zero()
        {
            switch (Prm.Type)
            {
                case AxisType.Common:
                case AxisType.Master:
                    return Card.Zero(Prm.Code);
                case AxisType.Reverse:
                    return ReverseAxis.Zero();
            }
            return Result.OK;
        }
        /// <summary>
        /// 回原點，不阻塞不清零
        /// </summary>
        /// <returns></returns>
        public Result GoHome()
        {
            Result rtn = Result.OK;
            switch (Prm.Type)
            {
                case AxisType.Common:
                    int enc, ele;
                    Card.Executer.GetGear(Prm.Code, out enc, out ele, Card.Key);
                    rtn = Card.GoHome
                      (Prm.Code, Prm.HomeMode, Prm.HomeVel * enc / ele * Prm.PulseEquiv, Prm.HomeAcc * enc / ele * Prm.PulseEquiv,
                      (int)(Prm.HomeOffset * enc / ele * Prm.PulseEquiv), Prm.HomeLowVel * enc / ele * Prm.PulseEquiv);
                    break;
                case AxisType.Master:
                case AxisType.Reverse:
                    rtn = ReverseAxis.GoHome();
                    break;
                case AxisType.Cylinder_TwoPos:
                    //rtn = ioAxisHome();
                    break;
            }
            if (rtn.IsOk)
            {
                isHomed = true;
            }
            else
            {
                DebugMsg.SendMsg($"{Prm.Key}回原點失敗, {rtn.ErrCode} + {rtn.ErrMsg}");
            }
            return rtn;
        }
        /// <summary>
        /// 回原點並阻塞，清零
        /// </summary>
        /// <returns></returns>
        public Result GoHomeAndReply(bool isHomeOffset = false)
        {
            switch (Prm.Type)
            {
                case AxisType.Common:
                case AxisType.Master:
                    var rtn = GoHome();
                    if (!rtn.IsOk)
                    {
                        return rtn;
                    }
                    Thread.Sleep(50);
                    WaitAxisHomeEndAndZero(isHomeOffset);
                    return rtn;
                case AxisType.Reverse:
                    return ReverseAxis.GoHomeAndReply();
                case AxisType.Cylinder_TwoPos:
                    var ta = GoHome();
                    Thread.Sleep(200);
                    return ta;
            }
            isHomed = true;
            //SetPosSoftLimit(1, Prm.SoftPosLimit, Prm.SoftNegLimit);
            return Result.OK;
        }
        /// <summary>
        /// 等待軸回原點結束並清零
        /// </summary>
        /// <param name="axis"></param>
        /// <param name="delay">最大等待回原點時間ms</param>
        /// <returns></returns>
        public void WaitAxisHomeEndAndZero(bool isHomeOffset = false)
        {
            WaitAxisHomeEnd();
            var rtn = Card.Executer.ZeroPos(Prm.Code, Card.Key);
            rtn += Card.Executer.ZeroBoardPos(Prm.Code, Card.Key);
            rtn += Card.Executer.SetBoardMode(Prm.Code, 0); //默認正向
            if (Prm.IsSoftLimit)
            {
                rtn += SetSoftLimitOn();
            }
            else
            {
                rtn += SetSoftLimitOff();
            }
            isHomed = true;
            return;
        }
        /// <summary>
        /// 等待軸回原點結束
        /// </summary>
        public void WaitAxisHomeEnd()
        {
            WaitAxisHomeEnd(Prm.Code);
        }
        private void WaitAxisHomeEnd(short axisCode)
        {
            bool isMove;
            //正在回原點中
            while (Card.Executer.GetIsHoming(axisCode, Card.Key))
            {
                //如果已經停止回原點
                Card.Executer.GetIsMove(axisCode, out isMove, Card.Key);
                if (isMove)
                {
                    return;
                }
                Thread.Sleep(10);
            }
            //等待停穩
            Thread.Sleep(300);
            isHomed = true;
        }
        /// <summary>
        /// 移動到某座標並阻塞，默認爲絕對座標
        /// </summary>
        /// <param name="pos"></param>
        /// <returns></returns>
        public Result MovAndReply(double pos, bool isAbs = true, double percent = 100)
        {
            Mov(pos, isAbs, percent);
            var rtn = WaitUnMove();
            if (rtn.IsOk)
            {
                Thread.Sleep(10);
                return WaitArrived();
            }
            return rtn;
        }
        public Result MovByJogVelAndReply(double pos, bool isAbs = true)
        {
            //switch (Prm.Type)
            //{
            //    case AxisType.Reverse:
            //        return ReverseAxis.MovByJogVelAndReply((-1) * pos, isAbs);
            //    case AxisType.Cylinder_TwoPos:
            //        return ioAxisMovAndReply(pos, isAbs);
            //}
            MovByJogVel(pos, isAbs);
            WaitUnMove();
            Thread.Sleep(10);
            return WaitArrived();
        }
        public Result Mov(double pos, bool isAbs = true, double percent = 100)
        {     //if (IsTrigSoftLimit(pos))
            //{
            //    return new Result(false, null, $"{Prm.Code}Axis has trigger the SoftLimit");
            //}
            if (percent > 120)
            {
                percent = 100;
            }
            double scale = percent / 100.0;
            Result rtn;
            if (isAbs)
            {
                rtn = Card.Executer.AbsMov(Prm.Code, pos, Prm.DefAcc * scale, Prm.DefDec * scale, Prm.DefVel * scale,
                    Prm.DefStartVel * scale, Prm.SmoothTime, Card.Key);
            }
            else
            {
                rtn = Card.Executer.RelMov(Prm.Code, pos, Prm.DefAcc * scale, Prm.DefDec * scale, Prm.DefVel * scale,
                    Prm.DefStartVel * scale, Prm.SmoothTime, Card.Key);
            }
            return rtn;
        }
        public Result MovByJogVel(double pos, bool isAbs = true)
        {
            Result rtn;
            if (isAbs)
            {
                rtn = Card.Executer.AbsMov(Prm.Code, pos, Prm.DefJogAcc, Prm.DefJogDec, Prm.DefJogVel,
                    Prm.DefJogStartVel, Prm.SmoothTime, Card.Key);
            }
            else
            {
                rtn = Card.Executer.RelMov(Prm.Code, pos, Prm.DefJogAcc, Prm.DefJogDec, Prm.DefJogVel,
                    Prm.DefJogStartVel, Prm.SmoothTime, Card.Key);
            }
            return rtn;
        }
        /// <summary>
        /// 等待軸運動到位
        /// </summary>
        /// <returns></returns>
        public Result WaitArrived()
        {
            if (Prm.Type == AxisType.Cylinder_TwoPos || Prm.Type == AxisType.Reverse)
            {
                return Result.OK;
            }
            Result rtn = Result.FAILED;
            bool isArrive = false;
            while (!isArrive)
            {
                rtn = Card.Executer.GetIsArrive(Prm.Code, out isArrive, Card.Key);
                if (!rtn.IsOk)
                {
                    return rtn;
                }
                Thread.Sleep(2);
            }
            return rtn;
        }
        /// <summary>
        /// 等待軸停止運動
        /// </summary>
        /// <returns></returns>
        public Result WaitUnMove()
        {
            switch (Prm.Type)
            {
                case AxisType.Common:
                case AxisType.Master:
                    Result rtn = Result.FAILED;
                    bool isMove = true;
                    while (isMove)
                    {
                        rtn = Card.Executer.GetIsMove(Prm.Code, out isMove, Card.Key);
                        if (!rtn.IsOk)
                        {
                            return rtn;
                        }
                        Thread.Sleep(2);
                    }
                    return rtn;
                case AxisType.Reverse:
                    return ReverseAxis.WaitUnMove();
            }
            return Result.OK;
        }
        public Result IsMove(out bool isMove)
        {
            return Card.Executer.GetIsMove(Prm.Code, out isMove, Card.Key);
        }
        /// <summary>
        /// Jog移動，速度爲正則爲正方向，負則爲負方向
        /// </summary>
        /// <param name="vel"></param>
        /// <returns></returns>
        public Result JogMove(double vel)
        {
            if (vel > -0.0001 && vel < 0.0001)
            {
                return Result.OK;
            }
 
            bool isPos;
            if (vel > 0)
            {
                isPos = true;
            }
            else
            {
                isPos = false;
            }
            return Card.Executer.JogMov(Prm.Code, isPos, Prm.DefJogAcc,
                Prm.DefJogDec, Math.Abs(vel), Prm.DefJogStartVel, Prm.SmoothTime, Card.Key);       
        }
        /// <summary>
        /// 以默認速度Jog移動，isPos = true 時正向運動
        /// </summary>
        /// <param name="isPos"></param>
        /// <returns></returns>
        public Result JogMove(bool isPos, double percent = 1)
        {       
            return Card.Executer.JogMov(Prm.Code, isPos, Prm.DefJogAcc,
                Prm.DefJogDec, Prm.DefJogVel * percent, Prm.DefJogStartVel, Prm.SmoothTime, Card.Key);

        }
        /// <summary>
        /// 停止
        /// </summary>
        /// <param name="isAbout">是否急停</param>
        /// <returns></returns>
        public Result Stop(bool isAbout = false)
        {
            if (Prm.Type == AxisType.Cylinder_TwoPos)
            {
                return Result.OK;
            }
            else if (Prm.Type == AxisType.Reverse)
            {
                return ReverseAxis.Stop(isAbout);
            }
            return Card.StopAxis(Prm.Code, isAbout);
        }
        public Result StopGoHome()
        {
            if (Prm.Type == AxisType.Cylinder_TwoPos)
            {
                return Result.OK;
            }
            else if (Prm.Type == AxisType.Reverse)
            {
                return ReverseAxis.StopGoHome();
            }
            return Card.StopGoHome(Prm.Code);
        }
        /// <summary>
        /// 是否觸發軟限位
        /// </summary>
        /// <param name="pos"></param>
        /// <returns></returns>
        public bool IsTrigSoftLimit(double pos)
        {
            if (pos < Prm.SoftNegLimit || pos > Prm.SoftPosLimit)
            {
                return true;
            }
            else
            {
                return false;
            }
        }
    }
    /// <summary>
    /// 軸的類型
    /// </summary>
    public enum AxisType
    {
        /// <summary>
        /// 普通軸
        /// </summary>
        Common,
        /// <summary>
        /// IO控制的兩位置氣缸軸
        /// </summary>
        Cylinder_TwoPos,
        /// <summary>
        /// 與某軸運動方向相反的反向軸
        /// </summary>
        Reverse,
        /// <summary>
        /// 龍門主軸
        /// </summary>
        Master,
    }
}