You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
359 lines
15 KiB
C#
359 lines
15 KiB
C#
using HalconDotNet;
|
|
using Rs.Camera;
|
|
using Rs.Controls;
|
|
using Rs.Framework;
|
|
using Rs.Motion;
|
|
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
|
|
{
|
|
public enum ELoadAndUnloadTestStep
|
|
{
|
|
到取料位上方,
|
|
等待到取料位上方,
|
|
到取料位下方,
|
|
等待到取料位下方,
|
|
开真空,
|
|
取料真空检测,
|
|
取料完成抬起,
|
|
等待取料完成抬起,
|
|
到下相机拍照起始位,
|
|
等待到下相机拍照起始位,
|
|
到下相机拍照结束位,
|
|
等待到下相机拍照结束位,
|
|
等待相机拍照完成,
|
|
等待视觉处理结果,
|
|
到放料位上方,
|
|
等待到放料位上方,
|
|
到放料位下方,
|
|
等待到放料位下方,
|
|
放料完成抬起,
|
|
等待放料完成抬起,
|
|
到等待命令位,
|
|
等待到等待命令位,
|
|
保压上料,
|
|
等待保压上料完成,
|
|
保压下料,
|
|
等待保压下料完成
|
|
}
|
|
|
|
public class LoadAndUnloadTest:BaseFlow
|
|
{
|
|
private bool grabOK = false;
|
|
private static LoadAndUnloadTest instance;
|
|
|
|
public event Action<HObject[], List<MatchResult>> CameraOkEvent;
|
|
public static LoadAndUnloadTest Instance
|
|
{
|
|
get
|
|
{
|
|
if (instance == null)
|
|
{
|
|
instance = new LoadAndUnloadTest();
|
|
}
|
|
return instance;
|
|
}
|
|
}
|
|
private LoadAndUnloadTest()
|
|
{
|
|
//HikCamera.Instance.GrabOkEvent += (camIndex,img) => {
|
|
// grabOK = true;
|
|
//};
|
|
}
|
|
|
|
/// <summary>
|
|
/// 当前取料穴位
|
|
/// </summary>
|
|
private ushort CurFetchSlotIndex = 1;
|
|
/// <summary>
|
|
/// 当前组装穴位
|
|
/// </summary>
|
|
private ushort CurAssembleSlotInex = 1;
|
|
/// <summary>
|
|
/// 吸嘴索引
|
|
/// </summary>
|
|
private ushort NozzleIndex = 1;
|
|
|
|
bool[] stop = new bool[30];
|
|
|
|
ELoadAndUnloadTestStep flowStep = ELoadAndUnloadTestStep.到取料位上方;
|
|
HObject[] imgs = new HObject[2];
|
|
List<MatchResult> mrs = new List<MatchResult>();
|
|
int FetchNum = 0;//取料次数
|
|
bool bFetchBack = false;
|
|
double targetX = 0.0;
|
|
double targetY = 0.0;
|
|
TraySlot tray = new TraySlot();
|
|
ErrorCode errCode = ErrorCode.Ok;
|
|
|
|
public override void Run()
|
|
{
|
|
switch (flowStep)
|
|
{
|
|
case ELoadAndUnloadTestStep.到取料位上方:
|
|
SlotPoint sp1 = TrayPointManager.GetSlotPoint(GlobalVar.CurRecipe,"Input", CurFetchSlotIndex, EPointType.BASE);
|
|
SlotPoint dist = TrayPointManager.GetDistToNozzle1(NozzleIndex);
|
|
targetX = sp1.X + dist.X;
|
|
targetY = sp1.Y + dist.Y;
|
|
errCode = AxisControl.LoadX.MovePos(targetX, GlobalVar.WholeSpeed);
|
|
if (errCode != ErrorCode.Ok)
|
|
{
|
|
MessageQueue.Instance.Warn($"运动失败:{errCode}"); //运动失败
|
|
}
|
|
errCode = AxisControl.LoadY.MovePos(targetY, (int)(GlobalVar.WholeSpeed));
|
|
if (errCode != ErrorCode.Ok)
|
|
{
|
|
MessageQueue.Instance.Warn($"运动失败:{errCode}"); //运动失败
|
|
}
|
|
flowStep = ELoadAndUnloadTestStep.等待到取料位上方;
|
|
break;
|
|
case ELoadAndUnloadTestStep.等待到取料位上方:
|
|
if (Ops.IsStop("LoadX", "LoadY"))
|
|
{
|
|
Thread.Sleep(100);
|
|
flowStep = ELoadAndUnloadTestStep.到取料位下方;
|
|
|
|
}
|
|
break;
|
|
case ELoadAndUnloadTestStep.到取料位下方:
|
|
AxisControl.GetAxis($"NozzleZ{NozzleIndex}").MovePos(-5, GlobalVar.WholeSpeed);
|
|
flowStep = ELoadAndUnloadTestStep.等待到取料位下方;
|
|
break;
|
|
case ELoadAndUnloadTestStep.等待到取料位下方:
|
|
if (Ops.IsStop($"NozzleZ{NozzleIndex}"))
|
|
{
|
|
flowStep = ELoadAndUnloadTestStep.开真空;
|
|
}
|
|
break;
|
|
case ELoadAndUnloadTestStep.开真空:
|
|
IoManager.Instance.WriteOut($"{NozzleIndex}吸嘴真空吸电磁阀", 1);
|
|
Thread.Sleep(60);
|
|
flowStep = ELoadAndUnloadTestStep.取料完成抬起;
|
|
break;
|
|
|
|
case ELoadAndUnloadTestStep.取料完成抬起:
|
|
AxisControl.GetAxis($"NozzleZ{NozzleIndex}").MovePos(0, GlobalVar.WholeSpeed);
|
|
flowStep = ELoadAndUnloadTestStep.等待取料完成抬起;
|
|
break;
|
|
case ELoadAndUnloadTestStep.等待取料完成抬起:
|
|
if (Ops.IsStop($"NozzleZ{NozzleIndex}"))
|
|
{
|
|
flowStep = ELoadAndUnloadTestStep.取料真空检测;
|
|
//Thread.Sleep(100);
|
|
|
|
}
|
|
break;
|
|
case ELoadAndUnloadTestStep.取料真空检测:
|
|
if (IoManager.Instance.ReadIn($"{NozzleIndex}吸嘴真空吸检测") == 1 || GlobalVar.RunSpace)
|
|
{
|
|
FetchNum = 0;
|
|
if (NozzleIndex < 2)
|
|
{
|
|
NozzleIndex++;
|
|
flowStep = ELoadAndUnloadTestStep.到取料位上方;
|
|
}
|
|
else
|
|
{
|
|
NozzleIndex = 1;
|
|
flowStep = ELoadAndUnloadTestStep.到下相机拍照起始位;
|
|
}
|
|
CurFetchSlotIndex++;
|
|
}
|
|
else
|
|
{
|
|
FetchNum++;
|
|
if (FetchNum == 6)
|
|
{
|
|
Msg.ShowError($"吸嘴{NozzleIndex}取料{FetchNum}次失败报警,请处理后点击确定");
|
|
FetchNum = 0;
|
|
}
|
|
flowStep = ELoadAndUnloadTestStep.到取料位下方;
|
|
}
|
|
break;
|
|
case ELoadAndUnloadTestStep.到下相机拍照起始位:
|
|
if (XYCanMove())
|
|
{
|
|
ImageProcess.ClearAutoTrigger();
|
|
HikCamera.Instance.SetExposure("locationCamera", 40);
|
|
HikCamera.Instance.SetGain("locationCamera", 20);
|
|
AxisControl.LoadX.MovePos(SysConfigParam.GetValue<double>("Nozzle1CenterX")+30, GlobalVar.WholeSpeed);
|
|
AxisControl.LoadY.MovePos(SysConfigParam.GetValue<double>("Nozzle1CenterY"), GlobalVar.WholeSpeed);
|
|
flowStep = ELoadAndUnloadTestStep.等待到下相机拍照起始位;
|
|
}
|
|
|
|
break;
|
|
case ELoadAndUnloadTestStep.等待到下相机拍照起始位:
|
|
if (Ops.IsStop("LoadX","LoadY"))
|
|
{
|
|
flowStep = ELoadAndUnloadTestStep.到下相机拍照结束位;
|
|
Thread.Sleep(100);
|
|
HikCamera.Instance.SetTrigger("locationCamera", ETriggerMode.Auto);
|
|
AxisControl.LoadX.SetPosCompare(1, new double[] { SysConfigParam.GetValue<double>("Nozzle1CenterX"), SysConfigParam.GetValue<double>("Nozzle2CenterX") });
|
|
}
|
|
break;
|
|
|
|
case ELoadAndUnloadTestStep.到下相机拍照结束位:
|
|
if (XYCanMove())
|
|
{
|
|
IoManager.Instance.WriteOut("下左相机光源触发", 1);
|
|
Thread.Sleep(50);
|
|
AxisControl.LoadX.MovePos(SysConfigParam.GetValue<double>("Nozzle9CenterX") - 10, GlobalVar.FlyCameraSpeed);
|
|
flowStep = ELoadAndUnloadTestStep.等待到下相机拍照结束位;
|
|
}
|
|
|
|
break;
|
|
case ELoadAndUnloadTestStep.等待到下相机拍照结束位:
|
|
AxisControl.LoadX.IsStop(out stop[0]);
|
|
Stopwatch stopwatch = new Stopwatch();
|
|
if (Ops.IsStop("LoadX"))
|
|
{
|
|
AxisControl.LoadX.ClearCompare(1);
|
|
IoManager.Instance.WriteOut("下左相机光源触发", 0);
|
|
flowStep = ELoadAndUnloadTestStep.等待相机拍照完成;
|
|
stopwatch.Start();
|
|
}
|
|
break;
|
|
case ELoadAndUnloadTestStep.等待相机拍照完成:
|
|
if (GlobalVar.RunSpace)
|
|
{
|
|
flowStep = ELoadAndUnloadTestStep.到放料位上方;
|
|
mrs = new List<MatchResult>();
|
|
mrs.Add(new MatchResult());
|
|
mrs.Add(new MatchResult());
|
|
}
|
|
else
|
|
{
|
|
imgs = ImageProcess.GetAutoImage();
|
|
if (imgs != null && imgs.Length == 2)
|
|
{
|
|
CameraOkEvent?.Invoke(imgs, null);
|
|
//拍照完成
|
|
flowStep = ELoadAndUnloadTestStep.等待视觉处理结果;
|
|
|
|
}
|
|
}
|
|
break;
|
|
case ELoadAndUnloadTestStep.等待视觉处理结果:
|
|
mrs = VisionProcess.Instance.MatchDownCam(imgs);
|
|
if (mrs != null && mrs.Count == 2)
|
|
{
|
|
CameraOkEvent?.Invoke(imgs, mrs);
|
|
flowStep = ELoadAndUnloadTestStep.到放料位上方;
|
|
//Thread.Sleep(2000);
|
|
}
|
|
else
|
|
{
|
|
flowStep = ELoadAndUnloadTestStep.到下相机拍照起始位;
|
|
}
|
|
break;
|
|
case ELoadAndUnloadTestStep.到放料位上方:
|
|
if (XYCanMove())
|
|
{
|
|
targetX = 0.0;
|
|
targetY = 0.0;
|
|
SlotPoint sp2 = TrayPointManager.GetSlotPoint(GlobalVar.CurRecipe, "Turnover", CurAssembleSlotInex, EPointType.RUN);
|
|
SlotPoint nozzleDist = TrayPointManager.GetDistToNozzle1(NozzleIndex);
|
|
AxisControl.LoadX.MovePos(sp2.X+nozzleDist.X, GlobalVar.WholeSpeed);
|
|
AxisControl.LoadY.MovePos(sp2.Y + nozzleDist.Y, GlobalVar.WholeSpeed);
|
|
flowStep = ELoadAndUnloadTestStep.等待到放料位上方;
|
|
}
|
|
|
|
break;
|
|
case ELoadAndUnloadTestStep.等待到放料位上方:
|
|
AxisControl.LoadX.IsStop(out stop[0]);
|
|
AxisControl.LoadY.IsStop(out stop[1]);
|
|
if (Ops.IsStop("LoadX","LoadY"))
|
|
{
|
|
Thread.Sleep(100);
|
|
flowStep = ELoadAndUnloadTestStep.到放料位下方;
|
|
|
|
}
|
|
break;
|
|
case ELoadAndUnloadTestStep.到放料位下方:
|
|
AxisControl.GetAxis($"NozzleZ{NozzleIndex}").MovePos(-5, GlobalVar.WholeSpeed);
|
|
flowStep = ELoadAndUnloadTestStep.等待到放料位下方;
|
|
break;
|
|
case ELoadAndUnloadTestStep.等待到放料位下方:
|
|
if (Ops.IsStop($"NozzleZ{NozzleIndex}"))
|
|
{
|
|
IoManager.Instance.WriteOut($"{NozzleIndex}吸嘴真空吸电磁阀", 0);
|
|
Thread.Sleep(50);
|
|
IoManager.Instance.WriteOut($"{NozzleIndex}吸嘴真空破电磁阀", 1);
|
|
Thread.Sleep(100);
|
|
IoManager.Instance.WriteOut($"{NozzleIndex}吸嘴真空破电磁阀", 0);
|
|
flowStep = ELoadAndUnloadTestStep.放料完成抬起;
|
|
}
|
|
break;
|
|
case ELoadAndUnloadTestStep.放料完成抬起:
|
|
AxisControl.GetAxis($"NozzleZ{NozzleIndex}").MovePos(0, GlobalVar.WholeSpeed);
|
|
flowStep = ELoadAndUnloadTestStep.等待放料完成抬起;
|
|
break;
|
|
case ELoadAndUnloadTestStep.等待放料完成抬起:
|
|
if (Ops.IsStop($"NozzleZ{NozzleIndex}"))
|
|
{
|
|
Thread.Sleep(100);
|
|
if (NozzleIndex < 2)
|
|
{
|
|
NozzleIndex++;
|
|
flowStep = ELoadAndUnloadTestStep.到放料位上方;
|
|
}
|
|
else
|
|
{
|
|
NozzleIndex = 1;
|
|
flowStep = ELoadAndUnloadTestStep.到等待命令位;
|
|
}
|
|
CurAssembleSlotInex++;
|
|
if (CurAssembleSlotInex == 91)
|
|
{
|
|
CurAssembleSlotInex = 1;
|
|
CurFetchSlotIndex = 1;
|
|
}
|
|
}
|
|
break;
|
|
case ELoadAndUnloadTestStep.到等待命令位:
|
|
AxisControl.LoadX.MovePos(SysConfigParam.GetValue<double>("LoadXStartPos"),GlobalVar.WholeSpeed);
|
|
AxisControl.LoadY.MovePos(SysConfigParam.GetValue<double>("LoadYStartPos"), GlobalVar.WholeSpeed);
|
|
flowStep = ELoadAndUnloadTestStep.等待到等待命令位;
|
|
break;
|
|
case ELoadAndUnloadTestStep.等待到等待命令位:
|
|
if(Ops.IsStop("LoadX", "LoadY"))
|
|
{
|
|
flowStep = ELoadAndUnloadTestStep.保压上料;
|
|
}
|
|
break;
|
|
case ELoadAndUnloadTestStep.保压上料:
|
|
//TurnoverFlow.Instance.PressLoad();
|
|
flowStep = ELoadAndUnloadTestStep.等待保压上料完成;
|
|
break;
|
|
case ELoadAndUnloadTestStep.等待保压上料完成:
|
|
if(TurnoverFlow.Instance.LoadFinished())
|
|
{
|
|
flowStep = ELoadAndUnloadTestStep.等待保压下料完成;
|
|
//TurnoverFlow.Instance.PressUnload();
|
|
}
|
|
break;
|
|
case ELoadAndUnloadTestStep.等待保压下料完成:
|
|
if (TurnoverFlow.Instance.UnloadFinished())
|
|
{
|
|
flowStep = ELoadAndUnloadTestStep.到取料位上方;
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
private bool XYCanMove()
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
}
|