Bioloid Robotis User Manual Download

Page: 131 / 142

User’s Guide

void TxDString(byte *bData);
void TxD8Hex(byte bSentData);
void TxD32Dec(long lLong);
byte RxD81(void);
void MiliSec(word wDelayTime);
void PortInitialize(void);
void SerialInitialize(byte bPort, byte bBaudrate, byte bInterrupt);
byte TxPacket(byte bID, byte bInstruction, byte bParameterLength);
byte RxPacket(byte bRxLength);
void PrintBuffer(byte *bpPrintBuffer, byte bLength);

// --- Gloval Variable Number ---
volatile byte gbpRxInterruptBuffer[256];
byte gbpParameter[128];
byte gbRxBufferReadPointer;
byte gbpRxBuffer[128];
byte gbpTxBuffer[128];
volatile byte gbRxBufferWritePointer;

int main(void)
{
byte bCount,bID, bTxPacketLength,bRxPacketLength;

PortInitialize(); //Port In/Out Direction Definition
RS485_RXD; //Set RS485 Direction to Input State.

SerialInitialize(SERIAL_PORT0,DEFAULT_BAUD_RAT
E,RX_INTERRUPT);//RS485
Initializing(RxInterrupt)

SerialInitialize(SERIAL_PORT1,DEFAULT_BAUD_RATE,0);

//RS232

Initializing(None Interrupt)

gbRxBufferReadPointer = gbRxBufferWritePointer = 0; //RS485

RxBuffer Clearing.

sei(); //Enable Interrupt -- Compiler Function
TxDString("\r\n [The Example of Dynamixel Evaluation with

ATmega128,GCC-AVR]");

//Dynamixel Communication Function Execution Step.
// Step 1. Parameter Setting (gbpParameter[]). In case of no

parameter instruction(Ex. INST_PING), this
step is not needed.

// Step 2. TxPacket(ID,INSTRUCTION,LengthOfParameter); --Total

TxPacket Length is returned

// Step 3. RxPacket(ExpectedReturnPacketLength); -- Real RxPacket

Length is returned

// Step 4 PrintBuffer(BufferStartPointer,LengthForPrinting);

bID = 1;
TxDString("\r\n\n Example 1. Scanning Dynamixels(0~9). -- Any Key

to Continue."); RxD8();

for(bCount = 0; bCount < 0x0A; +)
{
bTxPacketLength = TxPacket(bCount,INST_PING,0);
bRxPacketLength = RxPacket(255);
TxDString("\r\n

TxD:");

PrintBuffer(gbpTxBuffer,bTxPacketLength);

TxDString(",

RxD:");

PrintBuffer(gbpRxBuffer,bRxPacketLength);

if(bRxPacketLength == DEFAULT_RETURN_PACKET_SIZE)
{
TxDString(" Found!! ID:");TxD8Hex(bCount);
bID = bCount;
}
}

TxDString("\r\n\n Example 2. Read Firmware Version. -- Any Key to

Continue."); RxD8();

gbpParameter[0] = P_VERSION; //Address of Firmware Version
gbpParameter[1] = 1; //Read Length
bTxPacketLength = TxPacket(bID,INST_READ,2);
bRxPacketLength

RxPacket(DEFAULT_RETURN_PACKgbpParamet

er[1]);

TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength);
TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength);
if(bRxPacketLength == DEFAULT_RETURN_PACKgbpParameter[1])
{
TxDString("\r\n Return Error : ");TxD8Hex(gbpRxBuffer[4]);
TxDString("\r\n Firmware Version : ");TxD8Hex(gbpRxBuffer[5]);
}

TxDString("\r\n\n Example 3. LED ON -- Any Key to Continue.");

RxD8();

gbpParameter[0] = P_LED; //Address of LED
gbpParameter[1] = 1; //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 Example 4. LED OFF -- Any Key to Continue.");

RxD8();

gbpParameter[0] = P_LED; //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 Example 5. Read Control Table. -- Any Key to

Continue."); RxD8();

gbpParameter[0] = 0; //Reading Address
gbpParameter[1] = 49; //Read Length
bTxPacketLength = TxPacket(bID,INST_READ,2);
bRxPacketLength

RxPacket(DEFAULT_RETURN_PACKgbpParamet
er[1]);

TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength);
TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength);
if(bRxPacketLength == DEFAULT_RETURN_PACKgbpParameter[1])
{
TxDString("\r\n");
for(bCount = 0; bCount < 49; +)
{
TxD8('[');TxD8Hex(bCount);TxDString("]:");

TxD8Hex(gbpRxBuffer[5]);TxD8(' ');

}
}

TxDString("\r\n\n Example 6. Go 0x200 with Speed 0x100 -- Any Key

to Continue."); RxD8();

gbpParameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
gbpParameter[1] = 0x00; //Writing Data P_GOAL_POSITION_L
gbpParameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H
gbpParameter[3] = 0x00; //Writing Data P_GOAL_SPEED_L
gbpParameter[4] = 0x01; //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 7. Go 0x00 with Speed 0x40 -- Any Key to

Continue."); RxD8();

gbpParameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
gbpParameter[1] = 0x00; //Writing Data P_GOAL_POSITION_L
gbpParameter[2] = 0x00; //Writing Data P_GOAL_POSITION_H
gbpParameter[3] = 0x40; //Writing Data P_GOAL_SPEED_L
gbpParameter[4] = 0x00; //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 8. Go 0x3ff with Speed 0x3ff -- Any Key

to Continue."); RxD8();

131