RealSys.MotionPlat/Rs.MotionPlat/Flow/Space/TakeTrayTest.cs

using Rs.Controls;
using Rs.Framework;
using Rs.MotionPlat.Commom;
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.Space
{
    enum ETakeTrayTestStep
    {
        到取料盘上方,
        等待到取料盘上方,
        到下方取料盘,
        等待运动到取料位下方,
        取料盘完成抬起,
        取料盘后真空检测,
        到放料盘上方,
        等待到放料盘上方,
        到下方放料盘,
        等待运动到放料位下方,
        放料盘完成后抬起,
        等待放料完成抬起
    }
    public class TakeTrayTest:BaseFlow
    {
        Dictionary<int, string> trayNameDic = new Dictionary<int, string>();
        private static TakeTrayTest instance;
        public static TakeTrayTest Instance
        {
            get
            {
                if (instance == null)
                    instance = new TakeTrayTest();
                return instance;
            }
        }
        private TakeTrayTest()
        {
            trayNameDic.Add(1, "Empty1");
            trayNameDic.Add(2, "Input");
            trayNameDic.Add(3, "Ok");
            trayNameDic.Add(4, "Ng");
            trayNameDic.Add(5, "Multi");
            trayNameDic.Add(6, "Empty2");

        }
        int trayIndex = 1;
        Stopwatch stopwatch = new Stopwatch();
        ETakeTrayTestStep step = ETakeTrayTestStep.到取料盘上方;

        public override void Run()
        {
            switch (step)
            {
                case ETakeTrayTestStep.到取料盘上方:
                    double centerX = SysConfigParam.GetValue<double>($"Tray{trayNameDic[trayIndex]}CenterX");
                    double centerY = SysConfigParam.GetValue<double>($"Tray{trayNameDic[trayIndex]}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 = ETakeTrayTestStep.等待到取料盘上方;
                        }
                    }
                    break;
                case ETakeTrayTestStep.等待到取料盘上方:
                    if(Ops.IsStop("LoadX","LoadY"))
                    {
                        step = ETakeTrayTestStep.到下方取料盘;
                    }
                    break;
                case ETakeTrayTestStep.到下方取料盘:
                    Ops.On("上下气缸电磁阀动位");
                    Ops.Off("上下气缸电磁阀原位");
                    Thread.Sleep(150);
                    step = ETakeTrayTestStep.等待运动到取料位下方;
                    
                    stopwatch.Restart();
                    break;
                case ETakeTrayTestStep.等待运动到取料位下方:
                    if (Ops.IsOn("上下气缸动位") || stopwatch.ElapsedMilliseconds>5000)
                    {
                        stopwatch.Stop();
                        Ops.On("夹爪真空吸");
                        Thread.Sleep(150);
                        step = ETakeTrayTestStep.取料盘完成抬起;
                    }
                    break;
                case ETakeTrayTestStep.取料盘完成抬起:
                    Ops.Off("上下气缸电磁阀动位");
                    Ops.On("上下气缸电磁阀原位");
                    Thread.Sleep(150);
                    step = ETakeTrayTestStep.取料盘后真空检测;
                    break;
                case ETakeTrayTestStep.取料盘后真空检测:
                    if(Ops.IsOn("上下气缸原位"))
                    {
                        step = ETakeTrayTestStep.到放料盘上方;
                        if(trayIndex==6)
                        {
                            trayIndex = 1;
                        }
                        else
                        {
                            trayIndex++;
                        }
                       
                    }
                    break;
                case ETakeTrayTestStep.到放料盘上方:
                    centerX = SysConfigParam.GetValue<double>($"Tray{trayNameDic[trayIndex]}CenterX");
                    centerY = SysConfigParam.GetValue<double>($"Tray{trayNameDic[trayIndex]}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, GlobalVar.TakeTraySpeed);
                        if (errCode == Motion.ErrorCode.Ok)
                        {
                            step = ETakeTrayTestStep.等待到放料盘上方;
                        }
                    }
                    break;
                case ETakeTrayTestStep.等待到放料盘上方:
                    if (Ops.IsStop("LoadX", "LoadY"))
                    {
                        step = ETakeTrayTestStep.到下方放料盘;
                    }
                    break;
                case ETakeTrayTestStep.到下方放料盘:
                    Ops.On("上下气缸电磁阀动位");
                    Ops.Off("上下气缸电磁阀原位");
                    Thread.Sleep(150);
                    step = ETakeTrayTestStep.等待运动到放料位下方;
                    stopwatch.Restart();
                    break;
                case ETakeTrayTestStep.等待运动到放料位下方:
                    if(Ops.IsOn("上下气缸动位") || stopwatch.ElapsedMilliseconds > 5000)
                    {
                        stopwatch.Stop();
                        Ops.Off("夹爪真空吸");
                        Thread.Sleep(10);
                        Ops.On("夹爪真空破");
                        Thread.Sleep(200);
                        Ops.Off("夹爪真空破");
                        step = ETakeTrayTestStep.放料盘完成后抬起;
                    }
                    break;
                case ETakeTrayTestStep.放料盘完成后抬起:
                    Ops.Off("上下气缸电磁阀动位");
                    Ops.On("上下气缸电磁阀原位");
                    Thread.Sleep(150);
                    step = ETakeTrayTestStep.等待放料完成抬起;
                    break;
                case ETakeTrayTestStep.等待放料完成抬起:
                    if(Ops.IsOn("上下气缸原位"))
                    {
                        
                        step = ETakeTrayTestStep.到取料盘上方;
                    }
                    break;
            }
        }
    }
}