Metis_xctg/StageController/AxisController.cs

using System;
using System.Collections.Generic;
using System.Collections.Concurrent;
using System.IO.Ports;
using System.Threading;
using StageController.M3H;
using StageController.HDS;

namespace StageController
{
    public abstract class AxisController
    {
        //将流程控制从 M3HController.cs 中提取出来
        private Thread m_waitingThread;

        #region Open Close
        protected bool m_running = false;

        private void StopMoveflow()
        {
            if (m_currentMoveflow == null) return;
            m_currentMoveflow.Stop();
            m_currentMoveflow = null;
        }

        #endregion

        #region AxisList
        private Dictionary<AxisType, M3HAxis> AxisList;

        private M3HAxis m_AxisX => AxisList[AxisType.X];
        private M3HAxis m_AxisY => AxisList[AxisType.Y];
        private M3HAxis m_AxisZ => AxisList[AxisType.Z];

        private void InitAxisList()
        {
            AxisList = new Dictionary<AxisType, M3HAxis>();
            AxisList.Add(AxisType.X, new M3HAxis(M3HAxisType.X, this));
            AxisList.Add(AxisType.Y, new M3HAxis(M3HAxisType.Y, this));
            AxisList.Add(AxisType.Z, new M3HAxis(M3HAxisType.Z, this));
        }
        #endregion

        #region AxisShell List 虚拟轴控制        
        private Dictionary<AxisType, AxisShell> AxisShellList;
        /// <summary>
        /// M3H--> 0:停机自由,1:停机锁死,2:正向连续,3:反向连续,4:正向步进,5:反向步进
        /// </summary>
        public virtual int StateX => AxisShellList[AxisType.X].State;
        /// <summary>
        /// M3H--> 0:停机自由,1:停机锁死,2:正向连续,3:反向连续,4:正向步进,5:反向步进
        /// </summary>
        public virtual int StateY => AxisShellList[AxisType.Y].State;

        // Add by shayg 20220615 start
        public virtual int LimitX => AxisShellList[AxisType.X].Limit;
        public virtual int LimitY => AxisShellList[AxisType.Y].Limit;
        public virtual int LimitZ => AxisShellList[AxisType.Z].Limit;

        public virtual int ExcessStepX => AxisShellList[AxisType.X].ExcessStep;
        public virtual int ExcessStepY => AxisShellList[AxisType.Y].ExcessStep;
        public virtual int ExcessStepZ => AxisShellList[AxisType.Z].ExcessStep;
        // Add by shayg 20220615 end
        /// <summary>
        /// M3H--> 0:停机自由,1:停机锁死,2:正向连续,3:反向连续,4:正向步进,5:反向步进
        /// </summary>
        public virtual int StateZ => AxisShellList[AxisType.Z].State;

        public virtual double X => AxisShellList[AxisType.X].Actual;
        public virtual double Y => AxisShellList[AxisType.Y].Actual;
        public virtual double Z => AxisShellList[AxisType.Z].Actual;

        private void InitAxisShellList()
        {
            AxisShellList = new Dictionary<AxisType, AxisShell>();
            AxisShellList.Add(AxisType.X, new AxisShell(AxisType.X, this));
            AxisShellList.Add(AxisType.Y, new AxisShell(AxisType.Y, this));
            AxisShellList.Add(AxisType.Z, new AxisShell(AxisType.Z, this));
        }
        #endregion


        #region Command Runtime

        /// <summary>
        /// 运行中流程
        /// </summary>
        protected Moveflow m_currentMoveflow;

        protected ConcurrentQueue<CommandBase> m_cmdQueueM;

        protected Moveflow StartMoveflow()
        {
            m_cmdQueueM = new ConcurrentQueue<CommandBase>();
            return new Moveflow();
        }

        public void AddCommand(CommandBase command)
        {
            m_cmdQueueM.Enqueue(command);
        }

        private void Runtime()
        {
            while (m_running)
            {
                Thread.Sleep(25);
                Process();
            }
        }

        protected virtual void Process()
        {
            if (m_currentMoveflow != null && m_currentMoveflow.Done)
                StopMoveflow();

            #region 虚拟轴控制
            foreach (var axis in AxisShellList.Values)
            {
                axis.Execute();
            }
            #endregion
        }

        #endregion


        #region  Application Event 
        private List<IStageEvent> m_listApp = new List<IStageEvent>();
        #endregion


        //定义接收数据事件
        internal AxisController()
        {
            InitAxisList();
            InitAxisShellList();
        }
        public static string ControllerType = "HDS";
        static AxisController _instance;

        public static AxisController GetInstance(string type = null)
        {
            if (_instance == null)
            {
                if (type != null)
                {
                    ControllerType = type;
                }
                switch (ControllerType)
                {
                    case "M3H":
                        _instance = new M3HController();
                        break;
                    case "HDS":
                        _instance = new HDSController();
                        break;
                    default:
                        return null;
                }
                _instance.SetWorkspeedXY();
            }
            return _instance;
        }
        public abstract bool NeedSaveResetStageStatus { get; }
        public abstract bool FreeOrLockXYEnabled { get; }
        public abstract bool FreeOrLockZEnabled { get; }
        public abstract string Version { get; internal set; }
        public abstract bool IsOpen { get; }
        public virtual object LoadingStageModel
        {
            set
            {
                var m_loadingStageModel = value as PaintDotNet.Base.SettingModel.LoadingStageModel;

                m_AxisX.Stepping = double.Parse(m_loadingStageModel.SteppingX);
                m_AxisX.Reversed = true;
                m_AxisY.Stepping = double.Parse(m_loadingStageModel.SteppingY);
                m_AxisZ.Stepping = double.Parse(m_loadingStageModel.SteppingZ);
                //m_AxisZ.Reversed = true;//九孔台使用
            }
        }

        #region Position and State

        public abstract bool IsMoving { get; }
        public abstract void WaitMoveDone();
        #endregion Position and State

        public virtual string Open()
        {
            if (!m_running)
            {
                m_waitingThread = new Thread(new ThreadStart(Runtime));
                m_waitingThread.Start();
                m_running = true;
            }
            return "";
        }
        public virtual void Open(string portName, int baudRate, Parity parity, int dataBits, StopBits stopBits)
        {//M3H连接控制器的函数
        }
        public virtual void Close()
        {
            FreeStage();
            FreeZ();
        }
        public void AddApp(IStageEvent app)
        {
            var wk = new WeakReference(app);//弱引用
            m_listApp.Add((wk.Target as IStageEvent));
        }
        public void RemoveApp(IStageEvent app)
        {
            m_listApp.Remove(app);
            if (m_listApp.Count < 1)
            {
                FreeStage();
                FreeZ();
            }
        }
        protected virtual void ClearPosXY()
        {
            m_AxisX.ResetPosition();
            m_AxisY.ResetPosition();
        }
        public virtual void ClearPosZ()
        {
            m_AxisZ.ResetPosition();
            AxisShellList[AxisType.Z].ResetPosition();
        }
        public virtual void FreeStage()
        {
            StopMoveflow();

            AxisShellList[AxisType.X].Stop();
            AxisShellList[AxisType.Y].Stop();

            //Stop(AxisType.X, HandLock.F);
            //Stop(AxisType.Y, HandLock.F);
        }
        public virtual void FreeZ()
        {
            StopMoveflow();
            //Stop(AxisType.Z, HandLock.F);
            AxisShellList[AxisType.Z].Stop();
        }
        public virtual void LockStage()
        {
            StopMoveflow();
            _LockStage();
        }
        protected void _LockStage()
        {
            AxisShellList[AxisType.X].Stop();
            AxisShellList[AxisType.Y].Stop();

            Stop(AxisType.X, HandLock.S);
            Stop(AxisType.Y, HandLock.S);
        }
        public virtual void LockZ()
        {
            Stop(AxisType.Z, HandLock.S);
            AxisShellList[AxisType.Z].Stop();
        }
        public abstract void RefreshPosition();
        public abstract void SetWorkspeedXY();
        public virtual void SetSpeedXY(int speed)
        {
            var speed1 = SpeedConvertXY(speed);
            m_AxisX.Speed = speed1;
            m_AxisY.Speed = speed1;
            AxisShellList[AxisType.X].Speed = speed;
            AxisShellList[AxisType.Y].Speed = speed;
        }
        public virtual void SetSpeedZ(int speed)
        {
            m_AxisZ.Speed = SpeedConvertZ(speed);
            AxisShellList[AxisType.Z].Speed = speed;
        }
        /// <summary>
        /// XY速度换算  
        /// </summary>
        /// <param name="">微米每秒</param>
        /// <returns>速度映射值</returns>
        private int SpeedConvertXY(double umpers)
        {
            var speed = (1000000 * m_AxisX.Stepping / umpers - 33) / 10;
            return (int)speed;
        }
        /// <summary>
        /// Z速度换算  
        /// </summary>
        /// <param name="">微米每秒</param>
        /// <returns>速度映射值</returns>
        private int SpeedConvertZ(double umpers)
        {
            var speed = (1000000 * m_AxisZ.Stepping / umpers - 33) / 40;
            return (int)speed;
        }
        public virtual void Move(double x, double y)
        {
            if (m_currentMoveflow != null) StopMoveflow();
            if (IsMoving)
            {
                Console.WriteLine("XY运动中发送运动指令");
                return;
            }
            if (x != 0)
            {
                AxisShellList[AxisType.X].Move(x);
                m_AxisX.Move(x);
            }
            if (y != 0)
            {
                AxisShellList[AxisType.Y].Move(y);
                m_AxisY.Move(y);
            }
        }
        public virtual void To(double x, double y)
        {
            if (m_currentMoveflow != null) StopMoveflow();
            if (IsMoving) return;

            AxisShellList[AxisType.X].To(x);
            AxisShellList[AxisType.Y].To(y);

            m_AxisY.To(y);
            m_AxisX.To(x);
        }

        protected virtual void ToAction(AxisType axisType, double start)
        {
            AxisList[axisType].To(start);
        }

        protected virtual void MoveAction(AxisType axisType, double steplen)
        {
            AxisList[axisType].Move(steplen);
        }
        protected bool IsLimitXY()
        {
            return m_AxisX.Limit == LIMIT_N && m_AxisY.Limit == LIMIT_N;
        }
        protected virtual void Slide(AxisType axisType, bool isPositive)
        {
            switch (axisType)
            {
                case AxisType.X:
                    m_AxisX.Slide(isPositive);
                    break;
                case AxisType.Y:
                    m_AxisY.Slide(isPositive);
                    break;
                case AxisType.Z:
                    m_AxisZ.Slide(isPositive);
                    break;
                //case AxisType.W:
                //    m_AxisX.Slide(isPositive);
                //    break;
                default:
                    break;
            }
        }
        public virtual void Split(int direction)
        {
            if (m_currentMoveflow != null) StopMoveflow();

            switch (direction)
            {
                case 0:
                    m_AxisX.Slide(false);
                    AxisShellList[AxisType.X].Slide(false);
                    break;
                case 1:
                    AxisShellList[AxisType.X].Slide(false);
                    AxisShellList[AxisType.Y].Slide(false);
                    m_AxisX.Slide(false);
                    m_AxisY.Slide(false);
                    break;
                case 2:
                    AxisShellList[AxisType.Y].Slide(false);
                    m_AxisY.Slide(false);
                    break;
                case 3:
                    AxisShellList[AxisType.X].Slide(true);
                    AxisShellList[AxisType.Y].Slide(false);
                    m_AxisX.Slide(true);
                    m_AxisY.Slide(false);
                    break;
                case 4:
                    AxisShellList[AxisType.X].Slide(true);
                    m_AxisX.Slide(true);
                    break;
                case 5:
                    AxisShellList[AxisType.X].Slide(true);
                    AxisShellList[AxisType.Y].Slide(true);
                    m_AxisX.Slide(true);
                    m_AxisY.Slide(true);
                    break;
                case 6:
                    AxisShellList[AxisType.Y].Slide(true);
                    m_AxisY.Slide(true);
                    break;
                case 7:
                    AxisShellList[AxisType.X].Slide(false);
                    AxisShellList[AxisType.Y].Slide(true);
                    m_AxisX.Slide(false);
                    m_AxisY.Slide(true);
                    break;
            }
        }
        public virtual void Up(double z)
        {
            if (m_currentMoveflow != null) StopMoveflow();
            if (IsMoving) return;
            AxisShellList[AxisType.Z].Move(z);
            m_AxisZ.Move(z);
        }
        public virtual void Up(int z)
        {
            if (m_currentMoveflow != null) StopMoveflow();
            if (IsMoving)
            {
                Console.WriteLine("Z运动中发送运动指令");
                return;
            }

            AxisShellList[AxisType.Z].Move(z);
            m_AxisZ.Move(z);
        }
        public virtual void UpTo(double z)
        {
            if (m_currentMoveflow != null) StopMoveflow();
            if (IsMoving) return;

            AxisShellList[AxisType.Z].To(z);
            m_AxisZ.To(z);
        }

        /// <summary>
        /// Z轴连续运动
        /// </summary>
        /// <param name="isToTop">true:上；false：下</param>
        public virtual void GoTop(bool isToTop)
        {
            if (m_currentMoveflow != null) StopMoveflow();

            AxisShellList[AxisType.Z].Slide(isToTop);
            m_AxisZ.Slide(isToTop);
        }
        /// <summary>
        /// 平台中心位置
        /// </summary>
        protected virtual int _centerX => 0;
        /// <summary>
        /// 平台中心位置
        /// </summary>
        protected virtual int _centerY => 0;
        /// <summary>
        /// 轴锁定状态常量
        /// </summary>
        const int STATE_LOCK = 1;
        /// <summary>
        /// 负边界
        /// </summary>
        const int LIMIT_N = (int)Border.R;
        /// <summary>
        /// 边界内
        /// </summary>
        const int LIMIT_I = (int)Border.I;
        public virtual void ToCenter()
        {
            var mf = StartMoveflow();
            mf.Add(() =>
            {
                _LockStage();
            },
                () => { return m_AxisX.State == STATE_LOCK && m_AxisY.State == STATE_LOCK; });
            mf.Add(() =>
            {
                m_AxisX.Slide(false);
                m_AxisY.Slide(false);
            },
                () => { return m_AxisX.Limit == LIMIT_N && m_AxisY.Limit == LIMIT_N; });
            mf.Add(
                () =>
                {
                    m_AxisY.Move(_centerY);
                    m_AxisX.Move(_centerX);
                },
                () => { return m_AxisX.Limit == LIMIT_I || m_AxisY.Limit == LIMIT_I; });
            mf.Add(() => { }, () => { return m_AxisX.State == STATE_LOCK && m_AxisY.State == STATE_LOCK; });
            mf.Start();
            m_currentMoveflow = mf;
        }
        public virtual void ZScan(double start, double steplen, int times, Action shoot, Action package, Action actDone)
        {
            var mf = StartMoveflow();
            mf.Add(
                () => m_AxisZ.Stop(HandLock.S),
                null);
            mf.Add(() =>
            {
                m_AxisZ.To(start);
            },
                () =>
                {
                    RefreshPosition();
                    return Math.Abs(m_AxisZ.Actual - start) < m_AxisZ.Stepping;
                });
            mf.Add(shoot, null);
            for (int i = 0; i < times - 1; i++)
            {
                mf.Add(() =>
                {
                    m_AxisZ.Move(steplen);
                    start += steplen;
                },
                    () =>
                    {
                        RefreshPosition();
                        return Math.Abs(m_AxisZ.Actual - start) < m_AxisZ.Stepping;
                    });
                mf.Add(shoot, null);
            }
            mf.Add(package, null);
            mf.Start();
            mf.OnMoveflowDone += actDone;
            m_currentMoveflow = mf;
        }
        public abstract void ResetStage(Action actDone);

        public virtual void SetMergeMode(bool isMerge) { }
        public virtual void SetRockerEnabel(bool isAvailable) { }

        protected virtual void CommandError()
        {//M3H连接控制器的函数
            Logs.Write("CommandError");
            foreach (var app in m_listApp) app.OnErrorSend();
        }

        protected virtual void CommandTimeout()
        {//M3H连接控制器的函数
            Logs.Write("CommandTimeout");
            Console.Write("CommandTimeout");
            try
            {
                foreach (var app in m_listApp) app.OnTimeoutConnect();
            }
            catch { }
        }

        protected void PositionUpdate(CommandBase cmd)
        {
            var posInfo = (CommandAllPosition)cmd;
            m_AxisX.Step = posInfo.XStep;
            m_AxisY.Step = posInfo.YStep;
            m_AxisZ.Step = posInfo.ZStep;
            m_AxisX.Limit = (int)posInfo.XBorder;
            m_AxisY.Limit = (int)posInfo.YBorder;
            m_AxisZ.Limit = (int)posInfo.ZBorder;
            m_AxisX.State = posInfo.XState;
            m_AxisY.State = posInfo.YState;
            m_AxisZ.State = posInfo.ZState;

            AxisShellList[AxisType.X].Step = (m_AxisX.Reversed ? -posInfo.XStep : posInfo.XStep) * m_AxisX.Stepping;
            AxisShellList[AxisType.Y].Step = posInfo.YStep * m_AxisY.Stepping;
            AxisShellList[AxisType.Z].Step = posInfo.ZStep * m_AxisZ.Stepping;
            AxisShellList[AxisType.X].Limit = (int)posInfo.XBorder;
            AxisShellList[AxisType.Y].Limit = (int)posInfo.YBorder;
            AxisShellList[AxisType.Z].Limit = (int)posInfo.ZBorder;
            AxisShellList[AxisType.X].State = posInfo.XState;
            AxisShellList[AxisType.Y].State = posInfo.YState;
            AxisShellList[AxisType.Z].State = posInfo.ZState;

            // Add by shayg 20220615 start
            AxisShellList[AxisType.X].ExcessStep = posInfo.XExcessStep;
            AxisShellList[AxisType.Y].ExcessStep = posInfo.YExcessStep;
            AxisShellList[AxisType.Z].ExcessStep = posInfo.ZExcessStep;
            // Add by shayg 20220615 end

            for (int i = 0; i < m_listApp.Count; i++)
            {
                var app = m_listApp[i];
                if (app == null)
                    m_listApp.RemoveAt(i--);
                else
                    app.OnUpdatePosition();
            }
        }

        #region Stop
        protected virtual void Stop(AxisType axis, HandLock hlock)
        {
            AxisList[axis].Stop(hlock);
        }
        #endregion

    }
}