// 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); } } } } }