Listing 2: Data setups
and shortcuts.
#define DLED 16
#define nTxU2 19
// Dynamixel commands
#define DM_PING 0x01
#define DM_READ_DATA 0x02
#define DM_WRITE_DATA 0x03
#define DM_REG_WRITE 0x04
#define DM_ACTION 0x05
#define DM_RESET 0x06
#define DM_SYNC_WRITE 0x83
// Control Table memory addresses
uint8_t ct[25] = {3,4,5,6,8,11,12,
13,14,16,17,18,19,24,25,26,27,28,29,30,32,34,4
4,47,48};
// Control Table memory bytes, 1 byte is a
// single value, 2 bytes means a 16 bit
// word
uint8_t cb[25] = {1,1,1,2,2,1,1,1,2,1,1
,1,1,1,1,1,1,1,1,2,2,2,1,1,2};
dmRetDelay,
dmCWAngleLimit,
dmCCWAngleLimit,
dmHighTempLimit,
dmLowVoltLimit,
dmHighVoltLimit,
dmMaxTorque,
dmStatRetLevel,
dmAlarmLED,
dmAlarmShutdn,
dmReserved,
dmTorqueEn,
dmLED,
dmCWCompMargin,
dmCCWCompMargin,
dmCWCompSlope,
dmCCWCompSlope,
dmPosition,
dmMovSpeed,
dmTorqueLimit,
dmRegInst,
dmLock,
dmPunch
} DMcontrols_e;
uint8_t cmd[20];
// Command Table IDs
typedef enum DMControls_e
{
dmID =0,
dmBaud,
// The protocol takes
// several bytes per
// command
uint8_t cmdLen;
uint8_t stat[20];
uint8_t statLen;
uint8_t err;
get a reception from the servo.
It is true that to control the servo
we only need to write — and we can
ignore the response packets from the
servo — but if we ignore those
received data packets at the wrong
time, we will collide with our
command transmissions and odd
things will occur.
I am not going to repeat the
descriptions of the commands and
status packets and values here; you
can get those from the first
document link that I gave previously.
The driver that I created is not
very sophisticated since I didn’t have
a lot of time. It is a simple set of
functions, however, that can be used
as the core of a library to control
Dynamixel servos. Feel free to
innovate!
Firstly, you’ll notice that I’m using
uint8_t and uint16_t as variable
defines. These are GNU open source
standard defines and work great to
tell the coder eight-bit and 16-bit
data values. Using char and int does
not impart that much information.
The first part of the code to
discuss is the setup for everything.
Listing 1 shows the MPIDE (just like
Arduino) Setup() function:
The LookBus() function will scan
the Dynamixel bus looking for a servo
at address 0 through 20. It will print
out only the status packets received
from servos that respond.
This is the really simple part.
Listing 2 shows the data definitions
that I made global to simplify the
code. (Yes, globals are evil, but for
limited memory embedded systems,
they are a good thing!)
The data arrays detail the Control
Table addresses and parameters that
a DATA WRITE or DATA READ
command use to change or read the
current servo settings. That huge
enum is a simple way to specify what
element in the arrays to use for the
commands.
As you read through the code,
you’ll see how I use them. It really
does make it easier than
remembering a bunch of magic
numbers that do things.
I have made several functions
that handle specific commands:
CmdPing(), CmdLED(), CmdMove(),
CmdSpeed(), and CmdReadControl().
The functions of these commands are
suggested in their names: ping for a
servo; turn the LED on or off; move
the servo to a specific goal; set the
servo transit speed; and read from
the control table.
These functions will format the
cmd[] byte array for a command
transmission. A command packet
requires a checksum at the end of it
to handle simple detection of
transmission errors.
The function GetCSUM() does
this checksum. When the command
is properly formatted, the code calls
the SendCmd() function. This
function is at the heart of
communicating with the Dynamixel
bus. Listing 3 is this function.
There are lots of details to
discuss here. I use the standard
Arduino (MPIDE) Serial1.write(array,
arrayLen) function call that transmits
un-translated binary data out the
second UART port after I set the
~Tx/Rx bit low.
The MPIDE serial library doesn’t
have a call to tell me when the
transmission is complete, but the
PIC32 UART status register does —
SERVO 06.2013 13