/// <summary> Generates the packet for the motor controller: </summary>
/// <remarks>
/// One Start byte (value 2 for short packets and 3 for long packets)
/// One or two bytes specifying the packet length
/// The payload of the packet
/// Two bytes with a CRC checksum on the payload
/// One stop byte (value 3)
/// </remarks>
/// <param name="Speed"> Speed from -1.0 to 1.0 </param>
private byte[] ConstructPacket(List <byte> Payload)
{
List <byte> Packet = new List <byte>();
Packet.Add(2); // Start byte (short packet - payload <= 256 bytes)
if (this.CANForwardID >= 0)
{
Payload.Add((byte)UARTPacketID.FORWARD_CAN);
Payload.Add((byte)CANForwardID);
}
Packet.Add((byte)Payload.Count); // Length of payload
Packet.AddRange(Payload); // Payload
ushort Checksum = UtilData.CRC16(Payload.ToArray());
Packet.AddRange(UtilData.ToBytes(Checksum)); // Checksum
Packet.Add(3); // Stop byte
return(Packet.ToArray());
}