byte[] getMasterCommand(Servo.ServoPortEnum servo, functionInstructionEnum functionInstruction, byte memoryRegister, params byte[] payload)
{
List <byte> data = new List <byte>();
byte id = (byte)(servo - Servo.ServoPortEnum.V0);
byte msgLen = (byte)(3 + payload.Length); // instruction, register, checksum + payload
data.Add(0xff);
data.Add(0xff);
data.Add(id);
data.Add(msgLen);
data.Add((byte)functionInstruction);
data.Add(memoryRegister);
data.AddRange(payload);
byte checkSum = (byte)(id + msgLen + (byte)functionInstruction + memoryRegister);
foreach (byte p in payload)
{
checkSum += p;
}
data.Add((byte)~checkSum);
return(data.ToArray());
}
byte[] getMasterCommand(Servo.ServoPortEnum servo, functionInstructionEnum functionInstruction)
{
List <byte> data = new List <byte>();
byte id = (byte)(servo - Servo.ServoPortEnum.V0);
byte msgLen = 2; // instruction + checksum
data.Add(0xff);
data.Add(0xff);
data.Add(id);
data.Add(msgLen);
data.Add((byte)functionInstruction);
byte checkSum = (byte)(id + msgLen + (byte)functionInstruction);
data.Add((byte)~checkSum);
return(data.ToArray());
}
