RealSys.MotionPlat/Rs.MotionPlat/Flow/TakeTrayFlow.cs

using Rs.Controls;
using Rs.Framework;
using Rs.MotionPlat.Commom;
using Rs.MotionPlat.Flow.Space;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace Rs.MotionPlat.Flow
{

    enum ETakeTrayFlowStep
    {
        等待命令,
        到取料盘上方,
        等待到取料盘上方,
        到下方取料盘,
        等待运动到取料位下方,
        取料盘完成抬起,
        取料盘后真空检测,
        到放料盘上方,
        等待到放料盘上方,
        到下方放料盘,
        等待运动到放料位下方,
        放料盘完成后抬起,
        等待放料完成抬起
    }

    public enum ETakeStatus
    {
        Idle,
        Taking,
        TakeOK
    }

    public enum ELoadUnloadType
    {
        Load,
        Unload
    }
    public class TakeTrayFlow:BaseFlow
    {
        private ELoadUnloadType mLoadUnloadType = ELoadUnloadType.Unload;
        private static TakeTrayFlow intance;
        private bool bTake = false;
        public ETakeStatus TakeStatus { get; set; } = ETakeStatus.Idle;
        public static TakeTrayFlow Instance
        {
            get
            {
                if(intance == null)
                    intance = new TakeTrayFlow();
                return intance;
            }
        }
        Stopwatch stopwatch = new Stopwatch();
        ETakeTrayFlowStep step = ETakeTrayFlowStep.等待命令;
        private EStockType m_fromTray = EStockType.Empty2;
        private EStockType m_toTray = EStockType.Ok;
        public void Take(EStockType fromTray, EStockType toTray, ELoadUnloadType loadUnloadType= ELoadUnloadType.Load)
        {
            TakeStatus = ETakeStatus.Taking;
            m_fromTray = fromTray;
            m_toTray = toTray;
            bTake = true;
            mLoadUnloadType=loadUnloadType;
        }

        public override void Run()
        {
            switch (step)
            {
                case ETakeTrayFlowStep.等待命令:
                    if(bTake)
                    {
                        bTake = false;
                        step = ETakeTrayFlowStep.到取料盘上方;
                    }
                    break;
                case ETakeTrayFlowStep.到取料盘上方:
                    double centerX = SysConfigParam.GetValue<double>($"Tray{m_fromTray.ToString()}CenterX");
                    double centerY = SysConfigParam.GetValue<double>($"Tray{m_fromTray.ToString()}CenterY");

                    double cameraCenterX = SysConfigParam.GetValue<double>("ClampCenter2UpCameraCenterX");
                    double cameraCenterY = SysConfigParam.GetValue<double>("ClampCenter2UpCameraCenterY");
                    Motion.ErrorCode errCode = AxisControl.LoadX.MovePos(centerX + cameraCenterX, GlobalVar.TakeTraySpeed);
                    if (errCode == Motion.ErrorCode.Ok)
                    {
                        errCode = AxisControl.LoadY.MovePos(centerY + cameraCenterY, GlobalVar.TakeTraySpeed);
                        if (errCode == Motion.ErrorCode.Ok)
                        {
                            step = ETakeTrayFlowStep.等待到取料盘上方;
                        }
                    }
                    break;
                case ETakeTrayFlowStep.等待到取料盘上方:
                    if (Ops.IsStop("LoadX", "LoadY"))
                    {
                        Ops.Off($"{(int)m_fromTray}号料仓斜推气缸");
                        Ops.Off($"{(int)m_toTray}号料仓斜推气缸");
                        Thread.Sleep(500);
                        step = ETakeTrayFlowStep.到下方取料盘;
                    }
                    break;
                case ETakeTrayFlowStep.到下方取料盘:
                    Ops.On("上下气缸电磁阀动位");
                    Ops.Off("上下气缸电磁阀原位");
                    Thread.Sleep(150);
                    step = ETakeTrayFlowStep.等待运动到取料位下方;

                    stopwatch.Restart();
                    break;
                case ETakeTrayFlowStep.等待运动到取料位下方:
                    if (Ops.IsOn("上下气缸动位") && stopwatch.ElapsedMilliseconds < 5000)
                    {
                        stopwatch.Stop();
                        Ops.On("夹爪真空吸");
                        Thread.Sleep(150);
                        step = ETakeTrayFlowStep.取料盘完成抬起;
                    }
                    else if(stopwatch.ElapsedMilliseconds > 5000)
                    {
                        Msg.ShowError("上下气缸动位异常，请处理后点击确定");
                        stopwatch.Restart();
                    }
                    break;
                case ETakeTrayFlowStep.取料盘完成抬起:
                    Ops.Off("上下气缸电磁阀动位");
                    Ops.On("上下气缸电磁阀原位");
                    Thread.Sleep(150);
                    step = ETakeTrayFlowStep.取料盘后真空检测;
                    break;
                case ETakeTrayFlowStep.取料盘后真空检测:
                    if (Ops.IsOn("上下气缸原位"))
                    {
                        step = ETakeTrayFlowStep.到放料盘上方;
                        if(m_fromTray== EStockType.Empty2)
                        {
                            StockManager.Instance.Load(m_fromTray);
                        }
                    }
                    break;
                case ETakeTrayFlowStep.到放料盘上方:
                    centerX = SysConfigParam.GetValue<double>($"Tray{m_toTray.ToString()}CenterX");
                    centerY = SysConfigParam.GetValue<double>($"Tray{m_toTray.ToString()}CenterY");

                    cameraCenterX = SysConfigParam.GetValue<double>("ClampCenter2UpCameraCenterX");
                    cameraCenterY = SysConfigParam.GetValue<double>("ClampCenter2UpCameraCenterY");
                    errCode = AxisControl.LoadX.MovePos(centerX + cameraCenterX, GlobalVar.TakeTraySpeed);
                    if (errCode == Motion.ErrorCode.Ok)
                    {
                        errCode = AxisControl.LoadY.MovePos(centerY + cameraCenterY-2, GlobalVar.TakeTraySpeed);
                        if (errCode == Motion.ErrorCode.Ok)
                        {
                            step = ETakeTrayFlowStep.等待到放料盘上方;
                        }
                    }
                    break;
                case ETakeTrayFlowStep.等待到放料盘上方:
                    if (Ops.IsStop("LoadX", "LoadY"))
                    {
                        step = ETakeTrayFlowStep.到下方放料盘;
                    }
                    break;
                case ETakeTrayFlowStep.到下方放料盘:
                    Ops.On("上下气缸电磁阀动位");
                    Ops.Off("上下气缸电磁阀原位");
                    Thread.Sleep(150);
                    step = ETakeTrayFlowStep.等待运动到放料位下方;
                    stopwatch.Restart();
                    break;
                case ETakeTrayFlowStep.等待运动到放料位下方:
                    if (Ops.IsOn("上下气缸动位") || stopwatch.ElapsedMilliseconds > 5000)
                    {
                        stopwatch.Stop();
                        Ops.Off("夹爪真空吸");
                        Thread.Sleep(10);
                        Ops.On("夹爪真空破");
                        Thread.Sleep(200);
                        Ops.Off("夹爪真空破");
                        step = ETakeTrayFlowStep.放料盘完成后抬起;
                    }
                    break;
                case ETakeTrayFlowStep.放料盘完成后抬起:
                    Ops.Off("上下气缸电磁阀动位");
                    Ops.On("上下气缸电磁阀原位");
                    Thread.Sleep(150);
                    step = ETakeTrayFlowStep.等待放料完成抬起;
                    break;
                case ETakeTrayFlowStep.等待放料完成抬起:
                    if (Ops.IsOn("上下气缸原位"))
                    {
                        if(mLoadUnloadType== ELoadUnloadType.Load)
                        {
                            StockManager.Instance.Load(m_toTray,false);
                        }
                        step = ETakeTrayFlowStep.等待命令;
                        TakeStatus = ETakeStatus.TakeOK;
                    }
                    break;
            }
        }
    }
}