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.Drawing; using System.Linq; using System.Text; using System.Threading; using System.Threading.Tasks; using static GTN.mc_ecat; namespace Rs.MotionPlat.Flow { public enum ESpaceFlowStep { 到取料位上方, 等待到取料位上方, 到取料位下方, 等待到取料位下方, 开真空, 取料真空检测, 取料完成抬起, 等待取料完成抬起, 到下相机拍照起始位, 等待到下相机拍照起始位, 到下相机拍照结束位, 等待到下相机拍照结束位, 等待相机拍照完成, 等待视觉处理结果, 到放料位上方, 等待到放料位上方, 到放料位下方, 等待到放料位下方, 放料完成抬起, 等待放料完成抬起 } public class SpaceFlow:BaseFlow { private static SpaceFlow instance; public static SpaceFlow Instance { get { if (instance == null) instance = new SpaceFlow(); return instance; } } private bool grabOK = false; public event Action> CameraOkEvent; /// /// 当前取料穴位 /// private ushort CurFetchSlotIndex = 1; /// /// 当前组装穴位 /// private ushort CurAssembleSlotInex = 1; /// /// 吸嘴索引 /// private ushort NozzleIndex = 1; bool[] stop = new bool[30]; ESpaceFlowStep flowStep = ESpaceFlowStep.到取料位上方; HObject[] imgs = new HObject[2]; List mrs = new List(); int FetchNum = 0;//取料次数 bool bFetchBack = false; double targetX = 0.0; double targetY = 0.0; TraySlot tray = new TraySlot(); private SpaceFlow() { } public override void Run() { //switch (flowStep) //{ // case ESpaceFlowStep.到取料位上方: // if (XYCanMove()) // { // if (bFetchBack == false) // { // tray = GlobalTray.InputTray2.GetSlot(CurFetchSlotIndex); // } // else // { // tray = GlobalTray.OkTray2.GetSlot(CurFetchSlotIndex); // } // if (NozzleIndex == 1) // { // targetX = tray.Position.X + (GlobalVar.Instance.Nozzle1Center2CameraCenterDistX * -1); // targetY = tray.Position.Y - GlobalVar.Instance.Nozzle1Center2CameraCenterDistY; // AxisControl.LoadX.MovePos(targetX, GlobalVar.Instance.WholeSpeed); // AxisControl.LoadY.MovePos(targetY, (int)(GlobalVar.Instance.WholeSpeed)); // AxisControl.VacR1.MovePos(0, GlobalVar.Instance.WholeSpeed); // } // else if (NozzleIndex == 2) // { // targetX = tray.Position.X + (GlobalVar.Instance.Nozzle1Center2CameraCenterDistX * -1) + GlobalVar.Instance.Nozzle2Center2Nozzle1CenterDistX; // targetY = tray.Position.Y - GlobalVar.Instance.Nozzle1Center2CameraCenterDistY + GlobalVar.Instance.Nozzle2Center2Nozzle1CenterDistY; // AxisControl.LoadX.MovePos(targetX, GlobalVar.Instance.WholeSpeed); // AxisControl.LoadY.MovePos(targetY, 1); // AxisControl.VacR2.MovePos(0, GlobalVar.Instance.WholeSpeed); // } // flowStep = ESpaceFlowStep.等待到取料位上方; // } // break; // case ESpaceFlowStep.等待到取料位上方: // AxisControl.LoadX.IsStop(out stop[0]); // AxisControl.LoadY.IsStop(out stop[1]); // AxisControl.VacR1.IsStop(out stop[2]); // AxisControl.VacR2.IsStop(out stop[3]); // if (stop[0] && stop[1] && stop[2] && stop[3]) // { // Thread.Sleep(100); // flowStep = ESpaceFlowStep.到取料位下方; // } // break; // case ESpaceFlowStep.到取料位下方: // if (NozzleIndex == 1) // { // AxisControl.VacZ1.MovePos(GetVacOffsetHeight(FetchNum) + GlobalVar.Instance.Vac1Tray1FetchHeight, GlobalVar.Instance.WholeSpeed); // } // else // { // AxisControl.VacZ2.MovePos(GetVacOffsetHeight(FetchNum) + GlobalVar.Instance.Vac2Tray1FetchHeight, GlobalVar.Instance.WholeSpeed); // } // flowStep = ESpaceFlowStep.等待到取料位下方; // break; // case ESpaceFlowStep.等待到取料位下方: // AxisControl.VacZ1.IsStop(out stop[0]); // AxisControl.VacZ2.IsStop(out stop[1]); // if (stop[0] && stop[1]) // { // flowStep = ESpaceFlowStep.开真空; // } // break; // case ESpaceFlowStep.开真空: // IoManager.Instance.WriteOut($"吸嘴{NozzleIndex}真空吸", 1); // Thread.Sleep(60); // flowStep = ESpaceFlowStep.取料完成抬起; // break; // case ESpaceFlowStep.取料完成抬起: // if (NozzleIndex == 1) // { // AxisControl.VacZ1.MovePos(0, GlobalVar.Instance.WholeSpeed); // } // else // { // AxisControl.VacZ2.MovePos(0, GlobalVar.Instance.WholeSpeed); // } // flowStep = ESpaceFlowStep.等待取料完成抬起; // break; // case ESpaceFlowStep.等待取料完成抬起: // AxisControl.VacZ1.IsStop(out stop[0]); // AxisControl.VacZ2.IsStop(out stop[1]); // if (stop[0] && stop[1]) // { // flowStep = ESpaceFlowStep.取料真空检测; // //Thread.Sleep(100); // } // break; // case ESpaceFlowStep.取料真空检测: // if (IoManager.Instance.ReadIn($"吸嘴{NozzleIndex}真空信号") == 1 || GlobalVar.Instance.RunSpace) // { // FetchNum = 0; // if (bFetchBack == false) // { // GlobalTray.InputTray2.ChangeStatus(CurFetchSlotIndex, ESlotStatus.NotHave); // } // else // { // GlobalTray.OkTray2.ChangeStatus(CurFetchSlotIndex, ESlotStatus.NotHave); // } // if (NozzleIndex == 1) // { // NozzleIndex++; // flowStep = ESpaceFlowStep.到取料位上方; // } // else if (NozzleIndex == 2) // { // NozzleIndex = 1; // flowStep = ESpaceFlowStep.到下相机拍照起始位; // } // CurFetchSlotIndex++; // } // else // { // FetchNum++; // if (FetchNum == 6) // { // Msg.ShowError($"吸嘴{NozzleIndex}取料{FetchNum}次失败报警,请处理后点击确定"); // FetchNum = 0; // } // flowStep = ESpaceFlowStep.到取料位下方; // } // break; // case ESpaceFlowStep.到下相机拍照起始位: // if (XYCanMove()) // { // ImageProcess.ClearAutoTrigger(); // HikCamera.Instance.SetExposure("downCamera", 40); // HikCamera.Instance.SetGain("downCamera", 20); // AxisControl.LoadX.MovePos(GlobalVar.Instance.Nozzle1CameraPosX - 10, GlobalVar.Instance.WholeSpeed); // AxisControl.LoadY.MovePos(GlobalVar.Instance.Nozzle1CameraPosY, GlobalVar.Instance.WholeSpeed); // flowStep = ESpaceFlowStep.等待到下相机拍照起始位; // } // break; // case ESpaceFlowStep.等待到下相机拍照起始位: // AxisControl.LoadX.IsStop(out stop[0]); // AxisControl.LoadY.IsStop(out stop[1]); // if (stop[0] && stop[1]) // { // flowStep = ESpaceFlowStep.到下相机拍照结束位; // Thread.Sleep(100); // HikCamera.Instance.SetTrigger(GlobalVar.Instance.DownCameraName, ETriggerMode.Auto); // AxisControl.LoadX.SetPosCompare(1, new double[] { GlobalVar.Instance.Nozzle1CameraPosX, GlobalVar.Instance.Nozzle2CameraPosX }); // } // break; // case ESpaceFlowStep.到下相机拍照结束位: // if (XYCanMove()) // { // IoManager.Instance.WriteOut("下相机光源", 1); // Thread.Sleep(50); // AxisControl.LoadX.MovePos(GlobalVar.Instance.Nozzle2CameraPosX + 10, GlobalVar.Instance.FlyCameraSpeed); // flowStep = ESpaceFlowStep.等待到下相机拍照结束位; // } // break; // case ESpaceFlowStep.等待到下相机拍照结束位: // AxisControl.LoadX.IsStop(out stop[0]); // Stopwatch stopwatch = new Stopwatch(); // if (stop[0]) // { // AxisControl.LoadX.ClearCompare(1); // IoManager.Instance.WriteOut("下相机光源", 0); // flowStep = ESpaceFlowStep.等待相机拍照完成; // stopwatch.Start(); // } // break; // case ESpaceFlowStep.等待相机拍照完成: // imgs = ImageProcess.GetAutoImage(); // if (imgs != null && imgs.Length == 2) // { // CameraOkEvent?.Invoke(imgs, null); // if (GlobalVar.Instance.RunSpace) // { // flowStep = ESpaceFlowStep.到放料位上方; // mrs = new List(); // mrs.Add(new MatchResult()); // mrs.Add(new MatchResult()); // } // else // { // //拍照完成 // flowStep = ESpaceFlowStep.等待视觉处理结果; // } // } // break; // case ESpaceFlowStep.等待视觉处理结果: // mrs = VisionProcess.Instance.MatchDownCam(imgs); // if (mrs != null && mrs.Count == 2) // { // CameraOkEvent?.Invoke(imgs, mrs); // flowStep = ESpaceFlowStep.到放料位上方; // //Thread.Sleep(2000); // } // else // { // flowStep = ESpaceFlowStep.到下相机拍照起始位; // } // break; // case ESpaceFlowStep.到放料位上方: // if (XYCanMove()) // { // targetX = 0.0; // targetY = 0.0; // if (bFetchBack) // { // tray = GlobalTray.InputTray2.GetSlot(CurAssembleSlotInex); // } // else // { // tray = GlobalTray.OkTray2.GetSlot(CurAssembleSlotInex); // } // if (NozzleIndex == 1) // { // targetX = (mrs[0].OffsetCol) + tray.Position.X + (GlobalVar.Instance.Nozzle1Center2CameraCenterDistX * -1); // targetY = (mrs[0].OffsetRow * -1) + tray.Position.Y - GlobalVar.Instance.Nozzle1Center2CameraCenterDistY; // AxisControl.LoadX.MovePos(targetX, GlobalVar.Instance.WholeSpeed); // AxisControl.LoadY.MovePos(targetY, GlobalVar.Instance.WholeSpeed); // AxisControl.VacR1.MoveOffset(AngleTool.Rad2Deg((mrs[0].Angle)), GlobalVar.Instance.WholeSpeed); // } // else if (NozzleIndex == 2) // { // targetX = (mrs[1].OffsetCol) + tray.Position.X + (GlobalVar.Instance.Nozzle1Center2CameraCenterDistX * -1) + GlobalVar.Instance.Nozzle2Center2Nozzle1CenterDistX; // targetY = (mrs[1].OffsetRow * -1) + tray.Position.Y - GlobalVar.Instance.Nozzle1Center2CameraCenterDistY + GlobalVar.Instance.Nozzle2Center2Nozzle1CenterDistY; // AxisControl.LoadX.MovePos(targetX, GlobalVar.Instance.WholeSpeed); // AxisControl.LoadY.MovePos(targetY, 1); // AxisControl.VacR2.MoveOffset(AngleTool.Rad2Deg(mrs[1].Angle), GlobalVar.Instance.WholeSpeed); // } // flowStep = ESpaceFlowStep.等待到放料位上方; // } // break; // case ESpaceFlowStep.等待到放料位上方: // AxisControl.LoadX.IsStop(out stop[0]); // AxisControl.LoadY.IsStop(out stop[1]); // AxisControl.VacR1.IsStop(out stop[2]); // AxisControl.VacR2.IsStop(out stop[3]); // if (stop[0] && stop[1] && stop[2] && stop[3]) // { // Thread.Sleep(100); // flowStep = ESpaceFlowStep.到放料位下方; // } // break; // case ESpaceFlowStep.到放料位下方: // if (NozzleIndex == 1) // { // AxisControl.VacZ1.MovePos(GlobalVar.Instance.Vac1Tray1FetchHeight + 1, GlobalVar.Instance.WholeSpeed); // } // else // { // AxisControl.VacZ2.MovePos(GlobalVar.Instance.Vac2Tray1FetchHeight + 1, GlobalVar.Instance.WholeSpeed); // } // flowStep = ESpaceFlowStep.等待到放料位下方; // break; // case ESpaceFlowStep.等待到放料位下方: // AxisControl.VacZ1.IsStop(out stop[0]); // AxisControl.VacZ2.IsStop(out stop[1]); // if (stop[0] && stop[1]) // { // IoManager.Instance.WriteOut($"吸嘴{NozzleIndex}真空吸", 0); // Thread.Sleep(50); // IoManager.Instance.WriteOut($"吸嘴{NozzleIndex}真空破", 1); // Thread.Sleep(100); // IoManager.Instance.WriteOut($"吸嘴{NozzleIndex}真空破", 0); // flowStep = ESpaceFlowStep.放料完成抬起; // } // break; // case ESpaceFlowStep.放料完成抬起: // if (NozzleIndex == 1) // { // AxisControl.VacZ1.MovePos(0, GlobalVar.Instance.WholeSpeed); // } // else // { // AxisControl.VacZ2.MovePos(0, GlobalVar.Instance.WholeSpeed); // } // flowStep = ESpaceFlowStep.等待放料完成抬起; // break; // case ESpaceFlowStep.等待放料完成抬起: // AxisControl.VacZ1.IsStop(out stop[0]); // AxisControl.VacZ2.IsStop(out stop[1]); // if (stop[0] && stop[1]) // { // Thread.Sleep(100); // if (bFetchBack) // { // GlobalTray.InputTray2.ChangeStatus(CurAssembleSlotInex, ESlotStatus.Have); // } // else // { // GlobalTray.OkTray2.ChangeStatus(CurAssembleSlotInex, ESlotStatus.Have); // } // if (NozzleIndex == 1) // { // NozzleIndex++; // flowStep = ESpaceFlowStep.到放料位上方; // } // else if (NozzleIndex == 2) // { // NozzleIndex = 1; // flowStep = ESpaceFlowStep.到取料位上方; // } // CurAssembleSlotInex++; // if (CurAssembleSlotInex == 261) // { // CurAssembleSlotInex = 1; // CurFetchSlotIndex = 1; // //if(bFetchBack) // //{ // // GlobalTray.InputTray2.Fill(); // // GlobalTray.OkTray2.Clear(); // //} // //else // //{ // // GlobalTray.InputTray2.Fill(); // // GlobalTray.OkTray2.Clear(); // //} // bFetchBack = !bFetchBack; // } // } // break; // default: // break; //} } private double GetVacOffsetHeight(int fetchNum) { switch (fetchNum) { case 0: return 0; case 1: return -0.1; case 2: return -0.2; case 3: return -0.3; case 4: return 0.1; case 5: return 0.2; case 6: return 0.3; default: return 0; } } private bool XYCanMove() { //AxisControl.VacZ1.GetOrgStatus(out bool bOrgZ1); //AxisControl.VacZ2.GetOrgStatus(out bool bOrgZ2); //if (AxisControl.LoadX.HomeStatus == EHomeStatus.Finished // && AxisControl.LoadY.HomeStatus == EHomeStatus.Finished // && bOrgZ1 && bOrgZ2) //{ // return true; //} return false; } } }