// 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);
}
// Add next path to agv when previous path reach goal (agv.ExitNode is being goal) (Simulation Mode)
public static void AddNextPathOfSimAGV(AGV agv)
{
// Clear old path
agv.Path.Clear();
// Remove old task
if (agv.Tasks.Count != 0 && agv.ExitNode == agv.Tasks[0].DropNode)
{
// Store pallet code to SimListColumn at this goal node
RackColumn column = RackColumn.SimListColumn.Find(c => c.AtNode == agv.ExitNode);
column.PalletCodes[agv.Tasks[0].DropLevel - 1] = agv.Tasks[0].PalletCode;
if (column.AtNode != 51 & column.AtNode != 52)
{
Pallet pallet = new Pallet(agv.Tasks[0].PalletCode, true,
DateTime.Now.ToString("dddd, MMMM dd, yyyy h:mm:ss tt"),
column.Block, column.Number, agv.Tasks[0].DropLevel);
Pallet.SimListPallet.Add(pallet);
DBUtility.InsertNewPalletToDB("SimPalletInfoTable", pallet.Code, pallet.InStock, pallet.StoreTime,
pallet.AtBlock, pallet.AtColumn, pallet.AtLevel);
}
// Note: remove task in agv.Tasks and also in Task.SimListTask
Task.SimListTask.Remove(Task.SimListTask.Find(a => a.Name == agv.Tasks[0].Name));
agv.Tasks.RemoveAt(0);
}
// Add path to parking when don't have any task
if (agv.Tasks.Count == 0)
{
// find all parking node
List <int> parkingNode = new List <int>();
foreach (Node n in Node.ListNode)
{
if (n.LocationCode.Length == 0)
{
continue;
}
if (n.LocationCode[0] == 'P')
{
parkingNode.Add(n.ID);
}
}
// If current node is parking node or don't have parking node for this agv, do nothing,
// otherwise, add path to park agv (agv will park at location in order of index)
if (parkingNode.Contains(agv.ExitNode))
{
return;
}
int parkAtNode = parkingNode.Find(n => parkingNode.IndexOf(n) == AGV.SimListAGV.IndexOf(agv));
if (parkAtNode == 0)
{
return; // be careful: in my node definition, parking nodes don't have node 0
}
agv.Path = Algorithm.A_starFindPath(agv.ExitNode, parkAtNode);
// Add navigation frame of parking path
string fr = Navigation.GetNavigationFrame(agv.Path, agv.Orientation, agv.DistanceToExitNode);
agv.navigationArr = fr.Split(new char[] { ',' }, StringSplitOptions.RemoveEmptyEntries);
return;
}
// Add next path
if (agv.ExitNode != agv.Tasks[0].PickNode)
{
agv.Path = Algorithm.A_starFindPath(agv.ExitNode, agv.Tasks[0].PickNode);
}
else
{
agv.Path = Algorithm.A_starFindPath(agv.Tasks[0].PickNode, agv.Tasks[0].DropNode);
agv.Tasks[0].Status = "Doing";
// update button add pallet
if (agv.Tasks[0].PickNode == 53)
{
HomeScreenForm.isPickSimPalletInput1 = true;
}
else if (agv.Tasks[0].PickNode == 54)
{
HomeScreenForm.isPickSimPalletInput2 = true;
}
}
// Add next navigation frame of next path
string frame = Navigation.GetNavigationFrame(agv.Path, agv.Orientation, agv.DistanceToExitNode);
agv.navigationArr = frame.Split(new char[] { ',' }, StringSplitOptions.RemoveEmptyEntries);
}