Programming Servo Motions with DynamixelSDK on OpenCM 9.04 Microcontroller

This guide will show you how to set up the Arduino Integrated Development Environment (IDE), install the DynamixelSDK library for controlling servos, and set up a sketch to make a servo move.

To install the Arduino IDE

1. Go to https://www.arduino.cc/en/software and install the legacy version of the IDE, version 1.8.X.

2. Download the version of Arduino IDE that is appropriate for your operating system.

To install the OpenCM9.04 Board

1. Open the Arduino IDE.

2. Click File>Preferences.

3. Change the “additional boards manager url” to https://raw.githubusercontent.com/ROBOTIS-GIT/OpenCM9.04/master/arduino/opencm_release/package_opencm9.04_index.json.

4. Click OK.

5. Click Tools>Board>Board Manager.

6. Search for “opencm” and install the OpenCM9.04 board package. This may take 10 or 20 minutes, so please be patient. This will also install the DynamixelSDK library.

7. Now you should be able to use the DynamixelSDK packages and run example sketches. To enable these example sketches, Tools>Board>OpenCM>OpenCM9.04 Board. When selected, the bottom right corner of the IDE should now read “OpenCM9.04 Board, OpenCM Bootloader”.

8. With this board selected, you can now click File>Examples>OpenCM9.04>07_DynamixelSDK>protocol1.0 to view example sketches that show how to use the DynamixelSDK to control Dynamixels with Arduino.

See an example of how to use the DynamixelSDK

Let's open one of the example sketches and look at how it is written. This will help us write more sophisticated programs in the future.

Before we get started, let's turn on line numbers in the editor so we can refer to specific parts of the file. Click File>Preferences, then click the checkbox next to "Display line numbers".

Let's open an example sketch. Click File>Examples>OpenCM9.04>07DynamixelSDK>protocol1.0>read_write_ax. Because this example ships with the DynamixelSDK library, it is read-only. If you want to make edits, you can "save as" it somewhere else.

Note that there is a nice description on lines 1-10,

/*
 * Dynamixel : AX-series with Protocol 1.0
 * Controller : OpenCM9.04C + OpenCM 485 EXP
 * Power Source : SMPS 12V 5A
 * 
 * AX-Series are connected to Dynamixel BUS on OpenCM 485 EXP board or DXL TTL connectors on OpenCM9.04
 * http://emanual.robotis.com/docs/en/parts/controller/opencm485exp/#layout
 * 
 * This example will test only one Dynamixel at a time.
*/

You would be wise to start all your programs with a similar description, to help other people (or yourself in the future) use it. You can see that you'll need an AX (or MX) series servo, an OpenCM9.04 board, and a 12V power supply to run this program.

The first line of code is line 12, which tells the compiler to include the DynamixelSDK library:

#include <DynamixelSDK.h>

If you write your own libraries, you can include them just like this after saving them in Documents\Arduino\libraries. For more information on installing libraries, see: http://www.arduino.cc/en/Guide/Libraries.

Next, the sketch defines macros that contain the memory addresses in the Dynamixels (e.g. 24, 30) that correspond to various control and feedback variables:

// AX-series Control table address
#define ADDR_AX_TORQUE_ENABLE           24                 
#define ADDR_AX_GOAL_POSITION           30
#define ADDR_AX_PRESENT_POSITION        36

These numerical values must be set to match your particular servo model. You can look them up in that servo's e-manual. For example, here is a link to the MX-28 e-manual.

Next, the sketch sets the control protocol used. There are protocols 1 and 2. I always use protocol 1, but you could read about protocol 2 and convince me that it is better.

// Protocol version
#define PROTOCOL_VERSION                1.0                 

This example sketch is for controlling just one servo. Thus, they save macros that store the ID of their single servo, its baudrate, and which serial port on the OpenCM9.04 Board it broadcasts commands over:

// Default setting
#define DXL_ID          1           // Dynamixel ID: 1
#define BAUDRATE        1000000
#define DEVICENAME      "3"         //DEVICENAME "1" -> Serial1(OpenCM9.04 DXL TTL Ports)
                                    //DEVICENAME "2" -> Serial2
                                    //DEVICENAME "3" -> Serial3(OpenCM 485 EXP)

Note that they use the devicename "3", which is Serial3 on the OpenCM. However, if we look at the OpenCM9.04 Board e-manual, we see that we want to broadcast over Serial1, which includes our 3-pin TTL pins. Their comment also points this out.

On lines 28-34 they define some other macros that will make their code easier to read and write. This is a great programming tip. Always define numbers at the top of the program; never have "naked" number values throughout your program. When you need to change numbers later, it is easier to change one line at the top of your program rather than hunting for all the "3"s throughout the program. When you are reading the program later, you will be able to understand the meaning of the code because of descriptive macro and variable names, rather than trying to remember what "3" is the value of.

Line 38 defines the setup() method. Each Arduino sketch has a setup() method, which runs once, and a loop() method, which runs immediately after and repeats indefinitely.

Next, the program waits until a serial port is opened. This is accomplished by opening the serial monitor in the Arduino IDE after the program is downloaded to the board.

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  while(!Serial);


  Serial.println("Start..");

In the following lines, the program initializes instances of the PortHandler and PacketHandler structures. They are part of the dynamixel class. These lines of code can basically be copied into every Arduino sketch you write. Note that since these are initialized within the setup() method, they are limited in scope to setup(). There is another example sketch, read write ax in loop.ino, that shows how to initialize these structures with global scope.

  // Initialize PortHandler instance
  // Set the port path
  // Get methods and members of PortHandlerLinux or PortHandlerWindows
  dynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);

  // Initialize PacketHandler instance
  // Set the protocol version
  // Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
  dynamixel::PacketHandler *packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);

To read and write data to the servos, we must open the portHandler and set the baudrate of the portHandler, which again are lines you can copy into any program:

  // Open port
  if (portHandler->openPort())
  {
    Serial.print("Succeeded to open the port!\n");
  }
  else
  {
    Serial.print("Failed to open the port!\n");
    Serial.print("Press any key to terminate...\n");
    return;
  }

  // Set port baudrate
  if (portHandler->setBaudRate(BAUDRATE))
  {
    Serial.print("Succeeded to change the baudrate!\n");
  }
  else
  {
    Serial.print("Failed to change the baudrate!\n");
    Serial.print("Press any key to terminate...\n");
    return;
  }

One can see that the methods on lines 2 and 14 are type boolean, such that they return a value of false if something goes wrong and prevents them from executing. Checks like this are valuable for debugging as you write programs. If your robot malfunctions later, but you get the success messages from lines 4 and 16 in the serial monitor, then you can rule out basic communication errors.

On lines 88-101 we see the first attempt to send actual data to the servo:

  // Enable Dynamixel Torque
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_AX_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    packetHandler->getTxRxResult(dxl_comm_result);
  }
  else if (dxl_error != 0)
  {
    packetHandler->getRxPacketError(dxl_error);
  }
  else
  {
    Serial.print("Dynamixel has been successfully connected \n");
  }

There's a lot to unpack here. Look at line 2 above. You can see that the main method of interest is:

virtual int write1ByteTxRx  (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0) = 0;

This will send a 1 byte command (that's the Tx; Tx = transmit) and listen for the reply (that's the Rx; Rx = receive). To send the 1 byte command, we must provide the portHandler, the ID of the servo where the message is going (DXL_ID), which memory address to write the byte to (ADDR AX TORQUE ENABLE), the value of the byte to write (TORQUE ENABLE), and a pointer to a variable where write1ByteTxRx can store an error message (&dxl error). See what I mean by not having "naked" numerical values? We can now read this message in human language and it makes sense: "Write one byte using the portHandler to servo DXL ID. Put the value into address ADDR AX TORQUE ENABLE. The value is TORQUE ENABLE."

There are other methods that we will find helpful, which all follow the same pattern:

virtual int write1ByteTxRx  (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0) = 0;
virtual int write2ByteTxRx  (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0) = 0;
virtual int write4ByteTxRx  (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0) = 0;

These all work basically the same. The main difference is that the variable type of the transmitted data must match the method name: write1Byte sends uint8t (8 bit unsigned integer); write2Byte sends uint16t (16 bit unsigned integer); write4Byte sends uint32t (unsigned 32 bit integer). Each time you wish to write a new value to a servo's memory, you must use the write method that will send the number of bytes utilized by that memory address. For example, looking at the MX-28 e-manual again, we see that address 24 is 1 byte and signals whether Torque is Enabled, while address 30 is 2 bytes and signals the Commanded Position.

So, back to the actual program:

  // Enable Dynamixel Torque
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_AX_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    packetHandler->getTxRxResult(dxl_comm_result);
  }
  else if (dxl_error != 0)
  {
    packetHandler->getRxPacketError(dxl_error);
  }
  else
  {
    Serial.print("Dynamixel has been successfully connected \n");
  }

This code tells the servo to engage its motor (enable torque). It then checks to see if the command was sent correctly. If there was an error, it retrieves it and will print it. Then it checks to see if the servo itself encountered an error (e.g. it is overheated, it is overloaded). If so, it retrieves it and prints it. Else it prints that the connection to the servo was successful.

Next, the program moves the motor and reads the position as it moves. In older versions of the dynamixelSDK, the portHandler and packetHandler could not be initialized as global variables, meaning that one had to initialize them within the setup() method and then write a loop that wrote and read servo positions within setup(). The current version lets the programmer initialize the portHandler and packetHandler as global variables, and thus use the loop() method. Therefore, the following sections are out of date but still functional.

  while(1)
  {
    Serial.print("Press any key to continue! (or press q to quit!)\n");


    while(Serial.available()==0);

    int ch;

    ch = Serial.read();
    if( ch == 'q' )
      break;

Here they begin an infinite loop (while 1 == 1). At the top of each loop, they print to the serial window, asking the user to input a letter. In line 6, they call Serial.available() repeatedly, which returns false unless the user has entered a character via the serial window. Once something is available in the Serial port, the program proceeds from line 6, reading one character from the Serial stream and saving it to the integer variable ch. (This is kind of wonky, but characters are ultimately just unsigned integers, so this is ok). If the character is 'q', then it breaks the loop. I don't know why they did it this way; in my opinion, this is sloppy. It would be better to initialize a boolean outside the while loop; condition the while loop while(!boolean); and set it to false when ch == 'q'. But whatever, it's a free example program.

On lines 117 - 126 of the original program, they send a position command, which looks essentially identical to the torque enable earlier, except that goal position is a 2 byte value:

    // Write goal position
    dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_AX_GOAL_POSITION, dxl_goal_position[index], &dxl_error);
    if (dxl_comm_result != COMM_SUCCESS)
    {
      packetHandler->getTxRxResult(dxl_comm_result);
    }
    else if (dxl_error != 0)
    {
      packetHandler->getRxPacketError(dxl_error);
    }

On lines 128 - 147 of the original program, they read the position of the servo while the servo is moving toward the commanded value:

    do
    {
      // Read present position
      dxl_comm_result = packetHandler->read2ByteTxRx(portHandler, DXL_ID, ADDR_AX_PRESENT_POSITION, (uint16_t*)&dxl_present_position, &dxl_error);
      if (dxl_comm_result != COMM_SUCCESS)
      {
        packetHandler->getTxRxResult(dxl_comm_result);
      }
      else if (dxl_error != 0)
      {
        packetHandler->getRxPacketError(dxl_error);
      }

      Serial.print("[ID:");      Serial.print(DXL_ID);
      Serial.print(" GoalPos:"); Serial.print(dxl_goal_position[index]);
      Serial.print(" PresPos:");  Serial.print(dxl_present_position);
      Serial.println(" ");


    }while((abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD));

Here you can see the method:

virtual int read2ByteTxRx   (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0) = 0;

Data is read from the servo by initializing a variable with the appropriate size (for ADDR AX PRESENT POSITION, this is the unsigned 16 bit integer dxl present position), sending a pointer to that variable to the servo when calling read2ByteTxRx, then reading the value of the variable after the message is sent.

This example program prints the servo's ID, goal position, and present position while the absolute value between the goal position and present position is greater than DXL MOVING STATUS THRESHOLD. Once this threshold is crossed, the program changes the value of index, so that the servo moves back to the starting point in the next loop:

    // Change goal position
    if (index == 0)
    {
      index = 1;
    }
    else
    {
      index = 0;
    }

This was a very close look at an example program with which you can learn to use the basic dynamixelSDK commands. Once you understand this, feel free to look through the other example programs or write your own.