From 6428fad15dbe79e30a48ffc9aabe31e03a45426c Mon Sep 17 00:00:00 2001
From: patrick <patrick.xu@broconcentric.com>
Date: 星期四, 05 十二月 2019 12:50:13 +0800
Subject: [PATCH] 1. 添加部分log信息 2. 修改部分机器人动作流程 3. 开放OperationConfig的InputData参数
---
src/A032.Process/ProcessControl_Method.cs | 196 +++++++++++++++++++++++++++++++++++++-----------
1 files changed, 151 insertions(+), 45 deletions(-)
diff --git a/src/A032.Process/ProcessControl_Method.cs b/src/A032.Process/ProcessControl_Method.cs
index 97166ab..dc0d8c8 100644
--- a/src/A032.Process/ProcessControl_Method.cs
+++ b/src/A032.Process/ProcessControl_Method.cs
@@ -20,9 +20,54 @@
{
public partial class ProcessControl
{
+ const int WAITTIME = 5000;
+
Dictionary<int, int> machineFullTrayDict = new Dictionary<int, int>();
Dictionary<int, int> machineEmptyTrayDict = new Dictionary<int, int>();
List<TaskAssignInfo> taskAssignedList = new List<TaskAssignInfo>();
+
+ #region AGV浜嬩欢
+ private void OnAGVBatteryLvlChanged(SeerAGVDriver agv, float preBatteryLvl, float batteryLvl)
+ {
+ var bind = Config.AGVBindCollection.FirstOrDefault(u => u.AGVId == agv.Id);
+ SeerAGVInitialConfig iConfig = agv.InitialConfig as SeerAGVInitialConfig;
+ if (batteryLvl <= iConfig.BatteryLvlToCharge && preBatteryLvl > iConfig.BatteryLvlToCharge)
+ {
+ Task.Run(() =>
+ {
+ var position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.Charge);
+
+ if (position == null)
+ {
+ throw new ProcessException("鏈壘鍒板厖鐢靛湴鐐�");
+ }
+
+ while (bind.UnitStatus != TaskStatus.Available)
+ {
+ if (bind.SetAGVStatus(TaskStatus.Running))
+ {
+ bind.AGV.TaskOrder(position.PositionCode);
+ break;
+ }
+
+ Thread.Sleep(WAITTIME);
+ }
+ });
+
+ return;
+ }
+
+ if (batteryLvl >= iConfig.BatteryLvlChargeDone && preBatteryLvl < iConfig.BatteryLvlChargeDone)
+ {
+ var position = Config.PositionCollection.FirstOrDefault(u => u.PositionCode == agv.CurrentPosition);
+
+ if (position != null && position.Description == PathPositionDefinition.Charge)
+ {
+ bind.SetAGVStatus(TaskStatus.Available);
+ }
+ return;
+ }
+ }
private void OnAGVTaskStatusChanged(SeerAGVDriver agv, AGVTaskStatus taskStatus)
{
@@ -42,7 +87,7 @@
// bind.RobotStatus = TaskStatus.Running;
//}
- bind.AGVStatus = TaskStatus.Available;
+ bind.SetAGVStatus(TaskStatus.Available);
}
}
@@ -64,20 +109,26 @@
// bind.RobotStatus = TaskStatus.Running;
//}
- bind.AGVStatus = TaskStatus.Available;
+ bind.SetAGVStatus(TaskStatus.Available);
+ LogAsync(DateTime.Now, $"AGV鍒颁綅{positionCode}", "");
PathPosition position = Config.PositionCollection.FirstOrDefault(p => p.PositionCode == bind.AGVDest);
switch (position.Description)
{
case PathPositionDefinition.LoadEmptyTray:
+ LogAsync(DateTime.Now, $"AGV瀹屾垚锛屽噯澶囦笂绌篢ray", "");
Robot_LoadEmptyTray(bind.Id, position);
break;
case PathPositionDefinition.LoadFullTray:
+ LogAsync(DateTime.Now, $"AGV瀹屾垚锛屽噯澶囦笂婊ray", "");
+ Robot_LoadFullTraySnap(bind.Id, position);
break;
case PathPositionDefinition.UnloadEmptyTray:
+ LogAsync(DateTime.Now, $"AGV瀹屾垚锛屽噯澶囦笅绌篢ray", "");
Robot_UnloadEmptyTraySnap(bind.Id, position);
break;
case PathPositionDefinition.UnloadFullTray:
+ LogAsync(DateTime.Now, $"AGV瀹屾垚锛屽噯澶囦笅婊ray", "");
Robot_UnloadFullTraySnap(bind.Id, position);
break;
default:
@@ -85,6 +136,7 @@
}
}
}
+ #endregion
private void OnRobotMsgReceived(DateTime dt, AuboRobotDriver robot, RobotMsg msg)
{
@@ -142,8 +194,11 @@
if (machineEmptyTrayDict[msg.Para2] < Config.Machine_EmptyTrayNum)
{
- bind.RobotIOHandle.Reset();
- bind.RobotIOHandle.WaitOne();
+ //bind.RobotIOHandle.Reset();
+ //bind.RobotIOHandle.WaitOne();
+ bind.Robot.MonitorHandle.WaitOne();
+ //bind.Robot.IOChangedHandle.WaitOne();
+ Thread.Sleep((bind.Robot.InitialConfig as AuboRobotInitialConfig).ScanInterval);
if (!bind.IsFullTrayFull)
{
@@ -165,12 +220,19 @@
{
bind.CurrentFullTray = int.Parse(msg.Datas[1]);
- bind.RobotIOHandle.Reset();
- bind.RobotIOHandle.WaitOne();
+ //bind.RobotIOHandle.Reset();
+ //bind.RobotIOHandle.WaitOne();
+ bind.Robot.MonitorHandle.WaitOne();
+ //bind.Robot.IOChangedHandle.WaitOne();
+ Thread.Sleep((bind.Robot.InitialConfig as AuboRobotInitialConfig).ScanInterval);
if (!bind.IsFullTrayEmpty)
{
Camera_UnloadFullTray(robot.Id, msg.Para2);
+ }
+ else
+ {
+ bind.RobotStatus = TaskStatus.Available;
}
}
break;
@@ -210,12 +272,17 @@
break;
case RobotMsgAction.Calibration:
{
+ _calibReply.CalibIndex = int.Parse(msg.Datas[4]);
+ _calibReply.CalibPositionNo = msg.Para2;
+ _calibReply.RobotPosition = new CustomizedPoint(float.Parse(msg.Datas[0]), float.Parse(msg.Datas[1]));
+ _calibReply.CalibHandle.Set();
}
break;
case RobotMsgAction.StandardPoint:
{
-
+ _calibReply.CalibPositionNo = msg.Para2;
+ _calibReply.CalibHandle.Set();
}
break;
default:
@@ -309,7 +376,7 @@
while (bind.IsEmptyTrayEmpty && !bind.IsEmptyTrayTaskAssigned)
{
//if (bind.TaskList.Count == 0)
- if (bind.AGVStatus == TaskStatus.Available && bind.RobotStatus == TaskStatus.Available)
+ if (bind.UnitStatus == TaskStatus.Available)
{
//List<AGVTaskModel> models = new List<AGVTaskModel>();
//models.Add(new AGVTaskModel(TaskAvailableLevel.Both, "AGV_LoadEmptyTray"));
@@ -317,12 +384,14 @@
//AddNewTaskToBind(device.Id, models);
- AGV_LoadEmptyTray(bind.Id);
- bind.IsEmptyTrayTaskAssigned = true;
+ if (AGV_LoadEmptyTray(bind.Id))
+ {
+ bind.IsEmptyTrayTaskAssigned = true;
+ }
}
else
{
- Thread.Sleep(500);
+ Thread.Sleep(WAITTIME);
}
}
});
@@ -339,6 +408,7 @@
[ProcessMethod("", "Robot_Monitor_FullTrayFull", "鏈哄櫒浜虹洃鍚簨浠�-婊ray鍖哄煙鏀炬弧", true)]
public ProcessResponse Robot_Monitor_FullTrayFull(IOperationConfig config, IDevice device)
{
+ //(device as AuboRobotDriver).IOChangedHandle.Reset();
bool isFullTrayFull = config.InputPara[0] == 1;
var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == device.Id);
if (isFullTrayFull)
@@ -351,7 +421,7 @@
while (bind.IsFullTrayFull && !bind.IsFullTrayTaskAssigned)
{
//if (bind.TaskList.Count == 0)
- if (bind.AGVStatus == TaskStatus.Available && bind.RobotStatus == TaskStatus.Available)
+ if (bind.UnitStatus == TaskStatus.Available)
{
//List<AGVTaskModel> models = new List<AGVTaskModel>();
//models.Add(new AGVTaskModel(TaskAvailableLevel.Both, "AGV_UnloadFullTray"));
@@ -359,13 +429,14 @@
//AddNewTaskToBind(device.Id, models);
- AGV_UnloadFullTray(bind.Id);
-
- bind.IsFullTrayTaskAssigned = true;
+ if (AGV_UnloadFullTray(bind.Id))
+ {
+ bind.IsFullTrayTaskAssigned = true;
+ }
}
else
{
- Thread.Sleep(500);
+ Thread.Sleep(WAITTIME);
}
}
});
@@ -376,7 +447,9 @@
bind.IsFullTrayTaskAssigned = false;
}
- bind.RobotIOHandle.Set();
+ //(device as AuboRobotDriver).IOChangedHandle.Set();
+
+ //bind.RobotIOHandle.Set();
return new ProcessResponse(true);
}
@@ -384,11 +457,13 @@
[ProcessMethod("", "Robot_Monitor_FullTrayEmpty", "鏈哄櫒浜虹洃鍚簨浠�-婊ray鍖哄煙娓呯┖", true)]
public ProcessResponse Robot_Monitor_FullTrayEmpty(IOperationConfig config, IDevice device)
{
+ //(device as AuboRobotDriver).IOChangedHandle.Reset();
bool isFullTrayEmpty = config.InputPara[0] == 0;
var bind = Config.AGVBindCollection.FirstOrDefault(u => u.RobotId == device.Id);
bind.IsFullTrayEmpty = isFullTrayEmpty;
- bind.RobotIOHandle.Set();
+ //(device as AuboRobotDriver).IOChangedHandle.Set();
+ //bind.RobotIOHandle.Set();
return new ProcessResponse(true);
}
@@ -460,7 +535,7 @@
// return new ProcessResponse(true);
//}
- public void AGV_LoadEmptyTray(string bindId)
+ public bool AGV_LoadEmptyTray(string bindId)
{
var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == bindId);
PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.LoadEmptyTray);
@@ -470,10 +545,17 @@
throw new ProcessException("璺緞閰嶇疆鏈缃┖Tray涓婃枡鐐�");
}
- bind.AGVDest = position.PositionCode;
- bind.AGV.TaskOrder(position.PositionCode);
+ if (bind.SetAGVStatus(TaskStatus.Running))
+ {
+ bind.AGVDest = position.PositionCode;
+ bind.AGV.TaskOrder(position.PositionCode);
- bind.AGVStatus = TaskStatus.Running;
+ return true;
+ }
+ else
+ {
+ return false;
+ }
}
//[ProcessMethod("", "AfterEmptyTrayPositionArrived", "鍒拌揪绌篢ray涓婃枡鐐�", true)]
@@ -589,20 +671,21 @@
//bind.AddTask(model_AGV);
//bind.AddTask(model_Robot);
- AGV_UnloadEmptyTray(bind.Id, position);
-
- taskStatus.IsTaskAssgined = true;
- taskStatus.AgvId = bind.AGVId;
+ if (AGV_UnloadEmptyTray(bind.Id, position))
+ {
+ taskStatus.IsTaskAssgined = true;
+ taskStatus.AgvId = bind.AGVId;
+ }
}
}
- Thread.Sleep(300);
+ Thread.Sleep(WAITTIME);
}
});
}
//[ProcessMethod("", "AGV_UnloadEmptyTray", "AGV鍘诲線鍗歌浇绌篢ray鏂欎綅缃�", true)]
- public void AGV_UnloadEmptyTray(string bindId, PathPosition position)
+ public bool AGV_UnloadEmptyTray(string bindId, PathPosition position)
{
var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == bindId);
@@ -611,10 +694,17 @@
throw new ProcessException("璺緞閰嶇疆鏈缃┖Tray涓嬫枡鐐�");
}
- bind.AGVDest = position.PositionCode;
- bind.AGV.TaskOrder(position.PositionCode);
+ if (bind.SetAGVStatus(TaskStatus.Running))
+ {
+ bind.AGVDest = position.PositionCode;
+ bind.AGV.TaskOrder(position.PositionCode);
- bind.AGVStatus = TaskStatus.Running;
+ return true;
+ }
+ else
+ {
+ return false;
+ }
}
//[ProcessMethod("", "Robot_UnloadEmptyTray", "鏈哄櫒浜鸿繍鍔ㄨ嚦绌篢ray鎷嶇収浣嶇疆", true)]
@@ -756,7 +846,7 @@
adjust_X = (float)dx_Robot.D;
adjust_Y = (float)dy_Robot.D;
- adjust_Angle = angle;
+ adjust_Angle = visionConfig.StandardPoint.Angle - angle;
}
//bind.Robot.SendMsg(RobotMsgAction.Unload, RobotMsgParas.EmptyTray, position.PositionNo, new List<float>() { (float)dx_Robot.D, (float)dy_Robot.D, angle });
@@ -831,10 +921,11 @@
//bind.AddTask(model_AGV);
//bind.AddTask(model_Robot);
- AGV_LoadFullTray(bind.Id, position);
-
- taskStatus.IsTaskAssgined = true;
- taskStatus.AgvId = bind.AGVId;
+ if (AGV_LoadFullTray(bind.Id, position))
+ {
+ taskStatus.IsTaskAssgined = true;
+ taskStatus.AgvId = bind.AGVId;
+ }
}
}
@@ -844,7 +935,7 @@
}
//[ProcessMethod("", "AGV_LoadFullTray", "AGV鍘诲線婊ray涓婃枡浣嶇疆", true)]
- public void AGV_LoadFullTray(string bindId, PathPosition position)
+ public bool AGV_LoadFullTray(string bindId, PathPosition position)
{
var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == bindId);
@@ -855,10 +946,17 @@
throw new ProcessException("璺緞閰嶇疆鏈缃弧Tray涓婃枡鐐�");
}
- bind.AGVDest = position.PositionCode;
- bind.AGV.TaskOrder(position.PositionCode);
+ if (bind.SetAGVStatus(TaskStatus.Running))
+ {
+ bind.AGVDest = position.PositionCode;
+ bind.AGV.TaskOrder(position.PositionCode);
- bind.AGVStatus = TaskStatus.Running;
+ return true;
+ }
+ else
+ {
+ return false;
+ }
//return new ProcessResponse(true);
}
@@ -1026,7 +1124,7 @@
adjust_X = (float)dx_Robot.D;
adjust_Y = (float)dy_Robot.D;
- adjust_Angle = angle;
+ adjust_Angle = visionConfig.StandardPoint.Angle - angle;
}
//bind.Robot.SendMsg(RobotMsgAction.Load, RobotMsgParas.FullTray, position.PositionNo, new List<float>() { (float)dx_Robot.D, (float)dy_Robot.D, angle });
@@ -1060,7 +1158,7 @@
// return new ProcessResponse(true);
//}
- public void AGV_UnloadFullTray(string bindId)
+ public bool AGV_UnloadFullTray(string bindId)
{
var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == bindId);
PathPosition position = Config.PositionCollection.FirstOrDefault(u => u.Description == PathPositionDefinition.UnloadFullTray);
@@ -1070,10 +1168,17 @@
throw new ProcessException("璺緞閰嶇疆鏈缃弧Tray涓嬫枡鐐�");
}
- bind.AGVDest = position.PositionCode;
- bind.AGV.TaskOrder(position.PositionCode);
+ if (bind.SetAGVStatus(TaskStatus.Running))
+ {
+ bind.AGVDest = position.PositionCode;
+ bind.AGV.TaskOrder(position.PositionCode);
- bind.AGVStatus = TaskStatus.Running;
+ return true;
+ }
+ else
+ {
+ return false;
+ }
}
//[ProcessMethod("", "Robot_UnloadFullTray", "鏈哄櫒浜哄嵏杞芥弧Tray", true)]
@@ -1115,6 +1220,7 @@
bind.RobotStatus = TaskStatus.Running;
bind.Robot.SendMsg(RobotMsgAction.Move, RobotMsgParas.LineSnap, position.PositionNo);
+ //LogAsync(DateTime.Now, "Robot杩愬姩鑷充笅婊ray鎷嶇収", "");
}
//[ProcessMethod("", "Camera_UnloadFullTray", "鐩告満鎿嶄綔鍗歌浇婊ray", true)]
--
Gitblit v1.8.0