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