Robotis DX-116 - Manual (Page 33)

32
English Translation by: Tribotix Pty Ltd
www.tribotix.com
DYNAMIXEL DX-116
SetInterrupt(INUM_SERIAL0,Serial0Interrupt,INT_ENABLE|INT_RX, 7/*Priority*/);
//Memory Initialize
gbRxBufferReadPointer = gbRxBufferWritePointer = 0;
STI; //Interrupt Enable
/*
*
* Example For Driving Dynamixel DX-116
*
*/
TxDString("\r\n\n Dynamixel Driving Sample Program");
//Set ID to 3
bpParameter[0] = ADDRESS_ID;
bpParameter[1] = 3;
bPacketLength = TxPacket(bpTxBuffer, BROADCASTING_ID, INST_WRITE, bpParameter, 2/*Length of Parameter*/);
bID = 3;
TxDString("\r\n ->[Dynamixel]: "); PrintBuffer(bpTxBuffer,bPacketLength);
bPacketLength = RxPacket(bpRxBuffer);
TxDString("\r\n <-[Dynamixel]: "); PrintBuffer(bpRxBuffer,bPacketLength);
//Set Motor Torque Enable
bpParameter[0] = ADDRESS_TORQUE_ENABLE;
bpParameter[1] = 1;
bPacketLength = TxPacket(bpTxBuffer, bID, INST_WRITE, bpParameter, 2/*Length of Parameter*/);
TxDString("\r\n ->[Dynamixel]: "); PrintBuffer(bpTxBuffer,bPacketLength);
bPacketLength = RxPacket(bpRxBuffer);
TxDString("\r\n <-[Dynamixel]: "); PrintBuffer(bpRxBuffer,bPacketLength);
//Move to Position 0x0100 <-> 0x300
while(1)
{
bpParameter[0] = ADDRESS_GOAL_POSITION;
bpParameter[1] = 0x00; bpParameter[2] = 0x01;
bPacketLength = TxPacket(bpTxBuffer, bID, INST_WRITE, bpParameter, 3/*Length of Parameter*/);
TxDString("\r\n ->[Dynamixel]: "); PrintBuffer(bpTxBuffer,bPacketLength);
bPacketLength = RxPacket(bpRxBuffer);
TxDString("\r\n <-[Dynamixel]: "); PrintBuffer(bpRxBuffer,bPacketLength);
MiliSec(1000);
bpParameter[0] = ADDRESS_GOAL_POSITION;
bpParameter[1] = 0x00; bpParameter[2] = 0x03;
bPacketLength = TxPacket(bpTxBuffer, bID, INST_WRITE, bpParameter, 3/*Length of Parameter*/);
TxDString("\r\n ->[Dynamixel]: "); PrintBuffer(bpTxBuffer,bPacketLength);
bPacketLength = RxPacket(bpRxBuffer);
TxDString("\r\n <-[Dynamixel]: "); PrintBuffer(bpRxBuffer,bPacketLength);
MiliSec(1000);
}
//while(1);
}
void static interrupt far Serial0Interrupt(void) //Serial RxD Interrupt routine
{
STI; //Enable Interrupt
gbpInterruptRxBuffer[gbRxBufferWritePointer++] = RXD_DATA0; //Reading Arrival Data
outpw(EOI, 0x14); //End of Interrupt
}
byte RxPacket(byte *bpRxBuffer)
{