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