60
DYN2MS-02F-0116A17
//***************** Every Robot Instruction ******************
// Send a package with a function by Global_Func
// Displacement: -2^27 ~ 2^27 - 1
// Note: in the description of RS232 communication protocol above (Section 7), the last byte of packet is
//
always B0, but in the code of below, the first byte is always B0.
void DlgRun::Send_Package(char ID , long Displacement)
{
unsigned char B[8],Package_Length,Function_Code;
long TempLong;
B[1] = B[2] = B[3] = B[4] = B[5] = (unsigned char)0x80;
B[0] = ID&0x7f;
Function_Code = Global_Func & 0x1f;
TempLong = Displacement & 0x0fffffff;
//Max 28bits
B[5] += (unsigned char)TempLong&0x0000007f;
TempLong = TempLong>>7;
B[4] += (unsigned char)TempLong&0x0000007f;
TempLong = TempLong>>7;
B[3] += (unsigned char)TempLong&0x0000007f;
TempLong = TempLong>>7;
B[2] += (unsigned char)TempLong&0x0000007f;
Package_Length = 7;
TempLong = Displacement;
TempLong = TempLong >> 20;
if(( TempLong == 0x00000000) || ( TempLong == 0xffffffff))
{//Three byte data
B[2] = B[3];
B[3] = B[4];
B[4] = B[5];
Package_Length = 6;
}
TempLong = Displacement;
TempLong = TempLong >> 13;
if(( TempLong == 0x00000000) || ( TempLong == 0xffffffff))
{//Two byte data
B[2] = B[3];
B[3] = B[4];
Package_Length = 5;
}
TempLong = Displacement;
TempLong = TempLong >> 6;
if(( TempLong == 0x00000000) || ( TempLong == 0xffffffff))
{//One byte data
B[2] = B[3];
Package_Length = 4;
}
B[1] += (Package_Length-4)*32 + Function_Code;
Make_CRC_Send(Package_Length,B);
}
C++ Code for Serial Communication - Page 3
7.9A Appendix : C++ Code for Serial Communication Protocol
