// Update position AGV icon in simulation mode (speed: cm/s) public static Point UpdatePositionAGV(int agvID) { // Find AGV in ListAGV var index = AGV.ListAGV.FindIndex(a => a.ID == agvID); AGV agv = AGV.ListAGV[index]; Point position = new Point(); // If this node is pick node, remove pallet code that was picked and save this time if (agv.Tasks.Count != 0 && agv.ExitNode == agv.Tasks[0].PickNode) { RackColumn column = RackColumn.ListColumn.Find(c => c.AtNode == agv.ExitNode); Pallet.SaveDeliveryTime(column.PalletCodes[agv.Tasks[0].PickLevel - 1], Pallet.ListPallet); DBUtility.UpdatePalletDB("PalletInfoTable", column.PalletCodes[agv.Tasks[0].PickLevel - 1], false, DateTime.Now.ToString("dddd, MMMM dd, yyyy h:mm:ss tt"), Pallet.ListPallet); column.PalletCodes[agv.Tasks[0].PickLevel - 1] = null; } // Update label position List <Node> Nodes = Node.ListNode; int pixelDistance = (int)Math.Round(agv.DistanceToExitNode * Display.Scale); int x = Nodes[agv.ExitNode].X - Display.LabelAGV[agvID].Size.Width / 2; int y = Nodes[agv.ExitNode].Y - Display.LabelAGV[agvID].Size.Height / 2; switch (agv.Orientation) { case 'E': position.X = x + pixelDistance; position.Y = y; break; case 'W': position.X = x - pixelDistance; position.Y = y; break; case 'N': position.X = x; position.Y = y - pixelDistance; break; case 'S': position.X = x; position.Y = y + pixelDistance; break; } return(position); }
// Update position AGV icon in simulation mode (speed: cm/s) public static Point SimUpdatePositionAGV(int agvID, float speed) { // Find AGV in SimListAGV, get current point var index = AGV.SimListAGV.FindIndex(a => a.ID == agvID); AGV agv = AGV.SimListAGV[index]; Point position = Display.SimLabelAGV[agvID].Location; // Handle (waiting method) collision if it happens CollisionStatus status = Collision.SimHandle(agv, Collision.SimListCollision); if (status == CollisionStatus.Handling) { // Update agv status and velocity agv.Velocity = 0; agv.Status = "Stop"; return(position); } // return old point when agv has no path if (agv.Path.Count == 0) { return(position); } // Get navigation frame array. Note: string is a reference type, // so any change in navigationArr is also in AGV.SimListAGV[index].navigationArr string[] navigationArr = agv.navigationArr; // Check whether current point is a node or not // Note: shift position of label to center (+LabelAGV[].Width/2, +LabelAGV[].Height/2) var node = Node.ListNode.FirstOrDefault(n => { return((n.X == position.X + SimLabelAGV[agvID].Width / 2) && (n.Y == position.Y + SimLabelAGV[agvID].Height / 2)); }); // Current point is not a node and current position is start node, // it means initDistance to start node != 0, so go backward once then keep go ahead char orient = new char(); if (node == null && agv.ExitNode == agv.Path[0]) { switch (navigationArr[0]) { case "A": orient = UpdateOrient(agv.Orientation, "A"); break; case "B": orient = UpdateOrient(agv.Orientation, "B"); navigationArr[0] = "A"; break; } } // Current point is not a node and current position is not start node, // so keep go ahead else if (node == null) { orient = UpdateOrient(agv.Orientation, "A"); } // If goal was reached, no update position, remove old path, get next path (if exist) else if (node.ID == agv.Path.LastOrDefault()) { // Update AGV information agv.ExitNode = node.ID; // Update ExitNode orient = UpdateOrient(agv.Orientation, "A"); agv.Orientation = orient; // Update Orientation agv.DistanceToExitNode = 0f; // Update Distance to ExitNode agv.Status = "Stop"; // Update Status agv.Velocity = 0; // Update Velocity // Add next path Task.AddNextPathOfSimAGV(agv); return(position); } // Current point is at start node and initDistance to start node == 0 // Turn direction once then keep go ahead else if (node.ID == agv.Path[0] && agv.DistanceToExitNode == 0f) { switch (navigationArr[0]) { case "A": orient = UpdateOrient(agv.Orientation, "A"); break; case "B": orient = UpdateOrient(agv.Orientation, "B"); navigationArr[0] = "A"; break; case "L": orient = UpdateOrient(agv.Orientation, "L"); navigationArr[0] = "A"; break; case "R": orient = UpdateOrient(agv.Orientation, "R"); navigationArr[0] = "A"; break; } // If this node is pick node, remove pallet code that was picked and save this time if (agv.Tasks.Count != 0 && node.ID == agv.Tasks[0].PickNode) { RackColumn column = RackColumn.SimListColumn.Find(c => c.AtNode == agv.ExitNode); Pallet.SaveDeliveryTime(column.PalletCodes[agv.Tasks[0].PickLevel - 1], Pallet.SimListPallet); DBUtility.UpdatePalletDB("SimPalletInfoTable", column.PalletCodes[agv.Tasks[0].PickLevel - 1], false, DateTime.Now.ToString("dddd, MMMM dd, yyyy h:mm:ss tt"), Pallet.SimListPallet); column.PalletCodes[agv.Tasks[0].PickLevel - 1] = null; } } // Current point is a node but start node else { int idx = Array.IndexOf(navigationArr, node.ID.ToString()); string dir = navigationArr[idx + 1]; orient = UpdateOrient(agv.Orientation, dir); } // Modify speed to make sure AGV icon can reach next node int i = agv.Path.IndexOf(agv.ExitNode); int nextNode = agv.Path[i + 1]; int dnx = (position.X + SimLabelAGV[agvID].Width / 2) - Node.ListNode[nextNode].X; int dny = (position.Y + SimLabelAGV[agvID].Height / 2) - Node.ListNode[nextNode].Y; int nd = (int)Math.Sqrt(dnx * dnx + dny * dny); if (agv.ExitNode == agv.Path[0]) { // At first node of path, having 4 cases of agv position // (dnx * dny != 0) for 2 cases and (dnx * dnx0 > 0 || dny * dny0 > 0) for the others int dnx0 = (position.X + SimLabelAGV[agvID].Width / 2) - Node.ListNode[agv.Path[0]].X; int dny0 = (position.Y + SimLabelAGV[agvID].Height / 2) - Node.ListNode[agv.Path[0]].Y; int nd0 = (int)Math.Sqrt(dnx0 * dnx0 + dny0 * dny0); if (dnx * dny != 0 || dnx * dnx0 > 0 || dny * dny0 > 0) { nextNode = agv.Path[0]; nd = nd0; } } int sp = (int)Math.Round(speed * Display.Scale * (100.0 / 1000)); // timer1.Interval = 100ms int step = (nd % sp == 0) ? sp : (nd % sp); // Update AGV information before update position if (node != null) { agv.ExitNode = node.ID; // Update ExitNode } agv.Orientation = orient; // Update Orientation int exitNode = agv.ExitNode; int dx = (position.X + SimLabelAGV[agvID].Width / 2) - Node.ListNode[exitNode].X; int dy = (position.Y + SimLabelAGV[agvID].Height / 2) - Node.ListNode[exitNode].Y; agv.DistanceToExitNode = (float)Math.Sqrt(dx * dx + dy * dy) / Display.Scale; // Update Distance to ExitNode agv.Status = "Running"; // Update Status // Update next position of AGV icon in panel switch (orient) { case 'E': position = new Point(position.X + step, position.Y); break; case 'W': position = new Point(position.X - step, position.Y); break; case 'S': position = new Point(position.X, position.Y + step); break; case 'N': position = new Point(position.X, position.Y - step); break; } return(position); }