private static void timer5_Elapsed(object sender, System.Timers.ElapsedEventArgs e, AGVInitPacket sendFrame)
{
AGVInitPacket resendFrame = sendFrame;
if (ackReceived.AGVID == resendFrame.AGVID &&
ackReceived.FunctionCode == (byte)FUNC_CODE.RESP_ACK_INIT &&
ackReceived.ACK == (byte)'Y')
{
ackReceived = default(AckReceivePacket);
((System.Timers.Timer)sender).Dispose();
return;
}
var agv = AGV.ListAGV.Find(a => a.ID == (int)resendFrame.AGVID);
if (agv == null)
{
return;
}
try { Communicator.SerialPort.Write(resendFrame.ToArray(), 0, resendFrame.ToArray().Length); }
catch { return; }
// Display ComStatus
Display.UpdateComStatus("timeout", resendFrame.AGVID, "Initialize", System.Drawing.Color.Red);
Display.UpdateComStatus("send", resendFrame.AGVID, "Initialize", System.Drawing.Color.Blue);
}
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);
}
}
}
}
}
