/// <summary>
/// Encodes and sends a packet with the Herkulex protocol
/// </summary>
/// <param name="port">Serial port to use</param>
/// <param name="pID">Servo ID</param>
/// <param name="CMD">Command ID</param>
private void EncodeAndSendPacket(SerialPort port, byte pID, byte CMD)
{
byte[] packet = new byte[7];
packet[0] = 0xFF;
packet[1] = 0xFF;
packet[2] = 7;
packet[3] = pID;
packet[4] = CMD;
packet[5] = CommonMethods.CheckSum1(packet[2], packet[3], packet[4]);
packet[6] = CommonMethods.CheckSum2(packet[5]);
FrameQueue.Enqueue(packet);
MessageEnqueuedEvent.Set();
//port.Write(packet, 0, packet.Length);
}
/// <summary>
/// Encodes and sends a packet with the Herkulex protocol
/// </summary>
/// <param name="port">Serial port to use</param>
/// <param name="pID">Servo ID</param>
/// <param name="CMD">Command ID</param>
/// <param name="dataToSend">Data</param>
private void EncodeAndSendPacket(SerialPort port, byte pID, byte CMD, byte[] dataToSend)
{
byte packetSize = (byte)(7 + dataToSend.Length);
byte[] packet = new byte[packetSize];
packet[0] = 0xFF;
packet[1] = 0xFF;
packet[2] = packetSize;
packet[3] = pID;
packet[4] = CMD;
packet[5] = CommonMethods.CheckSum1(packet[2], packet[3], packet[4], dataToSend);
packet[6] = CommonMethods.CheckSum2(packet[5]);
for (int i = 0; i < dataToSend.Length; i++)
{
packet[7 + i] = dataToSend[i];
}
FrameQueue.Enqueue(packet);
//Console.WriteLine("inQueue : " + FrameQueue.Count);
MessageEnqueuedEvent.Set();
//port.Write(packet, 0, packet.Length);
}