// Predict coming time to remaining node on the path (first element is now time of current node)
private static List <DateTime> PredictTimeOnPath(AGV agv, DateTime timeNow)
{
// matrix of distance from node to other node (pixel)
int[,] D = Node.MatrixNodeDistance;
List <DateTime> timePath = new List <DateTime>();
timePath.Add(timeNow);
foreach (int n in agv.Path)
{
// don't predict node which is passed and current node
if (agv.Path.IndexOf(n) <= agv.Path.IndexOf(agv.ExitNode))
{
continue;
}
int pre_n = agv.Path[agv.Path.IndexOf(n) - 1];
float disToPreNode;
// disToPreNode is different in this case (pre_n is current node)
if (agv.Path.IndexOf(n) == agv.Path.IndexOf(agv.ExitNode) + 1)
{
disToPreNode = D[agv.ExitNode, n] / Display.Scale - agv.DistanceToExitNode;
}
else
{
disToPreNode = D[pre_n, n] / Display.Scale;
}
float time_Line = disToPreNode / AGV.SimSpeed;
float time_Turning;
int pre_idx = Array.IndexOf(agv.navigationArr, pre_n.ToString());
string pre_dir = agv.navigationArr[pre_idx + 1];
if (pre_dir == "L" || pre_dir == "R")
{
time_Turning = SimTurningTime;
}
else if (pre_dir == "B")
{
time_Turning = SimTurningTime;
}
else
{
time_Turning = 0;
}
timePath.Add(timePath[timePath.Count - 1].AddSeconds(time_Turning + time_Line));
}
return(timePath);
}
// 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);
}
public static void SendVelocitySetting(uint agvID, float velocity)
{
SetVelocityPacket sendFrame = new SetVelocityPacket();
sendFrame.Header = new byte[2] {
0xAA, 0xFF
};
sendFrame.FunctionCode = (byte)FUNC_CODE.WR_VELOCITY;
sendFrame.AGVID = Convert.ToByte(agvID);
sendFrame.Velocity = velocity;
// calculate check sum
ushort crc = 0;
crc += sendFrame.Header[0];
crc += sendFrame.Header[1];
crc += sendFrame.FunctionCode;
crc += sendFrame.AGVID;
crc += BitConverter.GetBytes(sendFrame.Velocity)[0];
crc += BitConverter.GetBytes(sendFrame.Velocity)[1];
crc += BitConverter.GetBytes(sendFrame.Velocity)[2];
crc += BitConverter.GetBytes(sendFrame.Velocity)[3];
sendFrame.CheckSum = crc;
sendFrame.EndOfFrame = new byte[2] {
0x0A, 0x0D
};
// send data via serial port
AGV agv = AGV.ListAGV.Find(a => a.ID == (int)agvID);
if (!agv.IsInitialized)
{
return;
}
try { Communicator.SerialPort.Write(sendFrame.ToArray(), 0, sendFrame.ToArray().Length); }
catch { };
// Display ComStatus
Display.UpdateComStatus("send", sendFrame.AGVID, "Set Velocity", System.Drawing.Color.Blue);
// wait ack
System.Timers.Timer timer4 = new System.Timers.Timer(timeout);
timer4.Elapsed += (sender, e) => timer4_Elapsed(sender, e, sendFrame);
timer4.Start();
}
// Send AGV info request packet (InfoType = 'A' for all except line tracking error, 'L' for line tracking error)
public static void SendAGVInfoRequest(uint agvID, char InfoType)
{
AGVInfoRequestPacket requestFrame = new AGVInfoRequestPacket();
requestFrame.Header = new byte[2] {
0xAA, 0xFF
};
requestFrame.FunctionCode = (byte)FUNC_CODE.REQ_AGV_INFO;
requestFrame.AGVID = Convert.ToByte(agvID);
requestFrame.InformationType = (byte)InfoType;
// calculate check sum
ushort crc = 0;
crc += requestFrame.Header[0];
crc += requestFrame.Header[1];
crc += requestFrame.FunctionCode;
crc += requestFrame.AGVID;
crc += requestFrame.InformationType;
requestFrame.CheckSum = crc;
requestFrame.EndOfFrame = new byte[2] {
0x0A, 0x0D
};
// send data via serial port
AGV agv = AGV.ListAGV.Find(a => a.ID == (int)agvID);
if (!agv.IsInitialized)
{
return;
}
try { Communicator.SerialPort.Write(requestFrame.ToArray(), 0, requestFrame.ToArray().Length); }
catch { };
// Display ComStatus
Display.UpdateComStatus("send", requestFrame.AGVID, "Request AGV info", System.Drawing.Color.Blue);
// wait ack
System.Timers.Timer timer1 = new System.Timers.Timer(timeout);
timer1.Elapsed += (sender, e) => timer1_Elapsed(sender, e, requestFrame);
timer1.Start();
}
private void UpdateMonitoringData(AGV agv, double linetrackingError)
{
// update agv info
lbStatus.Text = agv.Status;
lbExitNode.Text = agv.ExitNode.ToString();
lbOrient.Text = agv.Orientation.ToString();
lbDistance.Text = Math.Round(agv.DistanceToExitNode, 1).ToString() + " cm";
lbVelocity.Text = Math.Round(agv.Velocity, 1).ToString() + " cm/s";
lbBattery.Text = agv.Battery.ToString() + "%";
prgrbBattery.Value = agv.Battery;
// update current path and highlight current node
rtxtbCurrentPath.Clear();
if (agv.Path.Count == 0)
{
rtxtbCurrentPath.SelectedText = "None";
}
foreach (int n in agv.Path)
{
if (n == agv.ExitNode)
{
rtxtbCurrentPath.SelectionBackColor = Color.Yellow;
}
else
{
rtxtbCurrentPath.SelectionBackColor = Color.Lavender;
}
if (n != agv.Path[agv.Path.Count - 1])
{
rtxtbCurrentPath.SelectedText = n.ToString() + "->";
}
else
{
rtxtbCurrentPath.SelectedText = n.ToString();
}
}
// update graph
DrawGraph(zedGraphVelocity, agv.Velocity);
DrawGraph(zedGraphLineTrack, linetrackingError);
}
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(agv.ExitNode, agv.Tasks[0].DropNode));
}
else if (i == 0 && agv.Tasks[0].Status == "Waiting")
{
listRemainingPath.Add(Algorithm.A_starFindPath(agv.ExitNode, agv.Tasks[0].PickNode));
listRemainingPath.Add(Algorithm.A_starFindPath(agv.Tasks[0].PickNode, agv.Tasks[0].DropNode));
}
else
{
listRemainingPath.Add(Algorithm.A_starFindPath(agv.Tasks[i].PickNode, agv.Tasks[i].DropNode));
}
if (i < agv.Tasks.Count - 1)
{
listRemainingPath.Add(Algorithm.A_starFindPath(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);
}
private void btnRemove_Click(object sender, EventArgs e)
{
// Remove an AGV has ID in comboBox
if (String.IsNullOrEmpty(cbbID.Text) == false)
{
List <AGV> listAll = listOldAGV.Concat(listNewAGV).ToList();
AGV agvToRemove = listAll.Find(a => { return(a.ID == Convert.ToInt16(cbbID.Text)); });
if (listOldAGV.Contains(agvToRemove))
{
listOldAGV.Remove(agvToRemove);
}
if (listNewAGV.Contains(agvToRemove))
{
listNewAGV.Remove(agvToRemove);
}
}
// Put remaining AGV in listView
listViewAGV.Items.Clear();
foreach (AGV agv in listOldAGV.Concat(listNewAGV).ToList())
{
listViewAGV.Items.Add(" AGV#" + agv.ID, 0);
}
}
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;
}
// Check whether AGV ID exist in old and new list or not
foreach (AGV a in listOldAGV.Concat(listNewAGV).ToList())
{
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 agv = new AGV(Convert.ToInt16(txbID.Text), Convert.ToInt16(cbbExitNode.Text),
Convert.ToChar(cbbOrientation.Text), Convert.ToSingle(txbDistance.Text), "Stop");
if (Display.Mode == "Simulation")
{
agv.IsInitialized = true;
}
listNewAGV.Add(agv);
// Put new AGV ID in listView
listViewAGV.Items.Add(" AGV#" + agv.ID, 0);
// Clear textBox for next adding
txbID.Clear();
txbDistance.Clear();
}
// Send path information packet
public static void SendPathData(uint agvID, bool isPick, int pickLevel, string strNavigationFrame, bool isDrop, int dropLevel)
{
PathInfoSendPacket sendFrame = new PathInfoSendPacket();
// get path info (to byte array)
string[] arrNavigationFrame = strNavigationFrame.Split(new char[] { ',' }, StringSplitOptions.RemoveEmptyEntries);
byte[] arrPathFrame = new byte[arrNavigationFrame.Length + 4]; // 4 bytes of pick/drop info
if (isPick == true)
{
arrPathFrame[0] = (byte)'P';
}
else
{
arrPathFrame[0] = (byte)'N';
}
arrPathFrame[1] = Convert.ToByte(pickLevel);
if (isDrop == true)
{
arrPathFrame[arrNavigationFrame.Length + 2] = (byte)'D';
}
else
{
arrPathFrame[arrNavigationFrame.Length + 2] = (byte)'N';
}
arrPathFrame[arrNavigationFrame.Length + 3] = Convert.ToByte(dropLevel);
for (int i = 0; i < arrNavigationFrame.Length; i++)
{
if (i % 2 == 0)
{
arrPathFrame[i + 2] = (byte)(arrNavigationFrame[i][0]);
}
else
{
arrPathFrame[i + 2] = Convert.ToByte(arrNavigationFrame[i]);
}
}
// set frame to send as struct
// note: send reversed Header and End-of-frame because of Intel processors (in my laptop) use little endian
sendFrame.Header = new byte[2] {
0xAA, 0xFF
};
sendFrame.FunctionCode = (byte)FUNC_CODE.WR_PATH;
sendFrame.AGVID = Convert.ToByte(agvID);
sendFrame.PathByteCount = Convert.ToByte(arrPathFrame.Length);
sendFrame.Path = arrPathFrame;
// calculate check sum
ushort crc = 0;
crc += sendFrame.Header[0];
crc += sendFrame.Header[1];
crc += sendFrame.FunctionCode;
crc += sendFrame.AGVID;
crc += sendFrame.PathByteCount;
Array.ForEach(arrPathFrame, x => crc += x);
sendFrame.CheckSum = crc;
sendFrame.EndOfFrame = new byte[2] {
0x0A, 0x0D
};
// send data via serial port
AGV agv = AGV.ListAGV.Find(a => a.ID == (int)agvID);
if (!agv.IsInitialized)
{
return;
}
try { Communicator.SerialPort.Write(sendFrame.ToArray(), 0, sendFrame.ToArray().Length); }
catch { };
// Display ComStatus
Display.UpdateComStatus("send", sendFrame.AGVID, "Write Path", System.Drawing.Color.Blue);
// wait ack
System.Timers.Timer timer2 = new System.Timers.Timer(timeout);
timer2.Elapsed += (sender, e) => timer2_Elapsed(sender, e, sendFrame);
timer2.Start();
}
public static void GetData()
{
int rxBufferSize = 25;
byte[] rxBuffer = new byte[rxBufferSize];
int rxByteCount = Communicator.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 == (byte)FUNC_CODE.RESP_AGV_INFO) // receive AGV info except line tracking error
{
// waitting for receive enough frame data of this function code
if (bytesReceived.Count - startIndex < AGVInfoReceivePacketSize)
{
return;
}
// put data in an array
byte[] data = new byte[AGVInfoReceivePacketSize];
for (int j = 0; j < AGVInfoReceivePacketSize; j++)
{
data[j] = bytesReceived[startIndex + j];
}
AGVInfoReceivePacket receiveFrame = AGVInfoReceivePacket.FromArray(data);
// check sum
ushort crc = 0;
for (int j = 0; j < AGVInfoReceivePacketSize - 4; j++)
{
crc += data[j];
}
if (crc != receiveFrame.CheckSum)
{
continue;
}
bytesReceived.RemoveRange(0, startIndex + AGVInfoReceivePacketSize - 1);
// update AGV info to lists of AGVs (real-time mode)
var agv = AGV.ListAGV.Find(a => a.ID == receiveFrame.AGVID);
if (agv == null)
{
continue;
}
switch (Convert.ToChar(receiveFrame.Status).ToString())
{
case "R": agv.Status = "Running"; break;
case "S": agv.Status = "Stop"; break;
case "P": agv.Status = "Picking"; break;
case "D": agv.Status = "Dropping"; break;
}
switch (Convert.ToChar(receiveFrame.Orient).ToString())
{
case "E": agv.Orientation = 'E'; break;
case "W": agv.Orientation = 'W'; break;
case "S": agv.Orientation = 'S'; break;
case "N": agv.Orientation = 'N'; break;
}
agv.ExitNode = receiveFrame.ExitNode;
agv.DistanceToExitNode = receiveFrame.DisToExitNode;
agv.Velocity = receiveFrame.Velocity;
agv.Battery = receiveFrame.Battery;
}
else if (functionCode == (byte)FUNC_CODE.RESP_LINE_TRACK_ERR) // receive Line tracking error
{
// waitting for receive enough frame data of this function code
if (bytesReceived.Count - startIndex < AGVLineTrackErrorReceivePacketSize)
{
return;
}
// get data and take it out of the queue
byte[] data = new byte[AGVLineTrackErrorReceivePacketSize];
for (int j = 0; j < AGVLineTrackErrorReceivePacketSize; j++)
{
data[j] = bytesReceived[startIndex + j];
}
AGVLineTrackErrorReceivePacket receiveFrame = AGVLineTrackErrorReceivePacket.FromArray(data);
// check sum
ushort crc = 0;
for (int j = 0; j < AGVLineTrackErrorReceivePacketSize - 4; j++)
{
crc += data[j];
}
if (crc != receiveFrame.CheckSum)
{
continue;
}
bytesReceived.RemoveRange(0, startIndex + AGVLineTrackErrorReceivePacketSize - 1);
// update Line tracking error value
if (AGVMonitoringForm.selectedAGVID == receiveFrame.AGVID)
{
lineTrackError = receiveFrame.LineTrackError;
}
else
{
lineTrackError = 0;
}
}
else if (functionCode == (byte)FUNC_CODE.RESP_ACK_PATH ||
functionCode == (byte)FUNC_CODE.RESP_ACK_AGV_INFO ||
functionCode == (byte)FUNC_CODE.RESP_ACK_WAITING ||
functionCode == (byte)FUNC_CODE.RESP_ACK_VELOCITY ||
functionCode == (byte)FUNC_CODE.RESP_ACK_INIT) // receive ack
{
// waitting for receive enough frame data of this function code
if (bytesReceived.Count - startIndex < AckReceivePacketSize)
{
return;
}
// get data and take it out of the queue
byte[] data = new byte[AckReceivePacketSize];
for (int j = 0; j < AckReceivePacketSize; j++)
{
data[j] = bytesReceived[startIndex + j];
}
AckReceivePacket receiveFrame = AckReceivePacket.FromArray(data);
// check sum
ushort crc = 0;
for (int j = 0; j < AckReceivePacketSize - 4; j++)
{
crc += data[j];
}
if (crc != receiveFrame.CheckSum)
{
continue;
}
bytesReceived.RemoveRange(0, startIndex + AckReceivePacketSize - 1);
// update received ack
ackReceived = receiveFrame;
// if receive ACK of Initialization, set agv.Initialization = true
if (functionCode == (byte)FUNC_CODE.RESP_ACK_INIT && receiveFrame.ACK == (byte)'Y')
{
AGV agv = AGV.ListAGV.Find(a => a.ID == (int)receiveFrame.AGVID);
agv.IsInitialized = true;
agv.Status = "Initialized";
}
// Display ComStatus
string message = "";
if (receiveFrame.ACK == (byte)'Y')
{
if (functionCode == (byte)FUNC_CODE.RESP_ACK_PATH)
{
message = "ACK (path)";
}
else if (functionCode == (byte)FUNC_CODE.RESP_ACK_AGV_INFO)
{
message = "ACK (request AGV info)";
}
else if (functionCode == (byte)FUNC_CODE.RESP_ACK_WAITING)
{
message = "ACK (waiting)";
}
else if (functionCode == (byte)FUNC_CODE.RESP_ACK_VELOCITY)
{
message = "ACK (velocity setting)";
}
else if (functionCode == (byte)FUNC_CODE.RESP_ACK_INIT)
{
message = "ACK (initialized)";
}
Display.UpdateComStatus("receive", receiveFrame.AGVID, message, System.Drawing.Color.Green);
}
else if (receiveFrame.ACK == (byte)'N')
{
if (functionCode == (byte)FUNC_CODE.RESP_ACK_PATH)
{
message = "NACK (path)";
}
else if (functionCode == (byte)FUNC_CODE.RESP_ACK_AGV_INFO)
{
message = "NACK (request AGV info)";
}
else if (functionCode == (byte)FUNC_CODE.RESP_ACK_WAITING)
{
message = "NACK (waiting)";
}
else if (functionCode == (byte)FUNC_CODE.RESP_ACK_VELOCITY)
{
message = "NACK (velocity setting)";
}
else if (functionCode == (byte)FUNC_CODE.RESP_ACK_INIT)
{
message = "NACK (initialize)";
}
Display.UpdateComStatus("receive", receiveFrame.AGVID, message, System.Drawing.Color.Red);
}
}
}
}
}
// 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);
}
// Collision detection
public static void Detect(List <AGV> listAGV)
{
if (listAGV.Count < 2)
{
return;
}
// matrix of distance from node to other node (pixel)
int[,] D = Node.MatrixNodeDistance;
// Check each pair of agvs
for (int i = 0; i < listAGV.Count - 1; i++)
{
for (int j = i + 1; j < listAGV.Count; j++)
{
AGV agv1 = listAGV[i];
AGV agv2 = listAGV[j];
// if no path, do not detect
if (agv1.Path.Count == 0 || agv2.Path.Count == 0)
{
continue;
}
// predict coming time to remaining node on the path
List <DateTime> timePath1 = new List <DateTime>();
List <DateTime> timePath2 = new List <DateTime>();
DateTime timeNow = DateTime.Now;
timePath1 = PredictTimeOnPath(agv1, timeNow);
timePath2 = PredictTimeOnPath(agv2, timeNow);
// get remaining path of each agv
List <int> remainingPath1 = new List <int>();
List <int> remainingPath2 = new List <int>();
foreach (int n in agv1.Path)
{
// don't get node which is passed
if (agv1.Path.IndexOf(n) < agv1.Path.IndexOf(agv1.ExitNode))
{
continue;
}
remainingPath1.Add(n);
}
foreach (int n in agv2.Path)
{
// don't get node which is passed
if (agv2.Path.IndexOf(n) < agv2.Path.IndexOf(agv2.ExitNode))
{
continue;
}
remainingPath2.Add(n);
}
List <int> remainingPath2_reverse = new List <int>(remainingPath2);
remainingPath2_reverse.Reverse();
// detect overlap nodes on two paths
List <int> overLapNodes = new List <int>();
for (int w_size = remainingPath1.Count; w_size > 0; w_size--)
{
for (int idx1 = 0; idx1 <= remainingPath1.Count - w_size; idx1++)
{
for (int idx2 = 0; idx2 <= remainingPath2_reverse.Count - w_size; idx2++)
{
var w1 = remainingPath1.GetRange(idx1, w_size);
var w2 = remainingPath2_reverse.GetRange(idx2, w_size);
if (w1.SequenceEqual(w2))
{
overLapNodes = w1;
break;
}
}
if (overLapNodes.Count > 0)
{
break;
}
}
if (overLapNodes.Count > 0)
{
break;
}
}
// no overlapping
if (overLapNodes.Count < 1)
{
continue;
}
// overall time of overlapping
int idxOverLapBegin1 = remainingPath1.IndexOf(overLapNodes[0]);
int idxOverLapEnd1 = remainingPath1.IndexOf(overLapNodes[overLapNodes.Count - 1]);
TimeSpan deltaTOverLap = timePath1[idxOverLapEnd1].Subtract(timePath1[idxOverLapBegin1]);
int idxOverLapBegin2 = remainingPath2.IndexOf(overLapNodes[overLapNodes.Count - 1]);
DateTime time1 = timePath1[idxOverLapBegin1];
DateTime time2 = timePath2[idxOverLapBegin2];
TimeSpan deltaT = time1.Subtract(time2).Duration();
// detect head-on collision
if (deltaT < deltaTOverLap)
{
// waiting for later AGV to come close to the overlapping zone
if ((time1 >= time2 && time1 > timeNow.AddSeconds(AGV.Length / AGV.SimSpeed)) ||
(time1 < time2 && time2 > timeNow.AddSeconds(AGV.Length / AGV.SimSpeed)))
{
continue;
}
// AGV coming to overlap-begin-node later must wait or re-route (put at collision.OnAGVs[0])
int[] collisionOnAGVs = new int[2];
if (time1 >= time2)
{
collisionOnAGVs[0] = agv1.ID; collisionOnAGVs[1] = agv2.ID;
}
else
{
collisionOnAGVs[0] = agv2.ID; collisionOnAGVs[1] = agv1.ID;
}
// -----------add real-time later--------------
Collision collision = new Collision(CollisionStatus.New, CollisionType.HeadOn,
collisionOnAGVs, overLapNodes.ToArray(), DateTime.Now,
(float)deltaTOverLap.TotalSeconds + AGV.Length / AGV.SimSpeed);
// skip collision that exists
if (SimListCollision.Exists(c => c.Type == collision.Type &&
c.OnAGVs.SequenceEqual(collision.OnAGVs)))
{
continue;
}
Collision.SimListCollision.Add(collision);
}
// detect creoss collision
else if (deltaTOverLap == TimeSpan.Zero) // overlap only one node
{
// find next node and distance to next node of agv1
int idx1 = agv1.Path.IndexOf(agv1.ExitNode);
if (idx1 == agv1.Path.Count - 1)
{
continue; // at goal node
}
int nextNodeAGV1 = agv1.Path[idx1 + 1];
float disToNextNodeAGV1 = D[agv1.ExitNode, nextNodeAGV1] / Display.Scale - agv1.DistanceToExitNode;
// find next node and distance to next node of agv2
int idx2 = agv2.Path.IndexOf(agv2.ExitNode);
if (idx2 == agv2.Path.Count - 1)
{
continue; // at goal node
}
int nextNodeAGV2 = agv2.Path[idx2 + 1];
float disToNextNodeAGV2 = D[agv2.ExitNode, nextNodeAGV2] / Display.Scale - agv2.DistanceToExitNode;
// conditions for cross collisions to occur
float delta = disToNextNodeAGV1 - disToNextNodeAGV2; // uint: cm
if (nextNodeAGV1 == nextNodeAGV2 && Math.Abs(delta) < AGV.Length)
{
// waiting for later AGV to come close to the collision node
if ((delta >= 0 && disToNextNodeAGV1 > AGV.Length) ||
(delta < 0 && disToNextNodeAGV2 > AGV.Length))
{
continue;
}
int[] collisionOnAGVs = new int[2];
if (delta >= 0)
{
collisionOnAGVs[0] = agv1.ID; collisionOnAGVs[1] = agv2.ID;
}
else
{
collisionOnAGVs[0] = agv2.ID; collisionOnAGVs[1] = agv1.ID;
}
// -----------add real-time later--------------
Collision collision = new Collision(CollisionStatus.New, CollisionType.Cross,
collisionOnAGVs, overLapNodes.ToArray(), DateTime.Now, 2.5f);
// skip collision that exists
if (SimListCollision.Exists(c => c.OnAGVs.SequenceEqual(collision.OnAGVs) ||
c.OnAGVs.SequenceEqual(new int[2] {
collision.OnAGVs[1], collision.OnAGVs[0]
})))
{
continue;
}
Collision.SimListCollision.Add(collision);
}
}
}
}
}
// 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);
}