Bioloid Robotis - Page 132

User’s Guide
gbpParameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
gbpParameter[1] = 0xff; //Writing Data P_GOAL_POSITION_L
gbpParameter[2] = 0x03; //Writing Data P_GOAL_POSITION_H
gbpParameter[3] = 0xff; //Writing Data P_GOAL_SPEED_L
gbpParameter[4] = 0x03; //Writing Data P_GOAL_SPEED_H
bTxPacketLength = TxPacket(bID,INST_WRITE,5);
bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE);
TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength);
TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength);
TxDString("\r\n\n Example 9. Torque Off -- Any Key to Continue.");
RxD8();
gbpParameter[0] = P_TORQUE_ENABLE; //Address of LED
gbpParameter[1] = 0; //Writing Data
bTxPacketLength = TxPacket(bID,INST_WRITE,2);
bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE);
TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength);
TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength);
TxDString("\r\n\n End. Push reset button for repeat");
while(1);
}
void PortInitialize(void)
{
DDRA = DDRB = DDRC = DDRD = DDRE = DDRF = 0; //Set all port to
input direction first.
PORTB = PORTC = PORTD = PORTE = PORTF = PORTG = 0x00; //PortData
initialize to 0
cbi(SFIOR,2); //All Port Pull Up ready
DDRE |= (BIT_RS485_DIRECTION0|BIT_RS485_DIRECTION1); //set output
the bit RS485direction
DDRD |=
(BIT_ZIGBEE_RESET|BIT_ENABLE_RXD_LINK_PC|BIT_E
NABLE_RXD_LINK_ZIGBEE);
PORTD &= ~_BV(BIT_LINK_PLUGIN); // no pull up
PORTD |= _BV(BIT_ZIGBEE_RESET);
PORTD |= _BV(BIT_ENABLE_RXD_LINK_PC);
PORTD |= _BV(BIT_ENABLE_RXD_LINK_ZIGBEE);
}
/*
TxPacket() send data to RS485.
TxPacket() needs 3 parameter; ID of Dynamixel, Instruction byte,
Length of parameters.
TxPacket() return length of Return packet from Dynamixel.
*/
byte TxPacket(byte bID, byte bInstruction, byte bParameterLength)
{
byte bCount,bCheckSum,bPacketLength;
gbpTxBuffer[0] = 0xff;
gbpTxBuffer[1] = 0xff;
gbpTxBuffer[2] = bID;
gbpTxBuffer[3] = bParameterLength+2;
//Length(Paramter,Instruction,Checksum)
gbpTxBuffer[4] = bInstruction;
for(bCount = 0; bCount < bParameterLength; bCount++)
{
gbpTxBuffer[bCount+5] = gbpParameter[bCount];
}
bCheckSum = 0;
bPacketLength = bParameterLength+4+2;
for(bCount = 2; bCount < bPacketLength-1; bCount++) //except
0xff,checksum
{
bCheckSum += gbpTxBuffer[bCount];
}
gbpTxBuffer[bCount] = ~bCheckSum; //Writing Checksum with Bit
Inversion
RS485_TXD;
for(bCount = 0; bCount < bPacketLength; bCount++)
{
sbi(UCSR0A,6);//SET_TXD0_FINISH;
TxD80(gbpTxBuffer[bCount]);
}
while(!CHECK_TXD0_FINISH); //Wait until TXD Shift register empty
RS485_RXD;
return(bPacketLength);
}
/*
RxPacket() read data from buffer.
RxPacket() need a Parameter; Total length of Return Packet.
RxPacket() return Length of Return Packet.
*/
byte RxPacket(byte bRxPacketLength)
{
#define RX_TIMEOUT_COUNT2 3000L
#define RX_TIMEOUT_COUNT1 (RX_TIMEOUT_COUNT2*10L)
unsigned long ulCounter;
byte bCount, bLength, bChecksum;
byte bTimeout;
bTimeout = 0;
for(bCount = 0; bCount < bRxPacketLength; bCount++)
{
ulCounter = 0;
while(gbRxBufferReadPointer == gbRxBufferWritePointer)
{
if(ulCounter++ > RX_TIMEOUT_COUNT1)
{
bTimeout = 1;
break;
}
}
if(bTimeout) break;
gbpRxBuffer[bCount] =
gbpRxInterruptBuffer[gbRxBufferReadPointer++];
}
bLength = bCount;
bChecksum = 0;
if(gbpTxBuffer[2] != BROADCASTING_ID)
{
if(bTimeout && bRxPacketLength != 255)
{
TxDString("\r\n [Error:RxD Timeout]");
CLEAR_BUFFER;
}
if(bLength > 3) //checking is available.
{
if(gbpRxBuffer[0] != 0xff || gbpRxBuffer[1] != 0xff )
{
TxDString("\r\n [Error:Wrong Header]");
CLEAR_BUFFER;
return 0;
}
if(gbpRxBuffer[2] != gbpTxBuffer[2] )
{
TxDString("\r\n [Error:TxID != RxID]");
CLEAR_BUFFER;
return 0;
}
if(gbpRxBuffer[3] != bLength-4)
{
TxDString("\r\n [Error:Wrong Length]");
CLEAR_BUFFER;
return 0;
}
for(bCount = 2; bCount < bLength; bCount++) bChecksum +=
gbpRxBuffer[bCount];
if(bChecksum != 0xff)
132