private static void timer4_Elapsed(object sender, System.Timers.ElapsedEventArgs e, SetVelocityPacket sendFrame)
{
SetVelocityPacket resendFrame = sendFrame;
if (ackReceived.AGVID == resendFrame.AGVID &&
ackReceived.FunctionCode == (byte)FUNC_CODE.RESP_ACK_VELOCITY &&
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, "Set Velocity", System.Drawing.Color.Red);
Display.UpdateComStatus("send", resendFrame.AGVID, "Set Velocity", System.Drawing.Color.Blue);
}
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();
}
