From 4e8bf084f8a04617a9f542099183b8d829fa7c4b Mon Sep 17 00:00:00 2001
From: patrick <patrick.xu@broconcentric.com>
Date: 星期五, 18 十月 2019 19:07:51 +0800
Subject: [PATCH] 1. 修改机器人通信协议及相关流程触发和操作
---
src/A032.Process/ProcessControl_Method.cs | 1064 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 files changed, 1,053 insertions(+), 11 deletions(-)
diff --git a/src/A032.Process/ProcessControl_Method.cs b/src/A032.Process/ProcessControl_Method.cs
index 1ff4d8d..61fb662 100644
--- a/src/A032.Process/ProcessControl_Method.cs
+++ b/src/A032.Process/ProcessControl_Method.cs
@@ -1,7 +1,10 @@
-锘縰sing Bro.Common.Helper;
+锘縰sing Bro.Common.Base;
+using Bro.Common.Helper;
using Bro.Common.Interface;
using Bro.Common.Model;
+using Bro.Device.AuboRobot;
using Bro.Device.HikCamera;
+using Bro.Device.SeerAGV;
using HalconDotNet;
using System;
using System.Collections.Generic;
@@ -14,24 +17,1063 @@
namespace A032.Process
{
- public enum ResultState
- {
- OK = 1,
- NG = -1,
- Undetermined = -2,
- }
-
public partial class ProcessControl
{
+ Dictionary<int, int> machineFullTrayDict = new Dictionary<int, int>();
+ Dictionary<int, int> machineEmptyTrayDict = new Dictionary<int, int>();
+ List<TaskAssignInfo> taskAssignedList = new List<TaskAssignInfo>();
+
+ private void OnAGVTaskStatusChanged(SeerAGVDriver agv, AGVTaskStatus taskStatus)
+ {
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.AGVId == agv.Id);
+
+ if (bind == null)
+ {
+ throw new ProcessException("鏈兘鏍规嵁AGV淇℃伅鑾峰彇缁戝畾璁惧淇℃伅", null);
+ }
+
+ if (bind.AGVDest == agv.CurrentPosition && taskStatus == AGVTaskStatus.Completed)
+ {
+ //PathPosition loadEmptyTrayPosition = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadEmptyTray);
+
+ //if (bind.AGVDest == loadEmptyTrayPosition.PositionCode)
+ //{
+ // bind.RobotStatus = TaskStatus.Running;
+ //}
+
+ bind.AGVStatus = TaskStatus.Available;
+ }
+ }
+
+ private void OnAGVPositionChanged(SeerAGVDriver agv, string positionCode)
+ {
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.AGVId == agv.Id);
+
+ if (bind == null)
+ {
+ throw new ProcessException("鏈兘鏍规嵁AGV淇℃伅鑾峰彇缁戝畾璁惧淇℃伅", null);
+ }
+
+ if (bind.AGVDest == positionCode && agv.TaskStatus == AGVTaskStatus.Completed)
+ {
+ //PathPosition loadEmptyTrayPosition = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadEmptyTray);
+
+ //if (bind.AGVDest == loadEmptyTrayPosition.PositionCode)
+ //{
+ // bind.RobotStatus = TaskStatus.Running;
+ //}
+
+ bind.AGVStatus = TaskStatus.Available;
+ }
+ }
+
+ private void OnRobotMsgReceived(DateTime dt, AuboRobotDriver robot, RobotMsg msg)
+ {
+ LogAsync(dt, robot.Name + "鎺ユ敹淇℃伅", msg.GetDisplayText());
+
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == robot.Id);
+
+ if (bind == null)
+ {
+ throw new ProcessException("鏈兘鏍规嵁鏈哄櫒浜轰俊鎭幏鍙栫粦瀹氳澶囦俊鎭�", null);
+ }
+
+ List<AGVTaskModel> models = new List<AGVTaskModel>();
+
+ switch (msg.Action)
+ {
+ case RobotMsgAction.Load:
+ {
+ switch (msg.Para1)
+ {
+ case RobotMsgParas.EmptyTray:
+ {
+ bind.RobotStatus = bind.AGVStatus = TaskStatus.Available;
+ }
+ break;
+ case RobotMsgParas.FullTray:
+ {
+ machineFullTrayDict[msg.Para2]--;
+ bind.CurrentFullTray = int.Parse(msg.Datas[1]);
+
+ if (machineFullTrayDict[msg.Para2] > 0 && !bind.IsFullTrayFull)
+ {
+ RobotMsg_UnloadEmptyTray.Para2 = msg.Para2;
+ robot.SendMsg(RobotMsg_UnloadEmptyTray, true);
+ }
+ else
+ {
+ bind.RobotStatus = TaskStatus.Available;
+ }
+ }
+ break;
+ default:
+ break;
+ }
+ }
+ break;
+ case RobotMsgAction.Unload:
+ {
+ switch (msg.Para1)
+ {
+ case RobotMsgParas.EmptyTray:
+ {
+ machineEmptyTrayDict[msg.Para2]++;
+ bind.CurrentEmptyTray = int.Parse(msg.Datas[0]);
+
+ if (machineEmptyTrayDict[msg.Para2] < Config.Machine_EmptyTrayNum)
+ {
+ bind.RobotIOHandle.Reset();
+ bind.RobotIOHandle.WaitOne();
+
+ if (!bind.IsFullTrayFull)
+ {
+ RobotMsg_UnloadEmptyTray.Para2 = msg.Para2;
+ robot.SendMsg(RobotMsg_UnloadEmptyTray, true);
+ }
+ }
+ else
+ {
+ bind.RobotStatus = TaskStatus.Available;
+ }
+ }
+ break;
+ case RobotMsgParas.FullTray:
+ {
+ bind.CurrentFullTray = int.Parse(msg.Datas[1]);
+
+ bind.RobotIOHandle.Reset();
+ bind.RobotIOHandle.WaitOne();
+
+ if (!bind.IsFullTrayEmpty)
+ {
+ Camera_UnloadFullTray(robot.Id, msg.Para2);
+ }
+ }
+ break;
+ default:
+ break;
+ }
+ }
+ break;
+ case RobotMsgAction.Move:
+ {
+ switch (msg.Para1)
+ {
+ case RobotMsgParas.LineSnap:
+ {
+ Camera_UnloadFullTray(robot.Id, msg.Para2);
+ }
+ break;
+ case RobotMsgParas.LoadFullTraySnap:
+ {
+ Camera_LoadFullTray(robot.Id, msg.Para2);
+ }
+ break;
+ case RobotMsgParas.UnloadEmptyTraySnap:
+ {
+ Camera_UnloadEmptyTray(robot.Id, msg.Para2);
+ }
+ break;
+ case RobotMsgParas.Home:
+ {
+ bind.RobotStatus = TaskStatus.Available;
+ }
+ break;
+ default:
+ break;
+ }
+ }
+ break;
+ case RobotMsgAction.Calibration:
+ {
+
+ }
+ break;
+ case RobotMsgAction.StandardPoint:
+ {
+
+ }
+ break;
+ default:
+ break;
+ }
+
+ if (models.Count > 0)
+ {
+ models.ForEach(model =>
+ {
+ if (!bind.TaskList.Any(t => t.MethodFunc.Method.Name == model.MethodFunc.Method.Name))
+ {
+ model.OpConfig = new AGVBindOpConfig(bind.Id);
+ bind.TaskList.Add(model);
+ }
+ });
+ }
+ }
+
public void QueryRobotIO()
{
-
+ RobotDict.Values.ToList().ForEach(r =>
+ {
+ r.SendMsg(RobotMsgAction.IO, RobotMsgParas.Query, 0);
+ });
}
- [ProcessMethod("HikCamera", "RobotCorrection", "鎷嶆憚锛岀‘璁ゆ満鍣ㄤ汉璋冩暣浣嶇疆", true)]
- public ProcessResponse RobotCorrection(IOperationConfig config)
+ private void OnBindUnitTaskInvoke(AGVBindUnit bind)
{
+ var task = bind.TaskList[0];
+ var response = task.MethodFunc.Invoke(task.OpConfig, task.Device);
+ }
+
+ #region Robot鐩戝惉浜嬩欢
+ private void AddNewTaskToBind(string robotId, List<AGVTaskModel> models)
+ {
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == robotId);
+
+ if (bind == null)
+ {
+ throw new ProcessException("鏈兘鏍规嵁鏈哄櫒浜轰俊鎭幏鍙栫粦瀹氳澶囦俊鎭�", null);
+ }
+
+ AddNewTaskToBind(bind, models);
+ }
+
+ private void AddNewTaskToBind(AGVBindUnit bind, List<AGVTaskModel> models)
+ {
+ if (models.Count > 0)
+ {
+ models.ForEach(model =>
+ {
+ if (!bind.TaskList.Any(t => t.MethodFunc.Method.Name == model.MethodFunc.Method.Name))
+ {
+ model.OpConfig = new AGVBindOpConfig(bind.Id);
+ bind.TaskList.Add(model);
+ }
+ });
+ }
+ }
+
+ [ProcessMethod("", "Robot_Monitor_Alarm", "鏈哄櫒浜虹洃鍚簨浠�-鎶ヨ", true)]
+ public ProcessResponse Robot_Monitor_Alarm(IOperationConfig config, IDevice device)
+ {
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == device.Id);
+
+ if (bind == null)
+ {
+ throw new ProcessException("鏈兘鏍规嵁鏈哄櫒浜轰俊鎭幏鍙栫粦瀹氳澶囦俊鎭�", null);
+ }
+
+ bind.AGV.PauseTask();
+ bind.RobotStatus = TaskStatus.Warning;
+
return new ProcessResponse(true);
}
+
+ [ProcessMethod("", "Robot_Monitor_EmptyTrayEmpty", "鏈哄櫒浜虹洃鍚簨浠�-绌篢ray鍖哄煙娓呯┖", true)]
+ public ProcessResponse Robot_Monitor_EmptyTrayEmpty(IOperationConfig config, IDevice device)
+ {
+ bool isEmptyTrayEmpty = config.InputPara[0] == 1;
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == device.Id);
+ if (isEmptyTrayEmpty)
+ {
+ bind.IsEmptyTrayEmpty = true;
+
+ Task.Run(() =>
+ {
+ Func<IOperationConfig, IDevice, ProcessResponse> action = AGV_LoadEmptyTray;
+ while (bind.IsEmptyTrayEmpty && bind.TaskList.Count == 0 && !bind.TaskList.Any(u => u.MethodFunc.Method.Name == action.Method.Name))
+ {
+ if (bind.TaskList.Count == 0)
+ {
+ List<AGVTaskModel> models = new List<AGVTaskModel>();
+ models.Add(new AGVTaskModel(TaskAvailableLevel.Both, AGV_LoadEmptyTray));
+ models.Add(new AGVTaskModel(TaskAvailableLevel.AGV, AfterEmptyTrayPositionArrived));
+
+ AddNewTaskToBind(device.Id, models);
+ }
+ else
+ {
+ Thread.Sleep(500);
+ }
+ }
+ });
+
+ }
+ else
+ {
+ bind.IsEmptyTrayEmpty = false;
+ }
+
+ return new ProcessResponse(true);
+ }
+
+ [ProcessMethod("", "Robot_Monitor_EmptyTrayEmpty", "鏈哄櫒浜虹洃鍚簨浠�-婊ray鍖哄煙鏀炬弧", true)]
+ public ProcessResponse Robot_Monitor_FullTrayFull(IOperationConfig config, IDevice device)
+ {
+ bool isFullTrayFull = config.InputPara[0] == 1;
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == device.Id);
+ if (isFullTrayFull)
+ {
+ bind.IsFullTrayFull = true;
+
+ Task.Run(() =>
+ {
+ Func<IOperationConfig, IDevice, ProcessResponse> action = AGV_UnloadFullTray;
+ while (bind.IsFullTrayFull && !bind.TaskList.Any(u => u.MethodFunc.Method.Name == action.Method.Name))
+ {
+ if (bind.TaskList.Count == 0)
+ {
+ List<AGVTaskModel> models = new List<AGVTaskModel>();
+ models.Add(new AGVTaskModel(TaskAvailableLevel.Both, AGV_UnloadFullTray));
+ models.Add(new AGVTaskModel(TaskAvailableLevel.AGV, Robot_UnloadFullTray));
+
+ AddNewTaskToBind(device.Id, models);
+ }
+ else
+ {
+ Thread.Sleep(500);
+ }
+ }
+ });
+ }
+ else
+ {
+ bind.IsFullTrayFull = false;
+ }
+
+ bind.RobotIOHandle.Set();
+
+ return new ProcessResponse(true);
+ }
+
+ [ProcessMethod("", "Robot_Monitor_FullTrayEmpty", "鏈哄櫒浜虹洃鍚簨浠�-婊ray鍖哄煙娓呯┖", true)]
+ public ProcessResponse Robot_Monitor_FullTrayEmpty(IOperationConfig config, IDevice device)
+ {
+ bool isFullTrayEmpty = config.InputPara[0] == 1;
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == device.Id);
+ bind.IsFullTrayEmpty = isFullTrayEmpty;
+
+ bind.RobotIOHandle.Set();
+
+ return new ProcessResponse(true);
+ }
+
+ //[ProcessMethod("", "Robot_Monitor_LoadEmptyTrayReady", "鏈哄櫒浜虹洃鍚簨浠�-杞藉叆绌篢ray灏辩华", true)]
+ //public ProcessResponse Robot_Monitor_LoadEmptyTrayReady(IOperationConfig config, IDevice device)
+ //{
+ // var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == device.Id);
+
+ // if (bind == null)
+ // {
+ // throw new ProcessException("鏈兘鏍规嵁鏈哄櫒浜轰俊鎭幏鍙栫粦瀹氳澶囦俊鎭�", null);
+ // }
+
+ // PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadEmptyTray);
+ // if (bind.AGV.CurrentPosition != position.PositionCode)
+ // {
+ // new ProcessException("涓婄┖Tray瀹屾垚淇″彿浠呭湪涓婄┖Tray鍦扮偣鏈夋晥", null);
+ // return new ProcessResponse(true);
+ // }
+
+ // List<AGVTaskModel> models = new List<AGVTaskModel>();
+ // models.Add(new AGVTaskModel(TaskAvailableLevel.AGV, EmptyTrayReady));
+ // AddNewTaskToBind(bind, models);
+
+ // return new ProcessResponse(true);
+ //}
+
+ [ProcessMethod("", "Robot_Monitor_Reset", "鏈哄櫒浜虹洃鍚簨浠�-Reset澶嶄綅", true)]
+ public ProcessResponse Robot_Monitor_Reset(IOperationConfig config, IDevice device)
+ {
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == device.Id);
+
+ if (bind == null)
+ {
+ throw new ProcessException("鏈兘鏍规嵁鏈哄櫒浜轰俊鎭幏鍙栫粦瀹氳澶囦俊鎭�", null);
+ }
+ bind.AGV.CancelTask();
+
+ //isEmptyTrayTaskAssigned = false;
+ //isFullTrayTaskAssigned = false;
+
+ taskAssignedList.RemoveAll(u => u.AgvId == device.Id);
+
+ bind.TaskList.Clear();
+ bind.RobotStatus = bind.AGVStatus = TaskStatus.Available;
+
+ return new ProcessResponse(true);
+ }
+ #endregion
+
+ #region 绌篢ray涓婃枡
+ [ProcessMethod("", "AGV_LoadEmptyTray", "AGV鍘诲線绌篢ray涓婃枡", true)]
+ public ProcessResponse AGV_LoadEmptyTray(IOperationConfig config, IDevice device)
+ {
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
+ PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadEmptyTray);
+
+ if (position == null)
+ {
+ throw new ProcessException("璺緞閰嶇疆鏈缃┖Tray涓婃枡鐐�");
+ }
+
+ bind.AGVDest = position.PositionCode;
+ bind.AGV.TaskOrder(position.PositionCode);
+
+ bind.AGVStatus = TaskStatus.Running;
+
+ return new ProcessResponse(true);
+ }
+
+ [ProcessMethod("", "AfterEmptyTrayPositionArrived", "鍒拌揪绌篢ray涓婃枡鐐�", true)]
+ public ProcessResponse AfterEmptyTrayPositionArrived(IOperationConfig config, IDevice device)
+ {
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
+ PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadEmptyTray);
+
+ if (position == null)
+ {
+ throw new ProcessException("璺緞閰嶇疆鏈缃┖Tray涓婃枡鐐�", null);
+ }
+
+ if (bind.AGV.CurrentPosition != position.PositionCode)
+ {
+ throw new ProcessException("AGV灏氭湭鍒拌揪绌篢ray涓婃枡鐐�", null);
+ }
+
+ bind.Robot.SendMsg(RobotMsgAction.Load, RobotMsgParas.EmptyTray, 0);
+ bind.RobotStatus = TaskStatus.Running;
+
+ return new ProcessResponse(true);
+ }
+
+ //[ProcessMethod("", "EmptyTrayReady", "绌篢ray涓婃枡瀹屾垚", true)]
+ //public ProcessResponse EmptyTrayReady(IOperationConfig config, IDevice device)
+ //{
+ // var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
+ // bind.RobotStatus = bind.AGVStatus = TaskStatus.Available;
+
+ // return new ProcessResponse(true);
+ //}
+ #endregion
+
+ #region 绌篢ray寰�鏈哄彴涓嬫枡
+ //bool isEmptyTrayNeed = false;
+ //bool isEmptyTrayTaskAssigned = false;
+ RobotMsg RobotMsg_UnloadEmptyTray = new RobotMsg();
+
+ [ProcessMethod("", "PLC_NoticeEmptyTray", "PLC閫氱煡闇�瑕佷笂绌篢ray", true)]
+ public ProcessResponse PLC_NoticeEmptyTray(IOperationConfig config, IDevice device)
+ {
+ if (config.InputPara == null || config.InputPara.Count == 0)
+ {
+ throw new ProcessException("涓婄┖Tray鏂规硶鏈厤缃緭鍏ュ弬鏁�", null);
+ }
+
+ bool isEmptyTrayNeed = config.InputPara[0] == 1;
+
+ if (isEmptyTrayNeed)
+ {
+ var position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.UnloadEmptyTray && u.DeviceOwner == device.Id);
+
+ machineEmptyTrayDict[position.PositionNo] = 0;
+
+ if (!taskAssignedList.Any(u => u.PositionNo == position.PositionNo))
+ {
+ taskAssignedList.Add(new TaskAssignInfo()
+ {
+ IsTaskNeed = true,
+ IsTaskAssgined = false,
+ PositionNo = position.PositionNo,
+ });
+ }
+
+ CheckEmptyTrayTask(position.PositionNo);
+ }
+
+ return new ProcessResponse(true);
+ }
+
+ private async void CheckEmptyTrayTask(int positionNo)
+ {
+ await Task.Run(() =>
+ {
+ var taskStatus = taskAssignedList.FirstOrDefault(u => u.PositionNo == positionNo);
+
+ if (taskStatus == null)
+ return;
+
+ while (taskStatus.IsTaskNeed && !taskStatus.IsTaskAssgined)
+ {
+ Func<IOperationConfig, IDevice, ProcessResponse> action = AGV_UnloadEmptyTray;
+
+ //if (!Config.AGVBindCollection.Any(b => b.TaskList.Any(t => t.MethodFunc.Method.Name == action.Method.Name)))
+ {
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.UnitStatus == TaskStatus.Available);
+ if (bind != null)
+ {
+ var position = Config.PositionCollection.FirstOrDefault(u => u.PositionNo == positionNo);
+ AGVTaskModel model_AGV = new AGVTaskModel(TaskAvailableLevel.Both, action, new AGVBindOpConfig(bind.Id, position));
+ AGVTaskModel model_Robot = new AGVTaskModel(TaskAvailableLevel.AGV, Robot_UnloadEmptyTray, new AGVBindOpConfig(bind.Id));
+
+ bind.TaskList.Add(model_AGV);
+ bind.TaskList.Add(model_Robot);
+
+ taskStatus.IsTaskAssgined = true;
+ taskStatus.AgvId = bind.AGVId;
+ }
+ }
+
+ Thread.Sleep(300);
+ }
+ });
+ }
+
+ //[ProcessMethod("", "AGV_UnloadEmptyTray", "AGV鍘诲線鍗歌浇绌篢ray鏂欎綅缃�", true)]
+ public ProcessResponse AGV_UnloadEmptyTray(IOperationConfig config, IDevice device)
+ {
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
+
+ PathPosition position = (config as AGVBindOpConfig).Position;
+
+ if (position == null)
+ {
+ throw new ProcessException("璺緞閰嶇疆鏈缃┖Tray涓嬫枡鐐�");
+ }
+
+ bind.AGVDest = position.PositionCode;
+ bind.AGV.TaskOrder(position.PositionCode);
+
+ bind.AGVStatus = TaskStatus.Running;
+
+ return new ProcessResponse(true);
+ }
+
+ //[ProcessMethod("", "Robot_UnloadEmptyTray", "鏈哄櫒浜鸿繍鍔ㄨ嚦绌篢ray鎷嶇収浣嶇疆", true)]
+ public ProcessResponse Robot_UnloadEmptyTray(IOperationConfig config, IDevice device)
+ {
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
+ PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.UnloadEmptyTray && u.PositionCode == bind.AGV.CurrentPosition);
+
+ if (position == null)
+ {
+ throw new ProcessException("璺緞閰嶇疆鏈缃┖Tray涓嬫枡鐐�");
+ }
+
+ taskAssignedList.RemoveAll(u => u.AgvId == bind.AGVId && u.PositionNo == position.PositionNo);
+
+ bind.RobotStatus = TaskStatus.Running;
+ bind.Robot.SendMsg(RobotMsgAction.Move, RobotMsgParas.UnloadEmptyTraySnap, position.PositionNo);
+
+ return new ProcessResponse(true);
+ }
+
+ //[ProcessMethod("", "Camera_UnloadEmptyTray", "鐩告満纭绌篢ray鍗歌浇鏈哄櫒浜轰綅缃皟鏁�", true)]
+ //public ProcessResponse Camera_UnloadEmptyTray(IOperationConfig config, IDevice device)
+ //{
+ // var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
+
+ // PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.UnloadEmptyTray);
+
+ // if (position == null)
+ // {
+ // throw new ProcessException("璺緞閰嶇疆鏈缃┖Tray涓嬫枡鐐�");
+ // }
+
+ // if (bind.AGV.CurrentPosition != position.PositionCode)
+ // {
+ // throw new ProcessException("AGV褰撳墠鏈浜庣┖Tray涓嬫枡鐐�");
+ // }
+
+ // PositionVisionConfig visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.PositionCode == position.PositionCode && u.CameraId == bind.CameraId);
+
+ // if (visionConfig == null)
+ // {
+ // throw new ProcessException("鏈厤缃鐩告満鐨勭┖Tray涓嬫枡鐐圭殑瑙嗚鎿嶄綔閰嶇疆");
+ // }
+
+ // float x = 0;
+ // float y = 0;
+ // float angle = 0;
+
+ // using (HObject hImage = CollectHImage(bind.Camera, visionConfig.CameraOpConfig, "Camera_UnloadEmptyTray"))
+ // {
+ // string toolPath = visionConfig.CameraOpConfig.AlgorithemPath;
+ // if (!_halconToolDict.ContainsKey(toolPath))
+ // {
+ // throw new ProcessException($"鏈厤缃瓹amera_UnloadEmptyTray鐨勮瑙夌畻娉曡矾寰�");
+ // }
+
+ // var tool = _halconToolDict[toolPath];
+ // tool.SetDictionary(new Dictionary<string, HTuple>() { { "OUTPUT_X", new HTuple() }, { "OUTPUT_Y", new HTuple() }, { "OUTPUT_Angle", new HTuple() } }, new Dictionary<string, HObject>() { { "INPUT_Image", hImage } });
+ // tool.RunProcedure();
+
+ // x = (float)tool.GetResultTuple("OUTPUT_X").D;
+ // y = (float)tool.GetResultTuple("OUTPUT_Y").D;
+ // angle = (float)tool.GetResultTuple("OUTPUT_Angle").D;
+ // }
+
+ // if (x <= 0 || y <= 0)
+ // {
+ // throw new ProcessException("Camera_UnloadEmptyTray瑙嗚璁＄畻鑾峰彇鐐逛綅涓嶅彲灏忎簬0");
+ // }
+
+ // float dx = visionConfig.StandardPoint.X - x;
+ // float dy = visionConfig.StandardPoint.Y - y;
+
+ // HOperatorSet.AffineTransPoint2d(new HTuple(visionConfig.Matrix[0], visionConfig.Matrix[1], 0, visionConfig.Matrix[3], visionConfig.Matrix[4], 0), dx, dy, out HTuple dx_Robot, out HTuple dy_Robot);
+
+ // bind.Robot.SendMsg(RobotMsgAction.Unload, RobotMsgParas.EmptyTray, position.PositionNo, new List<float>() { (float)dx_Robot.D, (float)dy_Robot.D, angle });
+
+ // return new ProcessResponse(true);
+ //}
+
+ public ProcessResponse Camera_UnloadEmptyTray(string robotId, int positionNum)
+ {
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == robotId);
+
+ PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.PositionNo == positionNum);
+
+ if (position == null)
+ {
+ throw new ProcessException("璺緞閰嶇疆鏈缃┖Tray涓嬫枡鐐�");
+ }
+
+ if (bind.AGV.CurrentPosition != position.PositionCode)
+ {
+ throw new ProcessException("AGV褰撳墠鏈浜庣┖Tray涓嬫枡鐐�");
+ }
+
+ PositionVisionConfig visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.PositionCode == position.PositionCode && u.CameraId == bind.CameraId);
+
+ if (visionConfig == null)
+ {
+ throw new ProcessException("鏈厤缃鐩告満鐨勭┖Tray涓嬫枡鐐圭殑瑙嗚鎿嶄綔閰嶇疆");
+ }
+
+ float x = 0;
+ float y = 0;
+ float angle = 0;
+
+ using (HObject hImage = CollectHImage(bind.Camera, visionConfig.CameraOpConfig, "Camera_UnloadEmptyTray"))
+ {
+ string toolPath = visionConfig.CameraOpConfig.AlgorithemPath;
+ if (!_halconToolDict.ContainsKey(toolPath))
+ {
+ throw new ProcessException($"鏈厤缃瓹amera_UnloadEmptyTray鐨勮瑙夌畻娉曡矾寰�");
+ }
+
+ var tool = _halconToolDict[toolPath];
+ tool.SetDictionary(new Dictionary<string, HTuple>() { { "OUTPUT_X", new HTuple() }, { "OUTPUT_Y", new HTuple() }, { "OUTPUT_Angle", new HTuple() } }, new Dictionary<string, HObject>() { { "INPUT_Image", hImage } });
+ tool.RunProcedure();
+
+ x = (float)tool.GetResultTuple("OUTPUT_X").D;
+ y = (float)tool.GetResultTuple("OUTPUT_Y").D;
+ angle = (float)tool.GetResultTuple("OUTPUT_Angle").D;
+ }
+
+ if (x <= 0 || y <= 0)
+ {
+ throw new ProcessException("Camera_UnloadEmptyTray瑙嗚璁＄畻鑾峰彇鐐逛綅涓嶅彲灏忎簬0");
+ }
+
+ float dx = visionConfig.StandardPoint.X - x;
+ float dy = visionConfig.StandardPoint.Y - y;
+
+ HOperatorSet.AffineTransPoint2d(new HTuple(visionConfig.Matrix[0], visionConfig.Matrix[1], 0, visionConfig.Matrix[3], visionConfig.Matrix[4], 0), dx, dy, out HTuple dx_Robot, out HTuple dy_Robot);
+
+ //bind.Robot.SendMsg(RobotMsgAction.Unload, RobotMsgParas.EmptyTray, position.PositionNo, new List<float>() { (float)dx_Robot.D, (float)dy_Robot.D, angle });
+ RobotMsg_UnloadEmptyTray.Action = RobotMsgAction.Unload;
+ RobotMsg_UnloadEmptyTray.Para1 = RobotMsgParas.EmptyTray;
+ RobotMsg_UnloadEmptyTray.Para2 = position.PositionNo;
+ RobotMsg_UnloadEmptyTray.Datas = new List<float>() { (float)dx_Robot.D, (float)dy_Robot.D, angle }.ConvertAll(s => s.ToString()).ToList();
+ bind.Robot.SendMsg(RobotMsg_UnloadEmptyTray, true);
+
+ return new ProcessResponse(true);
+ }
+ #endregion
+
+ #region 浠庢満鍙颁笂婊ray
+ //bool isFullTrayNeed = false;
+ //bool isFullTrayTaskAssigned = false;
+ RobotMsg RobotMsg_LoadFullTray = new RobotMsg();
+
+ [ProcessMethod("", "PLC_NoticeFullTray", "PLC閫氱煡婊ray闇�瑕佸彇璧�", true)]
+ public ProcessResponse PLC_NoticeFullTray(IOperationConfig config, IDevice device)
+ {
+ if (config.InputPara == null || config.InputPara.Count == 0)
+ {
+ throw new ProcessException("涓婄┖Tray鏂规硶鏈厤缃緭鍏ュ弬鏁�", null);
+ }
+
+ bool isFullTrayNeed = config.InputPara[0] == 1;
+ if (isFullTrayNeed)
+ {
+ var position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadFullTray && u.DeviceOwner == device.Id);
+
+ machineFullTrayDict[position.PositionNo] = Config.Machine_FullTrayNum;
+
+ if (!taskAssignedList.Any(u => u.PositionNo == position.PositionNo))
+ {
+ taskAssignedList.Add(new TaskAssignInfo()
+ {
+ IsTaskNeed = true,
+ IsTaskAssgined = false,
+ PositionNo = position.PositionNo,
+ });
+ }
+
+ CheckFullTrayTask(position.PositionNo);
+ }
+
+ return new ProcessResponse(true);
+ }
+
+ private async void CheckFullTrayTask(int positionNo)
+ {
+ await Task.Run(() =>
+ {
+ var taskStatus = taskAssignedList.FirstOrDefault(u => u.PositionNo == positionNo);
+
+ if (taskStatus == null)
+ return;
+
+ while (taskStatus.IsTaskNeed && !taskStatus.IsTaskAssgined)
+ {
+ Func<IOperationConfig, IDevice, ProcessResponse> action = AGV_LoadFullTray;
+
+ //if (!Config.AGVBindCollection.Any(b => b.TaskList.Any(t => t.MethodFunc.Method.Name == action.Method.Name)))
+ {
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.UnitStatus == TaskStatus.Available);
+ if (bind != null)
+ {
+ var position = Config.PositionCollection.FirstOrDefault(u => u.PositionNo == positionNo);
+ AGVTaskModel model_AGV = new AGVTaskModel(TaskAvailableLevel.Both, action, new AGVBindOpConfig(bind.Id, position));
+ AGVTaskModel model_Robot = new AGVTaskModel(TaskAvailableLevel.AGV, Robot_LoadFullTray, new AGVBindOpConfig(bind.Id));
+
+ bind.TaskList.Add(model_AGV);
+ bind.TaskList.Add(model_Robot);
+
+ taskStatus.IsTaskAssgined = true;
+ taskStatus.AgvId = bind.AGVId;
+ }
+ }
+
+ Thread.Sleep(300);
+ }
+ });
+ }
+
+ [ProcessMethod("", "AGV_LoadFullTray", "AGV鍘诲線婊ray涓婃枡浣嶇疆", true)]
+ public ProcessResponse AGV_LoadFullTray(IOperationConfig config, IDevice device)
+ {
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
+
+ PathPosition position = (config as AGVBindOpConfig).Position;
+
+ if (position == null)
+ {
+ throw new ProcessException("璺緞閰嶇疆鏈缃弧Tray涓婃枡鐐�");
+ }
+
+ bind.AGVDest = position.PositionCode;
+ bind.AGV.TaskOrder(position.PositionCode);
+
+ bind.AGVStatus = TaskStatus.Running;
+
+ return new ProcessResponse(true);
+ }
+
+ [ProcessMethod("", "Robot_LoadFullTray", "鏈哄櫒浜鸿繍鍔ㄨ嚦婊ray鎷嶇収浣嶇疆", true)]
+ public ProcessResponse Robot_LoadFullTray(IOperationConfig config, IDevice device)
+ {
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
+ PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadFullTray && u.PositionCode == bind.AGV.CurrentPosition);
+
+ if (position == null)
+ {
+ throw new ProcessException("璺緞閰嶇疆鏈缃弧Tray涓婃枡鐐�");
+ }
+
+ //if (bind.AGV.CurrentPosition != position.PositionCode)
+ //{
+ // throw new ProcessException("AGV褰撳墠鏈浜庢弧Tray涓婃枡鐐�");
+ //}
+
+ bind.RobotStatus = TaskStatus.Running;
+ bind.Robot.SendMsg(RobotMsgAction.Move, RobotMsgParas.LoadFullTraySnap, position.PositionNo);
+
+ return new ProcessResponse(true);
+ }
+
+ //[ProcessMethod("", "Camera_LoadFullTray", "鐩告満纭婊ray涓婃枡鏈哄櫒浜轰綅缃皟鏁�", true)]
+ //public ProcessResponse Camera_LoadFullTray(IOperationConfig config, IDevice device)
+ //{
+ // var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
+
+ // PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadFullTray);
+
+ // if (position == null)
+ // {
+ // throw new ProcessException("璺緞閰嶇疆鏈缃弧Tray涓婃枡鐐�");
+ // }
+
+ // if (bind.AGV.CurrentPosition != position.PositionCode)
+ // {
+ // throw new ProcessException("AGV褰撳墠鏈浜庢弧Tray涓婃枡鐐�");
+ // }
+
+ // PositionVisionConfig visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.PositionCode == position.PositionCode && u.CameraId == bind.CameraId);
+
+ // if (visionConfig == null)
+ // {
+ // throw new ProcessException("鏈厤缃鐩告満鐨勬弧Tray涓婃枡鐐圭殑瑙嗚鎿嶄綔閰嶇疆");
+ // }
+
+ // float x = 0;
+ // float y = 0;
+ // float angle = 0;
+
+ // using (HObject hImage = CollectHImage(bind.Camera, visionConfig.CameraOpConfig, "Camera_LoadFullTray"))
+ // {
+ // string toolPath = visionConfig.CameraOpConfig.AlgorithemPath;
+ // if (!_halconToolDict.ContainsKey(toolPath))
+ // {
+ // throw new ProcessException($"鏈厤缃瓹amera_LoadFullTray鐨勮瑙夌畻娉曡矾寰�");
+ // }
+
+ // var tool = _halconToolDict[toolPath];
+ // tool.SetDictionary(new Dictionary<string, HTuple>() { { "OUTPUT_X", new HTuple() }, { "OUTPUT_Y", new HTuple() }, { "OUTPUT_Angle", new HTuple() } }, new Dictionary<string, HObject>() { { "INPUT_Image", hImage } });
+ // tool.RunProcedure();
+
+ // x = (float)tool.GetResultTuple("OUTPUT_X").D;
+ // y = (float)tool.GetResultTuple("OUTPUT_Y").D;
+ // angle = (float)tool.GetResultTuple("OUTPUT_Angle").D;
+ // }
+
+ // if (x <= 0 || y <= 0)
+ // {
+ // throw new ProcessException("Camera_LoadFullTray瑙嗚璁＄畻鑾峰彇鐐逛綅涓嶅彲灏忎簬0");
+ // }
+
+ // float dx = visionConfig.StandardPoint.X - x;
+ // float dy = visionConfig.StandardPoint.Y - y;
+
+ // HOperatorSet.AffineTransPoint2d(new HTuple(visionConfig.Matrix[0], visionConfig.Matrix[1], 0, visionConfig.Matrix[3], visionConfig.Matrix[4], 0), dx, dy, out HTuple dx_Robot, out HTuple dy_Robot);
+
+ // bind.Robot.SendMsg(RobotMsgAction.Load, RobotMsgParas.FullTray, position.PositionNo, new List<float>() { (float)dx_Robot.D, (float)dy_Robot.D, angle });
+
+ // return new ProcessResponse(true);
+ //}
+
+ public ProcessResponse Camera_LoadFullTray(string robotId, int positionNum)
+ {
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == robotId);
+
+ PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.PositionNo == positionNum);
+
+ if (position == null)
+ {
+ throw new ProcessException("璺緞閰嶇疆鏈缃弧Tray涓婃枡鐐�");
+ }
+
+ if (bind.AGV.CurrentPosition != position.PositionCode)
+ {
+ throw new ProcessException("AGV褰撳墠鏈浜庢弧Tray涓婃枡鐐�");
+ }
+
+ PositionVisionConfig visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.PositionCode == position.PositionCode && u.CameraId == bind.CameraId);
+
+ if (visionConfig == null)
+ {
+ throw new ProcessException("鏈厤缃鐩告満鐨勬弧Tray涓婃枡鐐圭殑瑙嗚鎿嶄綔閰嶇疆");
+ }
+
+ float x = 0;
+ float y = 0;
+ float angle = 0;
+
+ using (HObject hImage = CollectHImage(bind.Camera, visionConfig.CameraOpConfig, "Camera_LoadFullTray"))
+ {
+ string toolPath = visionConfig.CameraOpConfig.AlgorithemPath;
+ if (!_halconToolDict.ContainsKey(toolPath))
+ {
+ throw new ProcessException($"鏈厤缃瓹amera_LoadFullTray鐨勮瑙夌畻娉曡矾寰�");
+ }
+
+ var tool = _halconToolDict[toolPath];
+ tool.SetDictionary(new Dictionary<string, HTuple>() { { "OUTPUT_X", new HTuple() }, { "OUTPUT_Y", new HTuple() }, { "OUTPUT_Angle", new HTuple() } }, new Dictionary<string, HObject>() { { "INPUT_Image", hImage } });
+ tool.RunProcedure();
+
+ x = (float)tool.GetResultTuple("OUTPUT_X").D;
+ y = (float)tool.GetResultTuple("OUTPUT_Y").D;
+ angle = (float)tool.GetResultTuple("OUTPUT_Angle").D;
+ }
+
+ if (x <= 0 || y <= 0)
+ {
+ throw new ProcessException("Camera_LoadFullTray瑙嗚璁＄畻鑾峰彇鐐逛綅涓嶅彲灏忎簬0");
+ }
+
+ float dx = visionConfig.StandardPoint.X - x;
+ float dy = visionConfig.StandardPoint.Y - y;
+
+ HOperatorSet.AffineTransPoint2d(new HTuple(visionConfig.Matrix[0], visionConfig.Matrix[1], 0, visionConfig.Matrix[3], visionConfig.Matrix[4], 0), dx, dy, out HTuple dx_Robot, out HTuple dy_Robot);
+
+ //bind.Robot.SendMsg(RobotMsgAction.Load, RobotMsgParas.FullTray, position.PositionNo, new List<float>() { (float)dx_Robot.D, (float)dy_Robot.D, angle });
+ RobotMsg_LoadFullTray.Action = RobotMsgAction.Load;
+ RobotMsg_LoadFullTray.Para1 = RobotMsgParas.FullTray;
+ RobotMsg_LoadFullTray.Para2 = position.PositionNo;
+ RobotMsg_LoadFullTray.Datas = new List<float>() { (float)dx_Robot.D, (float)dy_Robot.D, angle }.ConvertAll(s => s.ToString()).ToList();
+ bind.Robot.SendMsg(RobotMsg_LoadFullTray, true);
+
+ return new ProcessResponse(true);
+ }
+ #endregion
+
+ #region 婊ray浜х嚎涓嬫枡
+ [ProcessMethod("", "AGV_UnloadFullTray", "AGV鍘诲線鍗歌浇婊ray鏂�", true)]
+ public ProcessResponse AGV_UnloadFullTray(IOperationConfig config, IDevice device)
+ {
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
+ PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.UnloadFullTray);
+
+ if (position == null)
+ {
+ throw new ProcessException("璺緞閰嶇疆鏈缃弧Tray涓嬫枡鐐�");
+ }
+
+ bind.AGVDest = position.PositionCode;
+ bind.AGV.TaskOrder(position.PositionCode);
+
+ bind.AGVStatus = TaskStatus.Running;
+
+ return new ProcessResponse(true);
+ }
+
+ [ProcessMethod("", "Robot_UnloadFullTray", "鏈哄櫒浜哄嵏杞芥弧Tray", true)]
+ public ProcessResponse Robot_UnloadFullTray(IOperationConfig config, IDevice device)
+ {
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == (config as AGVBindOpConfig).BindId);
+ PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.UnloadFullTray);
+
+ if (position == null)
+ {
+ throw new ProcessException("璺緞閰嶇疆鏈缃弧Tray涓嬫枡鐐�");
+ }
+
+ if (bind.AGV.CurrentPosition != position.PositionCode)
+ {
+ throw new ProcessException("AGV褰撳墠鏈浜庢弧Tray涓嬫枡鐐�");
+ }
+
+ bind.RobotStatus = TaskStatus.Running;
+ bind.Robot.SendMsg(RobotMsgAction.Move, RobotMsgParas.LineSnap, position.PositionNo);
+
+ return new ProcessResponse(true);
+ }
+
+ //[ProcessMethod("", "Camera_UnloadFullTray", "鐩告満鎿嶄綔鍗歌浇婊ray", true)]
+ //public ProcessResponse Camera_UnloadFullTray(IOperationConfig config, IDevice device)
+ public ProcessResponse Camera_UnloadFullTray(string robotId, int positionNum)
+ {
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == robotId);
+ PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.UnloadFullTray && u.PositionNo == positionNum);
+
+ if (position == null)
+ {
+ throw new ProcessException("璺緞閰嶇疆鏈缃弧Tray涓嬫枡鐐�");
+ }
+
+ if (bind.AGV.CurrentPosition != position.PositionCode)
+ {
+ throw new ProcessException("AGV褰撳墠鏈浜庢弧Tray涓嬫枡鐐�");
+ }
+
+ PositionVisionConfig visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.PositionCode == position.PositionCode && u.CameraId == bind.CameraId);
+
+ if (visionConfig == null)
+ {
+ throw new ProcessException("鏈厤缃鐩告満鐨勬弧Tray涓嬫枡鐐圭殑瑙嗚鎿嶄綔閰嶇疆");
+ }
+
+ bool isLineReady = false;
+ int reTryTime = Config.LineBusyRetryTimes;
+
+ do
+ {
+ using (HObject hImage = CollectHImage(bind.Camera, visionConfig.CameraOpConfig, "Camera_UnloadFullTray"))
+ {
+ string toolPath = visionConfig.CameraOpConfig.AlgorithemPath;
+ if (!_halconToolDict.ContainsKey(toolPath))
+ {
+ throw new ProcessException($"鏈厤缃瓹amera_UnloadFullTray鐨勮瑙夌畻娉曡矾寰�");
+ }
+
+ _halconToolDict[toolPath].SetDictionary(new Dictionary<string, HTuple>() { { "OUTPUT_Result", new HTuple() } }, new Dictionary<string, HObject>() { { "INPUT_Image", hImage } });
+ _halconToolDict[toolPath].RunProcedure();
+
+ isLineReady = _halconToolDict[toolPath].GetResultTuple("OUTPUT_Result").I == 1;
+ }
+
+ if (!isLineReady)
+ {
+ Thread.Sleep(Config.LineBusyWaitInterval * 1000);
+ reTryTime--;
+ }
+ else
+ {
+ reTryTime = 0;
+ }
+ } while (reTryTime > 0);
+
+ //if (!isLineReady)
+ //{
+ // bind.Robot.SendMsg(RobotMsgType.Send, -1, true, RobotMsgAction.State, RobotMsgParas.LineSnap, new List<string>() { "-1" });
+ // throw new ProcessException("浜х嚎蹇欙紝绛夊緟瓒呮椂");
+ //}
+ //else
+ //{
+ // bind.Robot.SendMsg(RobotMsgType.Send, -1, true, RobotMsgAction.State, RobotMsgParas.LineSnap, new List<string>() { "1" });
+ //}
+
+ if (isLineReady)
+ {
+ bind.Robot.SendMsg(RobotMsgAction.Unload, RobotMsgParas.FullTray, position.PositionNo);
+ }
+ else
+ {
+ bind.Robot.SendMsg(RobotMsgAction.Move, RobotMsgParas.Home, position.PositionNo);
+ }
+
+ return new ProcessResponse(true);
+ }
+ #endregion
+ }
+
+ public class AGVBindOpConfig : OperationConfigBase
+ {
+ public string BindId { get; set; }
+ public PathPosition Position { get; set; }
+
+ public AGVBindOpConfig() { }
+
+ public AGVBindOpConfig(string bindId, PathPosition position = null)
+ {
+ BindId = bindId;
+ Position = position;
+ }
+ }
+
+ public class TaskAssignInfo
+ {
+ public int PositionNo { get; set; }
+
+ public bool IsTaskNeed { get; set; } = false;
+
+ public bool IsTaskAssgined { get; set; } = false;
+
+ public string AgvId { get; set; }
}
}
--
Gitblit v1.8.0