// Convert Byte Arrays to Structs (method 2)
public static AGVInfoReceivePacket GetFromArray(byte[] bytes)
{
AGVInfoReceivePacket packet = new AGVInfoReceivePacket();
packet.Header = new byte[2] {
bytes[0], bytes[1]
};
packet.FunctionCode = bytes[2];
packet.AGVID = bytes[3];
packet.Status = bytes[4]; // use Convert.ToChar() to see the character
packet.ExitNode = BitConverter.ToUInt16(bytes, 5);
packet.DisToExitNode = BitConverter.ToSingle(bytes, 7);
packet.Orient = bytes[11]; // use Convert.ToChar() to see the character
packet.Velocity = BitConverter.ToSingle(bytes, 12);
packet.Battery = bytes[16];
packet.CheckSum = BitConverter.ToUInt16(bytes, 17);
packet.EndOfFrame = new byte[2] {
bytes[19], bytes[20]
};
return(packet);
}
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);
}
}
}
}
}
