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;
}
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 void AddLabelAGV(string Mode, int agvID, int initExitNode, char initOrientation, float initDistanceToExitNode)
{
/// Create new label for AGV
Label lbAGV = new Label();
lbAGV.BackColor = Color.Silver;
lbAGV.Font = new Font("Microsoft Sans Serif", 9.75F, System.Drawing.FontStyle.Regular,
System.Drawing.GraphicsUnit.Point, ((byte)(0)));
lbAGV.Size = new Size(50, 50);
lbAGV.ForeColor = Color.Black;
lbAGV.Text = "AGV#" + agvID.ToString();
lbAGV.TextAlign = ContentAlignment.MiddleCenter;
lbAGV.Name = "AGV" + agvID.ToString();
/// Create new label for Pallet in AGV
Label pallet = new Label();
pallet.BackColor = Color.Goldenrod;
pallet.Size = new Size(50, 11);
pallet.ForeColor = Color.Black;
pallet.Font = new Font("Microsoft Sans Serif", 8.0F, System.Drawing.FontStyle.Regular,
System.Drawing.GraphicsUnit.Point, ((byte)(0)));
pallet.Text = "Pallet";
pallet.TextAlign = ContentAlignment.MiddleCenter;
//pallet.BorderStyle = BorderStyle.FixedSingle;
pallet.Name = "PalletInAGV" + agvID.ToString();
pallet.Visible = false;
int initPixelDistance = (int)Math.Round(initDistanceToExitNode * 2);
// Init the Location of new AGV
List <Node> Nodes = DBUtility.GetDataFromDB <List <Node> >("NodeInfoTable");
int x = Nodes[initExitNode].X - lbAGV.Size.Width / 2;
int y = Nodes[initExitNode].Y - lbAGV.Size.Height / 2;
switch (initOrientation)
{
case 'E':
lbAGV.Location = new Point(x + initPixelDistance, y);
pallet.Size = new Size(15, 50);
pallet.Location = new Point(x - initPixelDistance - 15, y);
break;
case 'W':
lbAGV.Location = new Point(x - initPixelDistance, y);
pallet.Size = new Size(15, 50);
pallet.Location = new Point(x + initPixelDistance + 50, y);
break;
case 'N':
lbAGV.Location = new Point(x, y - initPixelDistance);
pallet.Size = new Size(50, 15);
pallet.Location = new Point(x, y + initPixelDistance + 50);
break;
case 'S':
lbAGV.Location = new Point(x, y + initPixelDistance);
pallet.Size = new Size(50, 15);
pallet.Location = new Point(x, y - initPixelDistance - 15);
break;
}
// Add to Array for use
switch (Mode)
{
case "Real Time":
LabelAGV[agvID] = lbAGV;
LabelPalletInAGV[agvID] = pallet;
break;
case "Simulation":
SimLabelAGV[agvID] = lbAGV;
SimLabelPalletInAGV[agvID] = pallet;
break;
}
}
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;
}