void Dynamixel::writeRaw(uint8 value) {
dxlTxEnable();
//TxByteToDXL(value);
mDxlUsart->regs->DR = (value & (u16)0x01FF);
while( (mDxlUsart->regs->SR & ((u16)0x0040)) == RESET );
dxlTxDisable();
//DXL_RXD
}
byte Dynamixel::txPacket(byte bID, byte bInstruction, byte bParameterLength){
byte bCount,bCheckSum,bPacketLength;
mTxBuffer[0] = 0xff;
mTxBuffer[1] = 0xff;
mTxBuffer[2] = bID;
mTxBuffer[3] = bParameterLength+2; //Length(Paramter,Instruction,Checksum)
mTxBuffer[4] = bInstruction;
for(bCount = 0; bCount < bParameterLength; bCount++)
{
mTxBuffer[bCount+5] = mParamBuffer[bCount];
}
bCheckSum = 0;
bPacketLength = bParameterLength+4+2;
for(bCount = 2; bCount < bPacketLength-1; bCount++) //except 0xff,checksum
{
bCheckSum += mTxBuffer[bCount];
}
mTxBuffer[bCount] = ~bCheckSum; //Writing Checksum with Bit Inversion
dxlTxEnable(); // this define is declared in dxl.h
for(bCount = 0; bCount < bPacketLength; bCount++)
{
writeRaw(mTxBuffer[bCount]);
}
dxlTxDisable();// this define is declared in dxl.h
return(bPacketLength);
}
