private void btnAdd_Click(object sender, EventArgs e) { if (String.IsNullOrEmpty(txbID.Text) || String.IsNullOrEmpty(cbbExitNode.Text) || String.IsNullOrEmpty(cbbOrientation.Text) || String.IsNullOrEmpty(txbDistance.Text)) { return; } AGV agv = new AGV(Convert.ToInt16(txbID.Text), Convert.ToInt16(cbbExitNode.Text), Convert.ToChar(cbbOrientation.Text), Convert.ToSingle(txbDistance.Text), "Stop"); switch (cbbModeList.Text) { case "Real Time": // Check whether AGV ID exist in old and new list or not foreach (AGV a in AGV.ListAGV) { if (Convert.ToInt16(txbID.Text) == a.ID) { MessageBox.Show("AGV ID already exists.\nPlease choose other AGV ID", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error); return; } } // If not exist, add new AGV into listNewAGV AGV.ListAGV.Add(agv); Display.AddLabelAGV(cbbModeList.Text, agv.ID, agv.CurrentNode, agv.CurrentOrient, agv.DistanceToCurrentNode); // Put new AGV ID in listView listViewAGV.Items.Add(" AGV#" + agv.ID, 0); MessageBox.Show(" Add AGV#" + agv.ID.ToString() + " Successful !", "Updates AGV", MessageBoxButtons.OK, MessageBoxIcon.Information); break; case "Simulation": // Check whether AGV ID exist in old and new list or not foreach (AGV a in AGV.SimListAGV) { if (Convert.ToInt16(txbID.Text) == a.ID) { MessageBox.Show("AGV ID already exists.\nPlease choose other AGV ID", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error); return; } } // If not exist, add new AGV into listNewAGV agv.Velocity = Convert.ToSingle(txtVelocity.Text); AGV.SimListAGV.Add(agv); AGV.SimFullPathOfAGV[agv.ID] = agv.CurrentNode.ToString() + "-G"; Display.AddLabelAGV(cbbModeList.Text, agv.ID, agv.CurrentNode, agv.CurrentOrient, agv.DistanceToCurrentNode); // Put new AGV ID in listView listViewAGV.Items.Add(" AGV#" + agv.ID, 0); MessageBox.Show(" Add AGV#" + agv.ID.ToString() + " Successful !", "Updates AGV", MessageBoxButtons.OK, MessageBoxIcon.Information); break; } // Clear textBox for next adding txbID.Clear(); txbDistance.Clear(); txtVelocity.Clear(); }
private void button2_Click(object sender, EventArgs e) { int agvID = 1; AGV agv = AGV.SimListAGV.Find(p => p.ID == agvID); if (agv.Status == "Stop") { List <int> path = Algorithm.A_starFindPath(Node.ListNode, Node.MatrixNodeDistance, agv.CurrentNode, 55); AGV.SimFullPathOfAGV[agvID] = Navigation.GetNavigationFrame(path, Node.MatrixNodeOrient); AGV.SimListAGV[0].Path.Add(path); } //agv.PathCopmpleted = 4; }
public static void UpdatePositionAGV(int agvID, Label lbagv) { var index = AGV.ListAGV.FindIndex(a => a.ID == agvID); AGV agv = AGV.ListAGV[index]; Point oldAGVPosition = Display.LabelAGV[agvID].Location; Point newAGVPosition = new Point(); int pixelDistance = (int)Math.Round(agv.DistanceToCurrentNode * Scale); List <Node> Nodes = DBUtility.GetDataFromDB <List <Node> >("NodeInfoTable"); int x, y; try { x = Nodes[agv.CurrentNode].X - (int)(50 / 2); y = Nodes[agv.CurrentNode].Y - (int)(50 / 2); } catch { x = oldAGVPosition.X; y = oldAGVPosition.Y; } switch (agv.CurrentOrient) { case 'E': newAGVPosition = new Point(x + pixelDistance, y); break; case 'W': newAGVPosition = new Point(x - pixelDistance, y); break; case 'S': newAGVPosition = new Point(x, y + pixelDistance); break; case 'N': newAGVPosition = new Point(x, y - pixelDistance); break; default: newAGVPosition = oldAGVPosition; break; } lbagv.Location = newAGVPosition; }
private void btnRemove_Click(object sender, EventArgs e) { // Remove an AGV has ID in comboBox if (String.IsNullOrEmpty(cbbIDRemove.Text) == false) { switch (cbbModeList.Text) { case "Real Time": AGV agvRealRemove = AGV.ListAGV.Find(a => { return(a.ID == Convert.ToInt16(cbbIDRemove.Text)); }); if (AGV.ListAGV.Contains(agvRealRemove)) { AGV.ListAGV.Remove(agvRealRemove); } // Put remaining AGV in listView listViewAGV.Clear(); foreach (AGV agv in AGV.ListAGV) { listViewAGV.Items.Add(" AGV#" + agv.ID, 0); } cbbIDRemove.Items.Clear(); MessageBox.Show(" Remove AGV#" + agvRealRemove.ID.ToString() + " Successful !", "Updates AGV", MessageBoxButtons.OK, MessageBoxIcon.Information); break; case "Simulation": AGV agvSimRemove = AGV.SimListAGV.Find(a => { return(a.ID == Convert.ToInt16(cbbIDRemove.Text)); }); if (AGV.SimListAGV.Contains(agvSimRemove)) { AGV.SimListAGV.Remove(agvSimRemove); Display.RemoveLabelAGV(cbbModeList.Text, agvSimRemove.ID); } // Put remaining AGV in listView listViewAGV.Clear(); foreach (AGV agv in AGV.SimListAGV) { listViewAGV.Items.Add(" AGV#" + agv.ID, 0); } cbbIDRemove.Items.Clear(); MessageBox.Show(" Remove AGV#" + agvSimRemove.ID.ToString() + " Successful !", "Updates AGV", MessageBoxButtons.OK, MessageBoxIcon.Information); break; } } }
private void RunAGV_Click(object sender, EventArgs e) { ToolStripMenuItem item = sender as ToolStripMenuItem; string[] arrItem = item.Text.Split(new char[] { '#' }, StringSplitOptions.RemoveEmptyEntries); int agvID = Convert.ToInt16(arrItem[1]); switch (Display.Mode) { case "Real Time": break; case "Simulation": AGV agv = AGV.SimListAGV.Find(a => a.ID == agvID); agv.Stop = false; agv.Status = "Running"; Display.SimLabelAGV[agv.ID].BackColor = Color.CornflowerBlue; break; } }
private static int CalculateTotalRemainingDistance(AGV agv) { int totalDistance = 0; // get list remaning path of this agv List <List <int> > listRemainingPath = new List <List <int> >(); for (int i = 0; i < agv.Tasks.Count; i++) { if (i == 0 && agv.Tasks[0].Status == "Doing") { listRemainingPath.Add(Algorithm.A_starFindPath(Node.ListNode, Node.MatrixNodeDistance, agv.CurrentNode, agv.Tasks[0].DropNode)); } else if (i == 0 && agv.Tasks[0].Status == "Waiting") { listRemainingPath.Add(Algorithm.A_starFindPath(Node.ListNode, Node.MatrixNodeDistance, agv.CurrentNode, agv.Tasks[0].PickNode)); listRemainingPath.Add(Algorithm.A_starFindPath(Node.ListNode, Node.MatrixNodeDistance, agv.Tasks[0].PickNode, agv.Tasks[0].DropNode)); } else { listRemainingPath.Add(Algorithm.A_starFindPath(Node.ListNode, Node.MatrixNodeDistance, agv.Tasks[i].PickNode, agv.Tasks[i].DropNode)); } if (i < agv.Tasks.Count - 1) { listRemainingPath.Add(Algorithm.A_starFindPath(Node.ListNode, Node.MatrixNodeDistance, agv.Tasks[i].DropNode, agv.Tasks[i + 1].PickNode)); } } // calculate total distance of list remaining path foreach (List <int> path in listRemainingPath) { for (int i = 0; i < path.Count - 1; i++) { totalDistance += Node.MatrixNodeDistance[path[i], path[i + 1]]; } } return(totalDistance); }
public static void DetectColission(AGV agv1, AGV agv2) { if (AGV.SimListAGV.Count < 2) { return; } int agv1_nextNode = 0, agv1_nextNode_1 = 0; int agv1_index = 0; try { goal1 = agv1.Path[0][agv1.Path[0].Count - 1]; agv1_index = agv1.Path[0].FindIndex(p => p == agv1.CurrentNode); } catch { } try { agv1_nextNode = agv1.Path[0][agv1_index + 1]; } catch { } Debug.WriteLine(agv1_nextNode.ToString()); try { agv1_nextNode_1 = agv1.Path[0][agv1_index + 2]; } catch { } int agv2_nextNode = 0, agv2_nextNode_1 = 0; int agv2_index = 0; try { agv2_index = agv2.Path[0].FindIndex(p => p == agv2.CurrentNode); goal2 = agv2.Path[0][agv2.Path[0].Count - 1]; } catch { } try { agv2_nextNode = agv2.Path[0][agv2_index + 1]; } catch { } try { agv2_nextNode_1 = agv2.Path[0][agv2_index + 2]; } catch { } if (agv1.Stop && !agv2.Stop) { float deltaDistance = (float)Math.Sqrt(Math.Pow(Node.ListNode[agv1.CurrentNode].X - Node.ListNode[agv1_nextNode].X, 2) + Math.Pow(Node.ListNode[agv1.CurrentNode].Y - Node.ListNode[agv1_nextNode].Y, 2)); Node.MatrixNodeDistance = Node.CreateAdjacencyMatrix(Node.ListNode); int[,] d = Node.MatrixNodeDistance; if (agv1.DistanceToCurrentNode < 20.0) { foreach (string adj in Node.ListNode[agv1.CurrentNode].AdjacentNode) { int i = Convert.ToInt32(adj); d[agv1.CurrentNode, i] = 10000; d[i, agv1.CurrentNode] = 10000; } foreach (string adj in Node.ListNode[agv1.CurrentNode].AdjacentNode) { int i = Convert.ToInt32(adj); if (agv2_nextNode == i) { List <int> newpath = Algorithm.A_starFindPath(Node.ListNode, d, agv2.CurrentNode, goal2); agv2.Path[0] = newpath; AGV.SimFullPathOfAGV[2] = Navigation.GetNavigationFrame(newpath, Node.MatrixNodeOrient); } } } else if (agv1.DistanceToCurrentNode > (deltaDistance / 2 - 20.0)) { foreach (string adj in Node.ListNode[agv1_nextNode].AdjacentNode) { int i = Convert.ToInt32(adj); d[agv1_nextNode, i] = 10000; d[i, agv1_nextNode] = 10000; } foreach (string adj in Node.ListNode[agv1_nextNode].AdjacentNode) { int i = Convert.ToInt32(adj); if (agv2_nextNode == i) { List <int> newpath = Algorithm.A_starFindPath(Node.ListNode, d, agv2.CurrentNode, goal2); agv2.Path[0] = newpath; AGV.SimFullPathOfAGV[2] = Navigation.GetNavigationFrame(newpath, Node.MatrixNodeOrient); } } } else if (agv1.DistanceToCurrentNode > 20.0 && agv1.DistanceToCurrentNode < (deltaDistance / 2 - 20.0)) { d[agv1.CurrentNode, agv1_nextNode] = 100000; d[agv1_nextNode, agv1.CurrentNode] = 100000; if (agv2.CurrentNode == agv1_nextNode || agv2.CurrentNode == agv1.CurrentNode) { List <int> newpath = Algorithm.A_starFindPath(Node.ListNode, d, agv2.CurrentNode, goal2); agv2.Path[0] = newpath; AGV.SimFullPathOfAGV[2] = Navigation.GetNavigationFrame(newpath, Node.MatrixNodeOrient); } } return; } else if (!agv1.Stop && agv2.Stop) { float delta2Distance = (float)Math.Sqrt(Math.Pow(Node.ListNode[agv2.CurrentNode].X - Node.ListNode[agv2_nextNode].X, 2) + Math.Pow(Node.ListNode[agv2.CurrentNode].Y - Node.ListNode[agv2_nextNode].Y, 2)); Node.MatrixNodeDistance = Node.CreateAdjacencyMatrix(Node.ListNode); int[,] d2 = Node.MatrixNodeDistance; if (agv2.DistanceToCurrentNode < 20.0) { foreach (string adj in Node.ListNode[agv2.CurrentNode].AdjacentNode) { int i = Convert.ToInt32(adj); d2[agv2.CurrentNode, i] = 10000; d2[i, agv2.CurrentNode] = 10000; } foreach (string adj in Node.ListNode[agv2.CurrentNode].AdjacentNode) { int i = Convert.ToInt32(adj); if (agv2_nextNode == i) { List <int> newpath = Algorithm.A_starFindPath(Node.ListNode, d2, agv1.CurrentNode, goal1); agv1.Path[0] = newpath; AGV.SimFullPathOfAGV[1] = Navigation.GetNavigationFrame(newpath, Node.MatrixNodeOrient); } } } else if (agv2.DistanceToCurrentNode > (delta2Distance / 2 - 20.0)) { foreach (string adj in Node.ListNode[agv2_nextNode].AdjacentNode) { int i = Convert.ToInt32(adj); d2[agv2_nextNode, i] = 10000; d2[i, agv2_nextNode] = 10000; } foreach (string adj in Node.ListNode[agv2_nextNode].AdjacentNode) { int i = Convert.ToInt32(adj); if (agv1_nextNode == i) { List <int> newpath = Algorithm.A_starFindPath(Node.ListNode, d2, agv1.CurrentNode, goal1); agv1.Path[0] = newpath; AGV.SimFullPathOfAGV[1] = Navigation.GetNavigationFrame(newpath, Node.MatrixNodeOrient); } } } else if (agv2.DistanceToCurrentNode > 20.0 && agv2.DistanceToCurrentNode < (delta2Distance / 2 - 20.0)) { d2[agv2.CurrentNode, agv2_nextNode] = 100000; d2[agv2_nextNode, agv2.CurrentNode] = 100000; if (agv1.CurrentNode == agv2_nextNode || agv1.CurrentNode == agv2.CurrentNode) { List <int> newpath = Algorithm.A_starFindPath(Node.ListNode, d2, agv1.CurrentNode, goal1); agv1.Path[0] = newpath; AGV.SimFullPathOfAGV[1] = Navigation.GetNavigationFrame(newpath, Node.MatrixNodeOrient); } } return; } else if (agv1.Stop && agv2.Stop) { Node.MatrixNodeDistance = Node.CreateAdjacencyMatrix(Node.ListNode); return; } Debug.WriteLine("H" + goal1.ToString()); Debug.WriteLine("G" + agv2.CurrentNode.ToString()); Node.MatrixNodeDistance = Node.CreateAdjacencyMatrix(Node.ListNode); if (agv2.CurrentNode == goal1 && CollisionType == 0) { Debug.WriteLine("ABC"); if (agv1_nextNode == goal1) { int instead_node = GotoNodeNeibor(agv1.CurrentNode, agv2.CurrentOrient, goal1); List <int> newpath = Algorithm.A_starFindPath(Node.ListNode, Node.MatrixNodeDistance, agv1.CurrentNode, instead_node); AGV.SimFullPathOfAGV[1] = Navigation.GetNavigationFrame(newpath, Node.MatrixNodeOrient); CollisionType = 4; agv1.Status = "Waitting"; } } else if (agv1.CurrentNode == goal2 && CollisionType == 0) { if (agv2_nextNode == goal2) { int instead_node = GotoNodeNeibor(agv2.CurrentNode, agv1.CurrentOrient, goal2); List <int> newpath = Algorithm.A_starFindPath(Node.ListNode, Node.MatrixNodeDistance, agv2.CurrentNode, instead_node); AGV.SimFullPathOfAGV[2] = Navigation.GetNavigationFrame(newpath, Node.MatrixNodeOrient); CollisionType = 5; agv2.Status = "Waitting"; } } else if (agv1_nextNode == agv2_nextNode) // va cham cheo { if (agv1_nextNode_1 != agv2.CurrentNode && agv2_nextNode_1 != agv1.CurrentNode && CollisionType == 0) { List <Node> Nodes = DBUtility.GetDataFromDB <List <Node> >("NodeInfoTable"); int x = Nodes[agv1_nextNode].X; int y = Nodes[agv1_nextNode].Y; int x1 = Nodes[agv1.CurrentNode].X; int y1 = Nodes[agv1.CurrentNode].Y; int x2 = Nodes[agv2.CurrentNode].X; int y2 = Nodes[agv2.CurrentNode].Y; double d1 = 0, d2 = 0; switch (agv1.CurrentOrient) { case 'E': d1 = Math.Sqrt(Math.Pow(x - x1 - agv1.DistanceToCurrentNode, 2) + Math.Pow(y - y1, 2)); break; case 'W': d1 = Math.Sqrt(Math.Pow(x - x1 + agv1.DistanceToCurrentNode, 2) + Math.Pow(y - y1, 2)); break; case 'S': d1 = Math.Sqrt(Math.Pow(x - x1, 2) + Math.Pow(y - y1 - agv1.DistanceToCurrentNode, 2)); break; case 'N': d1 = Math.Sqrt(Math.Pow(x - x1, 2) + Math.Pow(y - y1 + agv1.DistanceToCurrentNode, 2)); break; } switch (agv2.CurrentOrient) { case 'E': d2 = Math.Sqrt(Math.Pow(x - x2 - agv2.DistanceToCurrentNode, 2) + Math.Pow(y - y2, 2)); break; case 'W': d2 = Math.Sqrt(Math.Pow(x - x2 + agv2.DistanceToCurrentNode, 2) + Math.Pow(y - y2, 2)); break; case 'S': d2 = Math.Sqrt(Math.Pow(x - x2, 2) + Math.Pow(y - y2 - agv2.DistanceToCurrentNode, 2)); break; case 'N': d2 = Math.Sqrt(Math.Pow(x - x2, 2) + Math.Pow(y - y2 + agv2.DistanceToCurrentNode, 2)); break; } if (d1 < 100 && agv1.CurrentOrient != agv2.CurrentOrient) { agv1.IsColision = false; agv2.IsColision = true; agv2.Status = "Pausing"; CollisionType = 1; } else if (d2 < 100 && agv1.CurrentOrient != agv2.CurrentOrient) { agv1.Status = "Pausing"; agv1.IsColision = true; agv2.IsColision = false; CollisionType = 1; } } else if (agv1_nextNode_1 == agv2.CurrentNode && agv2_nextNode_1 != agv1.CurrentNode && CollisionType == 0) { //// agv1.IsColision = true; agv2.IsColision = false; CollisionType = 1; agv1.Status = "Pausing"; } else if (agv1_nextNode_1 != agv2.CurrentNode && agv2_nextNode_1 == agv1.CurrentNode && CollisionType == 0) { agv1.IsColision = false; agv2.IsColision = true; CollisionType = 1; agv2.Status = "Pausing"; } else if (agv1_nextNode_1 == agv2.CurrentNode && agv2_nextNode_1 == agv1.CurrentNode && CollisionType == 0) { int instead_node = GotoNodeNeibor(agv1.CurrentNode, agv2.CurrentOrient, goal2); List <int> newpath = Algorithm.A_starFindPath(Node.ListNode, Node.MatrixNodeDistance, agv1.CurrentNode, instead_node); AGV.SimFullPathOfAGV[1] = Navigation.GetNavigationFrame(newpath, Node.MatrixNodeOrient); CollisionType = 2; agv1.Status = "Avoid"; } } else if (agv1_nextNode == agv2.CurrentNode && agv2_nextNode == agv1.CurrentNode && CollisionType == 0) { int instead_node = GotoNodeNeibor(agv1.CurrentNode, agv2.CurrentOrient, goal2); List <int> newpath = Algorithm.A_starFindPath(Node.ListNode, Node.MatrixNodeDistance, agv1.CurrentNode, instead_node); //path_tam = AGV.FullPathOfAGV[1]; AGV.SimFullPathOfAGV[1] = Navigation.GetNavigationFrame(newpath, Node.MatrixNodeOrient); CollisionType = 2; agv1.Status = "Avoid"; } else { if (CollisionType == 2) { int node_ss = ReturnToNodeForward(agv2.CurrentNode); //int node_ss = GotoNodeNeibor(agv2.CurrentNode, agv2.CurrentOrient, goal2); if (agv1.CurrentNode == node_ss) { List <int> newpath = Algorithm.A_starFindPath(Node.ListNode, Node.MatrixNodeDistance, agv1.CurrentNode, goal1); AGV.SimFullPathOfAGV[1] = Navigation.GetNavigationFrame(newpath, Node.MatrixNodeOrient); agv1.PathCopmpleted = 1; CollisionType = 0; agv1.Status = "Running"; } } else if (CollisionType == 3) { int node_ss = ReturnToNodeForward(agv1.CurrentNode); //int node_ss = GotoNodeNeibor(agv1.CurrentNode, agv1.CurrentOrient, goal1); if (agv2.CurrentNode == node_ss) { List <int> newpath = Algorithm.A_starFindPath(Node.ListNode, Node.MatrixNodeDistance, agv2.CurrentNode, goal2); AGV.SimFullPathOfAGV[2] = Navigation.GetNavigationFrame(newpath, Node.MatrixNodeOrient); agv2.PathCopmpleted = 1; CollisionType = 0; agv1.Status = "Running"; } } else if (CollisionType == 4) { if (agv2.CurrentNode == agv1.Path[0][agv1.Path[0].Count - 2]) { List <int> newpath = Algorithm.A_starFindPath(Node.ListNode, Node.MatrixNodeDistance, agv1.CurrentNode, goal1); AGV.SimFullPathOfAGV[1] = Navigation.GetNavigationFrame(newpath, Node.MatrixNodeOrient); agv1.PathCopmpleted = 1; CollisionType = 0; agv1.Status = "Running"; } } else if (CollisionType == 5) { if (agv1.CurrentNode == agv2.Path[0][agv2.Path[0].Count - 2]) { List <int> newpath = Algorithm.A_starFindPath(Node.ListNode, Node.MatrixNodeDistance, agv2.CurrentNode, goal2); AGV.SimFullPathOfAGV[2] = Navigation.GetNavigationFrame(newpath, Node.MatrixNodeOrient); agv2.PathCopmpleted = 1; CollisionType = 0; agv2.Status = "Running"; } } else if (CollisionType == 1) { agv1.IsColision = false; agv2.IsColision = false; CollisionType = 0; agv1.Status = "Running"; agv2.Status = "Running"; } //i = 0; } }
// public static System.Windows.Forms.Timer timerToSendPath2Again; public static void GetDataRecieve() { int rxBufferSize = 18; byte[] rxBuffer = new byte[rxBufferSize]; int rxByteCount = Communication.SerialPort.Read(rxBuffer, 0, rxBufferSize); // add to a list of bytes received for (int i = 0; i < rxByteCount; i++) { bytesReceived.Add(rxBuffer[i]); } int startIndex = 0; byte functionCode = new byte(); // check header if (bytesReceived.Count < 3) { return; } for (int i = 0; i < bytesReceived.Count - 3; i++) { if (bytesReceived[i] == 0xAA && bytesReceived[i + 1] == 0xFF) { startIndex = i; functionCode = bytesReceived[i + 2]; if (functionCode == 0xA0) { // waitting for receive enough frame data of this function code if (bytesReceived.Count - startIndex < PIDInfoReceivePacketSize) { return; } // put data in an array byte[] data = new byte[PIDInfoReceivePacketSize]; for (int j = 0; j < PIDInfoReceivePacketSize; j++) { data[j] = bytesReceived[startIndex + j]; } PIDInfoReceivePacket receiveFrame = PIDInfoReceivePacket.FromArray(data); // check sum // ushort crc = 0; // for (int j = 0; j < PIDInfoReceivePacketSize - 4; j++) // crc += data[j]; // if (crc != receiveFrame.CheckSum) continue; bytesReceived.RemoveRange(0, startIndex + PIDInfoReceivePacketSize - 1); // update AGV info to lists of AGVs (real-time mode) // if (receiveFrame.Header[0] == 0xAA && receiveFrame.Header[1] == 0xFF && receiveFrame.EndOfFrame[0] == 0x0D && receiveFrame.EndOfFrame[1] == 0x0A) speed = receiveFrame.Velocity; //dk_Speed = receiveFrame.UdkVelocity; line = receiveFrame.LinePos; var agv = AGV.ListAGV.Find(a => a.ID == receiveFrame.AGVID); if (agv == null) { continue; } //udk_LinePos = receiveFrame.UdkLinePos; //if(receiveFrame.CurrentNode >=0 || receiveFrame.CurrentNode <= 56) agv.CurrentNode = receiveFrame.CurrentNode; agv.CurrentOrient = receiveFrame.currentOrient; agv.Velocity = receiveFrame.Velocity; agv.DistanceToCurrentNode = receiveFrame.distanceToPreNode; //Display.UpdateComStatus("receive", agv.ID, "Receive AGV Information", Color.Green); } if (functionCode == 0xE0) //Xac nhan chay xong Path1 { // waitting for receive enough frame data of this function code if (bytesReceived.Count - startIndex < PathCompleteReceivePacketSize) { return; } // put data in an array byte[] data = new byte[PathCompleteReceivePacketSize]; for (int j = 0; j < PathCompleteReceivePacketSize; j++) { data[j] = bytesReceived[startIndex + j]; } PathCompleteRecievePacket receiveFrame = PathCompleteRecievePacket.FromArray(data); bytesReceived.RemoveRange(0, startIndex + PathCompleteReceivePacketSize - 1); Debug.Write("PathRun_Completed"); AGV agv = AGV.ListAGV[0]; agv.PathCopmpleted = 1; Debug.WriteLine("PathComplete = 1"); FlagPath1Complete++; if (FlagPath1Complete == 1) { Display.UpdateComStatus("receive", agv.ID, " AGV Complete Path 1", Color.Green); } } if (functionCode == 0xE1) //Xac nhan chay xong Path2 { // waitting for receive enough frame data of this function code if (bytesReceived.Count - startIndex < PathCompleteReceivePacketSize) { return; } // put data in an array byte[] data = new byte[PathCompleteReceivePacketSize]; for (int j = 0; j < PathCompleteReceivePacketSize; j++) { data[j] = bytesReceived[startIndex + j]; } PathCompleteRecievePacket receiveFrame = PathCompleteRecievePacket.FromArray(data); bytesReceived.RemoveRange(0, startIndex + PathCompleteReceivePacketSize - 1); Debug.Write("PathRun_Completed"); AGV agv = AGV.ListAGV[0]; agv.PathCopmpleted = 3; Debug.WriteLine("PathComplete = 3"); FlagPath2Complete++; if (FlagPath2Complete == 1) { Display.UpdateComStatus("receive", agv.ID, " AGV Complete Path 2", Color.Green); } } if (functionCode == 0xC1) // ACK Speed { if (bytesReceived.Count - startIndex < PathCompleteReceivePacketSize) { return; } // put data in an array byte[] data = new byte[PathCompleteReceivePacketSize]; for (int j = 0; j < PathCompleteReceivePacketSize; j++) { data[j] = bytesReceived[startIndex + j]; } PathCompleteRecievePacket receiveFrame = PathCompleteRecievePacket.FromArray(data); bytesReceived.RemoveRange(0, startIndex + PathCompleteReceivePacketSize - 1); if (DashboardForm.timerToSendSpeedAgain.Enabled == true) { DashboardForm.timerToSendSpeedAgain.Stop(); } Display.UpdateComStatus("receive", receiveFrame.AGVID, " ACK Speed", Color.Green); } else if (functionCode == 0xD0) //NAK Speed { if (bytesReceived.Count - startIndex < PathCompleteReceivePacketSize) { return; } // put data in an array byte[] data = new byte[PathCompleteReceivePacketSize]; for (int j = 0; j < PathCompleteReceivePacketSize; j++) { data[j] = bytesReceived[startIndex + j]; } PathCompleteRecievePacket receiveFrame = PathCompleteRecievePacket.FromArray(data); bytesReceived.RemoveRange(0, startIndex + PathCompleteReceivePacketSize - 1); Display.UpdateComStatus("receive", receiveFrame.AGVID, " NAK Speed", Color.Red); // if (DashboardForm.timerToSendSpeedAgain.Enabled == true) // DashboardForm.timerToSendSpeedAgain.Stop(); DashboardForm.sendSpeedFrame(); } if (functionCode == 0xC2) //ACK Line { if (bytesReceived.Count - startIndex < PathCompleteReceivePacketSize) { return; } // put data in an array byte[] data = new byte[PathCompleteReceivePacketSize]; for (int j = 0; j < PathCompleteReceivePacketSize; j++) { data[j] = bytesReceived[startIndex + j]; } PathCompleteRecievePacket receiveFrame = PathCompleteRecievePacket.FromArray(data); bytesReceived.RemoveRange(0, startIndex + PathCompleteReceivePacketSize - 1); if (DashboardForm.timerToSendLineAgain.Enabled == true) { DashboardForm.timerToSendLineAgain.Stop(); } Display.UpdateComStatus("receive", receiveFrame.AGVID, " ACK Line", Color.Green); } else if (functionCode == 0xD1) //NAK Line { if (bytesReceived.Count - startIndex < PathCompleteReceivePacketSize) { return; } // put data in an array byte[] data = new byte[PathCompleteReceivePacketSize]; for (int j = 0; j < PathCompleteReceivePacketSize; j++) { data[j] = bytesReceived[startIndex + j]; } PathCompleteRecievePacket receiveFrame = PathCompleteRecievePacket.FromArray(data); bytesReceived.RemoveRange(0, startIndex + PathCompleteReceivePacketSize - 1); // if (DashboardForm.timerToSendLineAgain.Enabled == true) // DashboardForm.timerToSendLineAgain.Stop(); DashboardForm.sendLineFrame(); Display.UpdateComStatus("receive", receiveFrame.AGVID, " NAK Line", Color.Red); } if (functionCode == 0xC3) //ACK Path { if (bytesReceived.Count - startIndex < PathCompleteReceivePacketSize) { return; } // put data in an array byte[] data = new byte[PathCompleteReceivePacketSize]; for (int j = 0; j < PathCompleteReceivePacketSize; j++) { data[j] = bytesReceived[startIndex + j]; } PathCompleteRecievePacket receiveFrame = PathCompleteRecievePacket.FromArray(data); bytesReceived.RemoveRange(0, startIndex + PathCompleteReceivePacketSize - 1); FlagPath1Complete = 0; FlagPath2Complete = 0; if (AGV.ListAGV[0].PathCopmpleted == 1) { AGV.ListAGV[0].PathCopmpleted = 2; //if (DashboardForm.timerToSendPathAgain.Enabled == true) // DashboardForm.timerToSendPathAgain.Stop(); } else if (AGV.ListAGV[0].PathCopmpleted == 0) { AGV.ListAGV[0].Tasks[0].Status = "Doing"; AGV.ListAGV[0].Status = "Running"; // if (DashboardForm.timerToSendPathAgain.Enabled == true) // DashboardForm.timerToSendPathAgain.Stop(); } Debug.WriteLine("AcK Path"); Display.UpdateComStatus("receive", receiveFrame.AGVID, " ACK Path", Color.Green); } else if (functionCode == 0xD2) //NAK Path { if (bytesReceived.Count - startIndex < PathCompleteReceivePacketSize) { return; } // put data in an array byte[] data = new byte[PathCompleteReceivePacketSize]; for (int j = 0; j < PathCompleteReceivePacketSize; j++) { data[j] = bytesReceived[startIndex + j]; } PathCompleteRecievePacket receiveFrame = PathCompleteRecievePacket.FromArray(data); bytesReceived.RemoveRange(0, startIndex + PathCompleteReceivePacketSize - 1); if (AGV.ListAGV[0].PathCopmpleted == 1) { AGV.ListAGV[0].PathCopmpleted = 1; } else if (AGV.ListAGV[0].PathCopmpleted == 0) { AGV.ListAGV[0].PathCopmpleted = 0; // DashboardForm.timerToSendPathAgain.Stop(); // Task currentTask = AGV.ListAGV[0].Tasks[0]; // currentTask.Status = "Waiting"; // AGV.ListAGV[0].PathCopmpleted = 0; } Debug.WriteLine("NAK Path"); Display.UpdateComStatus("receive", receiveFrame.AGVID, " NAK Path", Color.Red); } } } }
public static void UpdatePathFromTaskOfAGV(AGV agv) { if (agv.Tasks.Count != 0) { Task currentTask = agv.Tasks[0]; if (currentTask.Status == "Waiting") { Debug.WriteLine("AAAAA"); agv.Path.Clear(); agv.Path.Add(Algorithm.A_starFindPath(Node.ListNode, Node.MatrixNodeDistance, agv.CurrentNode, agv.Tasks[0].PickNode)); AGV.FullPathOfAGV[agv.ID] = "N-0-" + agv.CurrentOrient + "-" + Navigation.GetNavigationFrame(agv.Path[0], Node.MatrixNodeOrient) + "-N-0"; Communication.SendPathData(AGV.FullPathOfAGV[agv.ID]); Display.UpdateComStatus("send", agv.ID, "Send Path 1", Color.Green); // if(DashboardForm.timerToSendPathAgain.Enabled == false) // DashboardForm.timerToSendPathAgain.Start(); // agv.Tasks[0].Status = "Doing"; agv.PathCopmpleted = 0; // agv.Status = "Running"; } else if (agv.CurrentNode == currentTask.PickNode && currentTask.Status == "Doing" && agv.PathCopmpleted == 1) { Debug.WriteLine("BBBB"); agv.Path.RemoveAt(0); agv.Path.Add(Algorithm.A_starFindPath(Node.ListNode, Node.MatrixNodeDistance, agv.CurrentNode, agv.Tasks[0].DropNode)); string pick, drop; if (currentTask.PickLevel == 1 || currentTask.PickLevel == 2 || currentTask.PickLevel == 3) { pick = "P-" + currentTask.PickLevel.ToString() + "-"; } else { pick = "N-0-"; } if (currentTask.DropLevel == 1 || currentTask.DropLevel == 2 || currentTask.DropLevel == 3) { drop = "-D-" + currentTask.DropLevel.ToString(); } else { drop = "-N-0"; } AGV.FullPathOfAGV[agv.ID] = pick + agv.CurrentOrient + "-" + Navigation.GetNavigationFrame(agv.Path[0], Node.MatrixNodeOrient) + drop; Communication.SendPathData(AGV.FullPathOfAGV[agv.ID]); Display.UpdateComStatus("send", agv.ID, "Send Path 2", Color.Green); // if (DashboardForm.timerToSendPathAgain.Enabled == false) // DashboardForm.timerToSendPathAgain.Start(); // agv.PathCopmpleted = 2; } else if (agv.CurrentNode == currentTask.DropNode && currentTask.Status == "Doing" && agv.PathCopmpleted == 3) { Debug.WriteLine("CCCC"); agv.Tasks.RemoveAt(0); agv.Path.Clear(); agv.Status = "Stop"; } } }
public static void SimUpdatePathFromTaskOfAGVs(AGV agv) { // Clear all path (this do not affect Task.SimListTask) //agv.Tasks.Clear(); // Find all task of this AGV //agv.Tasks = Task.SimListTask.FindAll(t => t.AGVID == agv.ID);&& agv.Path.Count ==0 // if not having task or path has been initialized, skip to next AGV if (agv.Tasks.Count != 0) { Task currentTask = agv.Tasks[0]; //agv.Path.RemoveAt(0);agv.CurrentNode != currentTask.PickNode && if (currentTask.Status == "Waiting") { agv.Path.Clear(); agv.Path.Add(Algorithm.A_starFindPath(Node.ListNode, Node.MatrixNodeDistance, agv.CurrentNode, agv.Tasks[0].PickNode)); AGV.SimFullPathOfAGV[agv.ID] = Navigation.GetNavigationFrame(agv.Path[0], Node.MatrixNodeOrient); agv.Tasks[0].Status = "Doing"; agv.PathCopmpleted = 0; agv.Status = "Running"; Display.SimLabelAGV[agv.ID].BackColor = Color.CornflowerBlue; } else if (agv.CurrentNode == currentTask.PickNode && currentTask.Status == "Doing" && agv.PathCopmpleted == 1) { agv.Path.RemoveAt(0); agv.Path.Add(Algorithm.A_starFindPath(Node.ListNode, Node.MatrixNodeDistance, agv.CurrentNode, agv.Tasks[0].DropNode)); AGV.SimFullPathOfAGV[agv.ID] = Navigation.GetNavigationFrame(agv.Path[0], Node.MatrixNodeOrient); agv.HavePallet = true; if (currentTask.Type == "Order") { Pallet pallet = Pallet.SimListPallet.Find(c => c.Code == currentTask.PalletCode); Display.DeleteLabelPallet(pallet); } else if (currentTask.Type == "Store") { } } else if ((agv.CurrentNode == currentTask.DropNode && currentTask.Status == "Doing" && agv.PathCopmpleted == 2)) { agv.Tasks.RemoveAt(0); agv.Path.Clear(); agv.Status = "Stop"; Display.SimLabelAGV[agv.ID].BackColor = Color.Silver; string timeComplete = DateTime.Now.ToString("dddd, MMMM dd, yyyy h:mm:ss tt"); DBUtility.InsertCompleteTaskToDB("SimHistoryTask", currentTask, timeComplete, "Done"); agv.HavePallet = false; if (currentTask.Type == "Order") { DBUtility.DeletePalletFromDB("SimPalletInfoTable", currentTask.PalletCode); Task.SimListTask.Remove(currentTask); } else if (currentTask.Type == "Store") { //agv.HavePallet = false; Pallet pallet = Pallet.SimStorePallet.Find(c => c.Code == currentTask.PalletCode); pallet.DeliverTime = DateTime.Now.ToString("dddd, MMMM dd, yyyy h:mm:ss tt"); DBUtility.InsertNewPalletToDB("SimPalletInfoTable", pallet.Code, pallet.Name, pallet.InStock, pallet.DeliverTime, pallet.AtBlock, pallet.AtColumn, pallet.AtLevel); Pallet.SimStorePallet.Remove(pallet); Pallet.SimListPallet.Add(pallet); Display.UpdateLabelPallet(pallet); Task.SimListTask.Remove(currentTask); } else if (currentTask.Type == "Input" || currentTask.Type == "Output") { Task.SimListTask.Remove(currentTask); } } } else { if (agv.CurrentNode == 55) { agv.Status = "Stop"; agv.Path.Clear(); } } }
public static void SimUpdatePositionAGV(int agvID, float speed, Panel pnFloor, Label lbagv, Label lbpallet) { //int step = (int)speed * 2 / 10; //1pixel = 0.5cm =>> 20cm/s=40pixel/s var index = AGV.SimListAGV.FindIndex(a => a.ID == agvID); AGV agv = AGV.SimListAGV[index]; //List<char> fullpath = AGV.FullPathOfAGV[agvID].ToList(); string[] frameArr = AGV.SimFullPathOfAGV[agvID].Split(new char[] { '-' }, StringSplitOptions.RemoveEmptyEntries); Point oldAGVPosition = Display.SimLabelAGV[agvID].Location; Point newAGVPosition = new Point(); Point oldPalletPosition = Display.SimLabelPalletInAGV[agvID].Location; Point newPalletPosition = new Point(); Size oldPalletSize = Display.SimLabelPalletInAGV[agvID].Size; Size newPalletSize = new Size(); int indexNode = Array.FindIndex(frameArr, a => a == agv.CurrentNode.ToString()); if (agv.Stop) { return; } if (agv.IsColision) { return; } if (frameArr[indexNode + 1] == "G" || frameArr[indexNode + 1] == null) { agv.PathCopmpleted++; if (agv.PathCopmpleted == 1 || agv.PathCopmpleted == 0) { Display.Points = new Point[] { new Point(), new Point() }; pnFloor.Refresh(); } } else { int currentNode = agv.CurrentNode; int nextNode = Convert.ToInt32(frameArr[indexNode + 2]); if (agv.CurrentOrient != Display.UpdateOrient(frameArr[indexNode + 1])) { agv.DistanceToCurrentNode -= speed / 5.2f; if (agv.DistanceToCurrentNode <= 0) { agv.CurrentOrient = Display.UpdateOrient(frameArr[indexNode + 1]); agv.DistanceToCurrentNode = 0; } } else { agv.DistanceToCurrentNode += speed / 5.2f; if (agv.DistanceToCurrentNode * 2 >= Node.MatrixNodeDistance[currentNode, nextNode]) { agv.DistanceToCurrentNode = 0; agv.CurrentNode = nextNode; if (frameArr[indexNode + 3] != "G") { agv.CurrentOrient = Display.UpdateOrient(frameArr[indexNode + 3]); } } } } int pixelDistance = (int)Math.Round(agv.DistanceToCurrentNode * 2); List <Node> Nodes = DBUtility.GetDataFromDB <List <Node> >("NodeInfoTable"); int x = Nodes[agv.CurrentNode].X - 50 / 2; int y = Nodes[agv.CurrentNode].Y - 50 / 2; switch (agv.CurrentOrient) { case 'E': newAGVPosition = new Point(x + pixelDistance, y); newPalletSize = new Size(15, 50); newPalletPosition = new Point(x + pixelDistance - 15, y); break; case 'W': newAGVPosition = new Point(x - pixelDistance, y); newPalletSize = new Size(15, 50); newPalletPosition = new Point(x - pixelDistance + 50, y); break; case 'S': newAGVPosition = new Point(x, y + pixelDistance); newPalletSize = new Size(50, 15); newPalletPosition = new Point(x, y + pixelDistance - 15); break; case 'N': newAGVPosition = new Point(x, y - pixelDistance); newPalletSize = new Size(50, 15); newPalletPosition = new Point(x, y - pixelDistance + 50); break; default: newAGVPosition = oldAGVPosition; newPalletPosition = oldPalletPosition; newPalletSize = oldPalletSize; break; } lbpallet.Size = newPalletSize; lbpallet.Location = newPalletPosition; //lbpallet.Visible = true; lbagv.Location = newAGVPosition; }