LIBMAS/StageController/M3H/AxisController.cs

using System;
using System.Collections.Generic;
using System.Collections.Concurrent;
using System.IO.Ports;
using System.Linq;
using System.Linq.Expressions;
using System.Text;
using System.Threading;
using PaintDotNet.Base.SettingModel;
using StageController.M3H;
using System.Threading.Tasks;

namespace StageController
{
    public delegate void DataReceiveEvent(CommandBase command, Result result, string msg);

    public class AxisController
    {
        //定义接收数据事件
        public event DataReceiveEvent DataReceived;

        #region Com Connection
        private SerialPort m_serialPort;
        private string m_portName;
        private int m_baudRate;
        #endregion

        private ConcurrentQueue<CommandBase> m_cmdQueueM;
        private Thread m_waitingThread;

        private bool IsSimulation = false;
        /// <summary>
        /// 发送试错次数
        /// </summary>
        private int m_retryCount = 3;

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

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

        private void InitAxisList()
        {
            AxisList = new Dictionary<M3HAxisType, M3HAxis>();
            AxisList.Add(M3HAxisType.X, new M3HAxis(M3HAxisType.X, this));
            AxisList.Add(M3HAxisType.Y, new M3HAxis(M3HAxisType.Y, this));
            AxisList.Add(M3HAxisType.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 int StateX => AxisShellList[AxisType.X].State;
        /// <summary>
        /// M3H--> 0:停机自由,1:停机锁死,2:正向连续,3:反向连续,4:正向步进,5:反向步进
        /// </summary>
        public int StateY => AxisShellList[AxisType.Y].State;
        /// <summary>
        /// M3H--> 0:停机自由,1:停机锁死,2:正向连续,3:反向连续,4:正向步进,5:反向步进
        /// </summary>
        public int StateZ => AxisShellList[AxisType.Z].State;

        //public bool StageRelox { get => StateX + StateY + StateZ == 0; }

        public double X => AxisShellList[AxisType.X].Actual;
        public double Y => AxisShellList[AxisType.Y].Actual;
        public 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));

            AxisShellList[AxisType.X].SpeedConvert = SpeedConvert;
            AxisShellList[AxisType.Y].SpeedConvert = SpeedConvert;
            AxisShellList[AxisType.Z].SpeedConvert = SpeedConvert;
        }

        private int SpeedConvert(int seed)
        {
            switch (seed)
            {
                case 2:
                    return 30000;
                case 4:
                    return 30000;
                case 8:
                    return 25000;
                case 10:
                    return 22500;
                case 20:
                    return 15200;
                case 40:
                    return 9650;
                case 60:
                    return 6950;
                case 80:
                    return 5450;
                case 100:
                    return 500;

            }
            return 5000;
        }
        #endregion

        #region Instance
        private static AxisController m_instance = null;
        private AxisController()
        {
            InitAxisList();
            InitAxisShellList();
            //IsSimulation = true;
            m_cmdQueueM = new ConcurrentQueue<CommandBase>();
            CommandBase.AddReceivedHandle(typeof(CommandAllPosition), new Action<CommandBase>(PositionUpdate));
            CommandBase.AddReceivedHandle(typeof(CommandVersion), new Action<CommandBase>(VersionUpdate));
        }
        public static AxisController GetInstance()
        {
            if (m_instance == null)
            {
                m_instance = new AxisController();
            }

            return m_instance;
        }
        #endregion

        public LoadingStageModel LoadingStageModel
        {
            set
            {
                var m_loadingStageModel = value;
                m_portName = m_loadingStageModel.BaseSetPort;
                m_baudRate = m_loadingStageModel.BaseSetBps;

                AxisShellList[AxisType.X].Stepping = double.Parse(m_loadingStageModel.SteppingX);
                AxisShellList[AxisType.Y].Stepping = double.Parse(m_loadingStageModel.SteppingY);
                AxisShellList[AxisType.Z].Stepping = double.Parse(m_loadingStageModel.SteppingZ);


                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);
            }
        }

        #region Open Close
        private bool m_running = false;

        /// <summary>
        /// 平台是否正常开启
        /// </summary>
        public bool IsOpen
        {
            get => m_running;
        }
        public string Version { get; private set; }

        // TODO: 此函数测试期间使用
        public string Open()
        {
            try
            {
                if (!m_running)
                    Open(m_portName, m_baudRate, Parity.None, 8, StopBits.One);
                return "";
            }
            catch (Exception ex)
            {
                return ex.Message;
            }
        }

        public void Open(string portName, int baudRate, Parity parity, int dataBits, StopBits stopBits)
        {
            if (m_serialPort == null || !m_serialPort.IsOpen)
            {
                m_serialPort = new SerialPort(portName, baudRate, parity, dataBits, stopBits);

                m_serialPort.Encoding = Encoding.ASCII;
                m_serialPort.ReadTimeout = 500;
                m_serialPort.DataReceived += new SerialDataReceivedEventHandler(SerialPort_DataReceived);
                m_serialPort.Open();
                CommandBase.SerialPort = m_serialPort;
            }

            if (!m_running)
            {
                m_cmdQueueM = new ConcurrentQueue<CommandBase>();
                m_curCommand = null;
                m_waitingThread = new Thread(new ThreadStart(Runtime));
                m_waitingThread.Start();
                m_running = true;

                ResetParameter();
            }
        }

        public void Close()
        {
            FreeStage();
            FreeZ();
            _closing = true;

        }
        #endregion

        // 接收串口返回数据
        private void SerialPort_DataReceived(object sender, SerialDataReceivedEventArgs e)
        {
            if (!m_running) return;
            string recvData = ((SerialPort)sender).ReadExisting();
            m_curCommand.Receive(recvData);
        }

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

        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);
        }
        #endregion

        #region Command Runtime
        private CommandBase m_curCommand = null;

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

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

        enum ConnectErrorType
        {
            SendFailed,
            Timeout
        }

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

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

            if (IsSimulation)
            {
                if (_closing)
                {
                    m_running = false;
                    _closing = false;
                }
                return;
            }
            #endregion

            #region 执行当前指令
            if (m_curCommand != null)
            {
                switch (m_curCommand.State)
                {
                    case -1:                    //error:停止当前运动控制服务.
                        if (m_curCommand.SendCount > m_retryCount) CommandError();
                        m_curCommand.Send();
                        return;
                    case 1:                     //等待平台应答
                        if (m_curCommand.IsTimeout)
                        {
                            m_curCommand = null;
                            // CommandTimeout();
                        }
                        break;
                    case 2:                     //完成应答处理
                        m_curCommand = null;
                        break;
                    case 0:                     //初始值, 此处不应该出现0的情况
                                                //m_curCommand.Send()
                    default:
                        break;
                }
            }
            #endregion

            #region 开始一个新指令,
            if (m_curCommand == null)
            {
                if (m_cmdQueueM.IsEmpty && _closing)
                {
                    m_running = false;
                    _closing = false;
                    return;
                }
                if (m_cmdQueueM.IsEmpty || !m_cmdQueueM.TryDequeue(out m_curCommand))
                {   // 空闲时间刷新位置
                    // m_curCommand = new CommandAllPosition();
                }
                if (m_curCommand != null && !m_curCommand.Send())
                {   //指令发送失败
                    CommandError();
                }
            }
            #endregion
        }

        private void CommandError()
        {
            Logs.Write("CommandError");
            foreach (var app in m_listApp) app.OnErrorSend();
            m_curCommand = null;
        }

        private void CommandTimeout()
        {
            Logs.Write("CommandTimeout");
            foreach (var app in m_listApp) app.OnTimeoutConnect();
            // m_running = false;
            // IsSimulation = true;
            m_serialPort.Close();
        }

        public void RefreshPosition()
        {
            if (m_cmdQueueM.Any(e => e is CommandAllPosition))
                return;
            AddCommand(new CommandAllPosition());
        }

        private 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;
            AxisShellList[AxisType.Y].Step = posInfo.YStep;
            AxisShellList[AxisType.Z].Step = posInfo.ZStep;
            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;

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

        private void VersionUpdate(CommandBase cmd)
        {
            Version = (cmd as CommandVersion).Version;
        }
        #endregion

        #region Motion
        /// <summary>
        /// 平台平移
        /// </summary>
        /// <param name="x">X轴移动距离(μm)</param>
        /// <param name="y">Y轴移动距离(μm)</param>
        public void Move(double x, double y)
        {
            if (m_currentMoveflow != null) StopMoveflow();
            //throw new Exception("MoveflowBusy");
            AxisShellList[AxisType.X].Move(x);
            AxisShellList[AxisType.Y].Move(y);
            m_AxisY.Move(y);
            m_AxisX.Move(x);
        }

        public void To(double x, double y)
        {
            if (m_currentMoveflow != null) StopMoveflow();
            //throw new Exception("MoveflowBusy");
            AxisShellList[AxisType.X].To(x);
            AxisShellList[AxisType.Y].To(y);

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

        public void Up(double z)
        {
            if (m_currentMoveflow != null) StopMoveflow();
            // throw new Exception("MoveflowBusy");

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

        public void UpTo(double z)
        {
            if (m_currentMoveflow != null) StopMoveflow();
            // throw new Exception("MoveflowBusy");
            
            AxisShellList[AxisType.Z].To(z);
            m_AxisZ.To(z);
        }

        public void Up(int z)
        {
            if (m_currentMoveflow != null) StopMoveflow();
            //throw new Exception("MoveflowBusy");
            AxisShellList[AxisType.Z].Move(z);

            m_AxisZ.Move(z);
        }

        /// <summary>
        /// Z连续
        /// </summary>
        /// <param name="isToTop">true:正向.false:反向</param>
        public void GoTop(bool isToTop)
        {
            if (m_currentMoveflow != null) StopMoveflow();
            //throw new Exception("MoveflowBusy");

            AxisShellList[AxisType.Z].Slide(isToTop);
            m_AxisZ.Slide(isToTop);
        }


        /// <summary>
        /// 平台朝一个水平方向连续运动
        /// </summary>
        /// <param name="direction">0-7 从X周负向逆时针</param>
        public void Split(int direction)
        {
            if (m_currentMoveflow != null) StopMoveflow();
            //   throw new Exception("MoveflowBusy");

            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;
            }
        }

        #endregion

        #region Stop
        public void FreeStage()
        {
            StopMoveflow();

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

            Stop(M3HAxisType.X, HandLock.F);
            Stop(M3HAxisType.Y, HandLock.F);
        }

        public void LockStage()
        {
            StopMoveflow();

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

            Stop(M3HAxisType.X, HandLock.S);
            Stop(M3HAxisType.Y, HandLock.S);
        }

        public void ClearPosXY()
        {
            m_AxisX.ResetPosition();
            m_AxisY.ResetPosition();

            RefreshPosition();

            AxisShellList[AxisType.X].ResetPosition();
            AxisShellList[AxisType.Y].ResetPosition();
        }

        public void FreeZ()
        {
            StopMoveflow();
            Stop(M3HAxisType.Z, HandLock.F);
            AxisShellList[AxisType.Z].Stop();
        }

        public void LockZ()
        {
            Stop(M3HAxisType.Z, HandLock.S);
            AxisShellList[AxisType.Z].Stop();
        }

        public void ClearPosZ()
        {
            m_AxisZ.ResetPosition();
            AxisShellList[AxisType.Z].ResetPosition();
        }
        private void Stop(M3HAxisType axis, HandLock hlock)
        {
            AxisList[axis].Stop(hlock);
        }
        #endregion

        #region Moveflow
        /// <summary>
        /// 平台中心位置
        /// </summary>
        private int _centerX = 106500;
        /// <summary>
        /// 平台中心位置
        /// </summary>
        private int _centerY = 73500;

        /// <summary>
        /// 轴锁定状态常量
        /// </summary>
        const int STATE_LOCK = 1;
        /// <summary>
        /// 正边界
        /// </summary>
        const int LIMIT_P = (int)Border.F;
        /// <summary>
        /// 负边界
        /// </summary>
        const int LIMIT_N = (int)Border.R;
        /// <summary>
        /// 边界内
        /// </summary>
        const int LIMIT_I = (int)Border.I;

        /// <summary>
        /// 运行中流程
        /// </summary>
        Moveflow m_currentMoveflow;
        private bool _closing;

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

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

        /// <summary>
        /// 开始中心移动流程
        /// </summary>
        public void ToCenter()
        {
            if (IsSimulation) return;

            var mf = StartMoveflow();
            mf.Add(() =>
                {
                    m_AxisY.Stop(HandLock.S);
                    m_AxisX.Stop(HandLock.S);
                },
                () => { 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;
        }

        /// <summary>
        /// 开始复位流程
        /// </summary>
        public void ResetStage(Action actDone)
        {
            if (IsSimulation)//模拟模式
            {
                ClearPosXY();
                actDone();
                return;
            }

            var mf = StartMoveflow();
            mf.Add(() =>
            {
                m_AxisY.Stop(HandLock.S);
                m_AxisX.Stop(HandLock.S);
            },
               null// () => { return m_AxisX.State == STATE_LOCK && m_AxisY.State == STATE_LOCK; }
                );
            mf.Add(() =>
                {
                    m_AxisX.Slide(false);
                    m_AxisY.Slide(false);
                },
                () =>
                {
                    RefreshPosition();
                    return m_AxisX.Limit == LIMIT_N && m_AxisY.Limit == LIMIT_N;
                });
            mf.Add(() =>
                {
                    ClearPosXY();
                },
                () => { return X == 0 && Y == 0; });
            mf.Start();
            mf.OnMoveflowDone += actDone;
            m_currentMoveflow = mf;
        }

        public void ZScan(double start, double steplen, int times, Action shoot, Action actDone)
        {
            if (IsSimulation)//模拟模式
                return;

            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.Start();
            mf.OnMoveflowDone += actDone;
            m_currentMoveflow = mf;
        }
        #endregion

        #region Speed
        public void SetSpeedXY(int speed)
        {
            m_AxisX.Speed = speed;
            m_AxisY.Speed = speed;

            AxisShellList[AxisType.X].Speed = speed;
            AxisShellList[AxisType.Y].Speed = speed;
        }

        public void SetSpeedZ(int speed)
        {
            m_AxisZ.Speed = speed;
        }
        #endregion

        public void ResetParameter()
        {
            AddCommand(new CommandAllPosition());
            AddCommand(new CommandParameter() { AxisSelect = 'x' });
            AddCommand(new CommandParameter() { AxisSelect = 'y' });
            // AddCommand(new CommandParameter() { AxisSelect = 'z', LockOrFree = 'F' });  // z轴参数下发不识别
            AddCommand(new CommandVersion());
        }

        /// <summary>
        /// 拼图模式
        /// </summary>
        public void SetMergeMode(bool isMerge)
        {
            if (isMerge)
                AddCommand(new CommandGlobal(ControlType.P));
            else
                AddCommand(new CommandGlobal(ControlType.Q));
        }

        /// <summary>
        /// 启用/禁用摇杆
        /// </summary>
        /// <param name="isAvailable">摇杆可用</param>
        public void SetRockerEnabel(bool isAvailable)
        {

        }
    }
}