private void LOG_cmdRec_check(Byte[] cmd, Byte rec_checkSum)
{
// check for [checksum error] and cmd [error byte] sub-bites disambiguation
Byte calc_checkSum = C_CheckSum.GET_checkSum(cmd);
if (C_CheckSum.CHECK_checkSum(calc_checkSum, rec_checkSum))
//if( calc_check == 0 )
{
//MessageBox.Show(string.Format("cmd[{0}] = {1}", i_cmdError, cmd[i_cmdError]));
if (cmd[i_cmdError] == 0)
{
// no error
LOG_cmd(cmd, e_cmd.received);
}
else
{
LOG_cmd(cmd, e_cmd.receivedWithError);
LOG_cmdError(cmd[i_cmdId], cmd[i_cmdError]);
}
}
else
{
LOG_cmd(cmd, e_cmd.receivedCheckNot);
LOG_msgAppendLine(String.Format("CheckSumGot != CheckSumCounted :: {0} != {1}", (Byte)rec_checkSum, (Byte)calc_checkSum));
//LOG_msgAppendLine(String.Format("CheckSumGot = {0} ", (Byte)calc_check));
}
}
private Byte[] CREATE_cmdFromInner(Byte[] inner, Byte id)
{
// Instruction Packet = from pc to servo
// OXFF 0XFF ID LENGTH INSTRUCTION PARAMETER1 …PARAMETER N CHECK SUM
// inner contains only these bytes:
// INSTRUCTION, PARAMETER_1, ..., PARAMETER_N
// this function adds first two startBytes [0xFF,0xFF], its id Byte, length Byte and Checksum Byte
// make it into ArrayList
Byte[] cmd = new Byte[5 + inner.Length];
//ArrayList a_cmd = new ArrayList();
//a_cmd.Add({0xFF, 0xFF})
//{ 0xFF, 0xFF, id, len, inner, 0x00 };
//{ 0 , 1 , 2 , 3 , 4...., last };
cmd[2] = id;
int q = 4;
foreach (Byte by in inner)
{
cmd[q] = by;
q++;
}
cmd[3] = (Byte)(inner.Length + 1); // = paramN+Instruction + 1 = paramN + 2 = len
cmd[q] = C_CheckSum.GET_checkSum(cmd);
cmd[0] = cmd[1] = 0xFF;
return(cmd);
}