Servo - 201306

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.