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()); }