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;
}
