ArbotiX-M: The Arduino Compatible Board for Robots with Dynamixel Servos

arbotix dynamixel servo with arduino

The ArbotiX is an open hardware (GPL 3 license) Arduino compatible board that can control up to 30 Dynamixel servo motors. It is ideal for building robots of various kinds (Legged robots, Robotic arms, …). In the following, we provide you with a detailed description of the ArbotiX-M board and its characteristics. Our code samples will help you quickly make your first project.

Outline:

Overview of the ArbotiX Board

The ArbotiX is an Arduino compatible board made for controlling small-to-medium size robots. It targets projects with AX/MX/RX/EX Dynamixel servos and BIOLOID Based robots.

After the success of the original ArbotiX board, Vanadium Labs released the ArbotiX-M. The M stands for Mini. This updated version is indeed 25% smaller than the original one. It still incorporates the 16MHz, 8-bit AVR ATMEGA644p microcontroller. But, it has 28 Digital I/O, 8 of which can also function as analog inputs. Besides, all the I/O have servo style 3-pin headers (gnd, vcc, signal). The ArbotiX-M also has XBEE socket to allow connecting an optional wireless radio shield.

The 3 TTL Dynamixel 3-pin style ports make the most prominent feature of the ArbotiX. Your TTL AX/MX DYNAMIXEL servos can be directly plugged in! The ArbotiX hardware and firmware is directly compatible with Dynamixel servos: AX-12A, AX-18A, AX-12W, MX-28T, MX-64T, MX-106T.

Arduino IDE Setup for the ArbotiX

Programming the ArbotiX is as easy as with any Arduino-based project. Indeed, you can make software for your ArbotiX-M based robot using the Arduino IDE. In our experiments, we have been successfully using the latest 1.8 version of the IDE.

Install the ArbotiX board hardware description and libraries

To install the ArbotiX board, first locate the Arduino’s IDE working folder. By default, this is the Arduino sub-folder of Document. You can lookup the exact location in the settings of the Arduino IDE. The last step is to copy the ArbotiX-M board hardware description and libraries into the Arduino’s IDE working folder. That is the content of folders hardware and libraries. Merge them with any folders with the same name if any.

Connect the ArbotiX and Make it Blink

Now, power the ArbotiX-M board and plug it to your computer via an FTDI to USB cable. Select the ArbotiX Std in the Arduino’s IDE Tools/Board menu. Select also in the Tools/Port the USB port assigned by the OS to your board. Time for a test. Upload the ArbotiXBlink sketch to the ArbotiX-M. The tiny green LED labelled USER on the board should start blinking.

Powering your Servos using the ArbotiX

The ArbotiX board does have two power buses for powering plugged in servos. The main one (in red on Figure 1) is for Dynamixel servos that require between 11V and 12V DC. While the secondary bus (in green on Figure 1) is for hobby servos, that often require between 5V and 6V DC. But, before using these power buses, ensure that the power setup jumper (surrounded in purple on Figure 1) is connected to the Vin pin.
ArbotiX-M has 2 power buses
Fig. 1: ArbotiX-M Power Buses for Dynamixel and Hobby Servos

The main bus directly connects Dynamixel servos’ sockets to the blue screw terminal and the DC jack (center-positive). So, you can use either a 12V DC power adapter or a 3-cell 11.1V LiPo battery.

The secondary power bus connects green screw terminal directly to the voltage pin on the 3-pin port for digital pins D12-D15. These pins are all tied to the hardware PWM pins on the ATMEGA644p and will work with analogWrite() and servo.write() functions in the Arduino IDE.

Dynamixel Servo IDs Assignment

Each Dynamixel servo has 2 TTL slots. This allows connecting multiple Dynamixel servos in a daisy chain. So, only the first one is connected to the control board. This bus organization requires that each servo in the chain has a unique ID. IDs range from 0 to 252.

Assigning IDs to servos should be done only once, prior to assembling your robot. The goal is to ensure that all your servos have different, but consecutive IDs. This operation can be done using the ArbotiX board.

Prepare the ArbotiX Board

The first step is to power the board and connect it to a computer via an FTDI to USB cable. Next, setup the ArbotiX to enable ID setup. This is achieved by uploading the SetupServoIDs sketch to the board, using the Arduino IDE.

The DynaManager Software

To actually setup servo IDs you need to download and run the DynaManager software on your computer. You’ll get a window similar to Figure 2-A. At this stage the software is not connected to the ArbotiX. Hit the Auto Search button. The connection takes several seconds to establish. The window will look like Figure 2-B.
dynaManager Dynamixel servo setup steps
Fig. 2: Set Dynamixel Servo ID using DynaManager

Servo ID Setup

Now your are ready to actually set up servo IDs. This operation needs to be done for only one servo at a time. For each servo, repeating the following steps:
  1. Plug a single servo to the ArbotiX. The servo should be disconnected from all the others.
  2. Click on the Scan button of the dynaManager app. After few seconds, you’ll see the servo ID and model as on Figure 2-C.
  3. Type in a unique ID and click the Set ID/Baud button.
  4. Once you get the confirmation as on Figure 2-D, you can unplug the servo.

Read Power Voltage Supplied to Servos

Safety should be a concern in every project relying on LiPO batteries. This is why you should check the voltage supplied to your servos upon startup is above the minimum acceptable value. This is can be easily done using the dxlGetSystemVoltage() function from the ax12.h library as shown in the code snippet below.
int NumberOfServos = 4;
int ServoIDs[] = {1, 2, 3, 4};
float voltage = dxlGetSystemVoltage(NumberOfServos, ServoIDs);
This is an excerpt from the CheckVoltage sketch available on our GitHub repository. Note that the ServoIDs parameter is optional. You can skip it if servos IDs are consecutive starting at 1.

Blink Servo LED

Each Dynamixel servo embeds a LED that can be turned ON and OFF using the dxlSetLED() function. A symetric function (dxlGetLed()) allows to get the status (true for ON and false for OFF) of the LED of any given servo. The code snippet below shows how to toggle the state of a servo’s LED.
int servoID = 1;
bool isLedOn = dxlGetLed(servoID);
if(isLedOn){
	dxlSetLED(servoID, false); //Turn OFF LED of Servo 1
} else {
	dxlSetLED(servoID, true); //Turn ON LED of Servo 1
}
In our Git repository, there are two sketches that show different uses of these functions. The first one is the BlinkServoLED sketch. It makes the LEDs turn ON in turn. The second sketch (the BlinkAllServoLEDs) shows how to turn all servo LEDs at once in a single instruction. It does use the dxlSetLED() function. But, we provide it with the DXL_BROADCAST constant (254) as a parameter, instead the servo’s ID.

Read-Write Servo Position

Our SetAndGetServoPosition sketch is a complete running example showing how to set and get a servo position. We go into the details, and present each function below.

Setting the Position of a Servo

To make a servo move to a given position, you need to use the dxlSetGoalPosition() function. This latter takes two parameters. The first one is the ID of the servo to control, and the latter is the goal position. The position is an integer within the a range defined by the type of the servo. It ranges between 0 and 1023 for Dynamixel AX servos and between 0 and 4096 for MX servos.

Getting the Position of a Servo

Reading a servo position is achieved via the dxlGetPosition() function. It requires a unique parameter, which is the ID of the servo. However, reading might fail, and you get -1 as a result, if the servo is busy. This is why it is recommended to wrap the call into a function that retries upon failures as shown in the above code snippet.
int getServoPosition(int servoID){
  int position;
  for(int i = 0; i < 10; i++){ position = dxlGetPosition(servoID); if(position > -1){
      return position;
    }
  }
  Serial.print("Failed reading position of servo ");
  Serial.println(servoID);
  return -1;
}

Waiting for a Servo to Finish Moving

The dxlGetMoving() function answers true while the servo is moving. It can be easily used in a loop to wait a servo perform a motion as examplified in the code below.
  while(dxlGetMoving(servoID)){
    delay(200);
  }

Controlling Dynamixel Servos’ Speed and Mode

Overview

Dynamixel servos allow controlling their motions in different ways. You can set the speed at which they move. Acceleration can also be set but only for MX servos. Another interesting feature is the mode (joint or wheel) that defines the set of possible motions.

We provide you with two different sketches to illustrate these features. The ServoSpeedInJointMode sketch shows how to set the goal speed and restrict positions in joint mode. Conversely, the ServoSpeedInWheelMode sketch shows how to use the servo for driving a wheel.

Joint Mode

By default, Dynamixel servos are set to function in joint mode. They are capable of moving between a minimum and a maximum position. The servo moves clockwise if the current position is greater than the goal position. It moves counter clockwise if the current position is smaller than the goal position.

Minimum and maximum positions are 0 and 1023 for AX servos, and 0 and 4096 for MX servos. These values can however be overridden to constrain servo’s positions as shown in the code snippet below. This feature is very useful to avoid breaking the hardware in case of a bug in the control program.

 int servoID = 1;
 int minPosition = 300;
 int maxPosition = 750;
 dxlSetJointMode(servoID, minPosition, maxPosition);   

Wheel Mode

Dynamixel servos can be used as motors to drive wheels. This is what the wheel mode is for. The servos can then rotate clockwise or counter clockwise depending on the speed value.

Setting wheel mode can be done using the axSetWheelMode() function for AX servos. For MX servos, you need to use the mxSetWheelMode() function. Both functions take a unique parameter, which is the ID of servo to setup.

Servo Mode and EEPROM Preservation

The chosen servo mode as well as the minimum and the maximum positions for joint mode are saved into the EEPROM of the servo. So, next time you power the servo, it restored the mode you have already defined.

EEPROMs have a limited number of write cycles. Thus, it’s a good idea to set the servo’s mode as few times as possible. To do so, you can rely on the dxlGetMode() function. It takes a servo’s ID as an integer parameter. The answer is an integer which value is 1 if the servo is in ‘joint’ mode. The function return 2 if the servo is in ‘wheel’ mode. Use it before setting the servo’s mode as shown in the code sample below.

 int jointMode = 1;
 int minPosition = 0;
 int maxPosition = 1023;
 int servoID = 1;
 int currentMode = dxlGetMode(servoID);
 if(mode != jointMode){
   dxlSetJointMode(servoID, minPosition, maxPosition);
 }

Setting a Servo’s Speed

Dynamixel servos allow to control their motion speed. This is done using the dxlSetGoalSpeed() function that takes two parameters. The first one is the ID of the targeted servo. The second parameter is the speed value. The speed value is an integer which value depends on the servo mode. Once set, the speed value is used for all future servo motions.

Speed in Joint Mode

In joint mode, the speed value can be between 0 and 1023. 0 means full speed. Other values are proportional to RPMs with 0.111 ratio for AX servos and 0.114 for MX servos. Any values above your servos maximum speed will be treated as the maximum speed. The following code sample shows the conversion for AX servos.
 int servoID = 1;
 int rpm = 30;
 int speedValue = rpm / 0.111;
 dxlSetGoalSpeed(servoID, speedValue);

Speed in Wheel Mode

Speed value in wheel mode can vary from 0 up to 2047. Values between 0 and 1023 correspond to counter clockwise rotation. Conversely, the servo will rotate clockwise if you set the speed to a value between 1024 and 2047. Setting the speed value to 0 or 1024 makes the servo stop. The ratio between RMPs and speed value is the same as with the joint mode, but only for the 0-1023 range. The code snippet below shows an example for AX servos where the RPM ratio is 0.111.
 int servoID = 1;
 int rpm = 30;
 int counterClowiseSpeedValue = rpm / 0.111;
 int clockwiseSpeedValue = 1023 + (rpm/0.111);

Reading a Servo’s Speed

There exist two functions for reading a Dynamixel servo’s speed. The first one is dxlGetGoalSpeed() that answers the goal speed. The second fuction is dxlGetSpeed(). It answers the current speed of the servo. This might be different from the goal speed, especially if the speed has just been changed. The above two functions return values between 0 and 2047. You get a value between 0 and 1023 when the servo rotates counter clockwise. While 1024 and beyond correspond to clockwise rotations.

Torque, Temperature, and Safe Servo Operations

Why Watching The Temperature of Servos

Servo operations consist in performing rotations, which means producing a torque. The torque is the product of the force (e.g. the weight of the load) and the lever arm between the center of the servo and the load. Depending on these two parameters, a servo might require to generate more or less torque. The higher is the torque, the more current is consumed by a servo. As a result, the servo heats up. Beyond a temperature limit the servo might burn. This is why it is important to watch the temperature in demanding applications such as humanoid robots.

Measuring Internal Temperature of Dynamixel Servos

Dynamixel servos have built-in temperature sensors. Thanks to the dxlGetTemperature() function you can read the current temperature of a given servo. To know if the obtained value is safe, you need to compare it to the highest acceptable temperature. This latter is returned by the dxlGetTempLimit(). The example below shows how to use these functions.
 int servoID = 1;
 int temperature = dxlGetTemperature(servoID)
 int tempLimit = dxlGetTempLimit(servoID);
A more detailed example is provided in the ServoTemperature sketch. It defines 3 temperature zones: “OK zone”, “Warning zone”, and “Danger zone”. The “Warning zone” is when the servo’s temperature is above 80% of the maximum temperature. The servo is in the “Danger zone” if its temperature is equal or greater than the maximum limit. If this ever happens, quickly shutdown your system to avoid irreversible damage. Then, wait for about 20 minutes until the servo cools down, before reuse.

Enabling and Disabling Torque

To prevent servos from overheating, one solution is to disable torque when unused. This can be done using the dxlSetTorqueEnable() function as shown in the example below. Beware that when disabling torque, the servo might rotate because of external forces such as gravity.
 int servoID = 1;
 dxlSetTorqueEnable(servoID, false); //Torque disabled
 dxlSetTorqueEnable(servoID, true); //Torque enabled
As we can see the dxlSetTorqueEnable() function allows to both enable and disable torque. But, you can also enable torque by setting a goal position or speed. To conclude this section, we leave you with two final tips illustrated in our ToggleTorque sketch on GitHub. First, you can enable or disable the torque for all servos by using the DXL_BROADCAST constant instead of the servoID parameter. Last, you can check the torque state using the dxlGetTorqueEnable() function.

Synchronizing Rotations of Multiple Servos

Why Synchronizing Servos?

Many robots use several servos that need to operate in a synchronized fashion. For example a legged robot (e.g. humanoid, hexabot) require coordinated joint motions for fast walks and stability. Synchronized servo rotations also allow for more natural movements.

Making Multiple Servo Move Synchronously

The dxlSyncWritePosition() function allows to control up to 26 servos at the same time. The SynchronizeStartMove sketch provides a complete example showing how to use this function. This is summarized in code snippet below.
 int numberOfServos = 4;
 int servoPositions[numberOfServos][2] = {
   {1, 0},
   {2, 200},
   {3, 500},
   {4, 800}};
 dxlSyncWritePosition(servoPositions, numberOfServos);
The dxlSyncWritePosition() function requires two parameters. The first one is a 2 dimensional array. It is an array that provides the ID and the goal position for each targeted servo. The second parameter of the dxlSyncWritePosition() function is the number of addressed servos. All servos start moving at the same time. However, the stop is not synchronous. Servos might reach their goal positions at different moments if they have different speeds or different angles to cover.

Synchronized Servo Start and Stop with the Bioloid Library

The Bioloid library illustrated using the SynchronizeStartAndStopMove sketch allows synchronizing both start and stop. It also allows defining the duration to perform the whole movement. Given these parameters, the library computes the intermediate positions for each individual servos. As a result servos start at the same time, and move at different speeds to finish the action at the same time. The following code sample illustrates this.
const int MaxServoID = 4;
const PROGMEM unsigned int goalPositions[] = {MaxServoID, 200, 300, 400, 500};
delay(100);
controller.loadPose(goalPositions);
controller.readPose();
int motionDurationInMilliseconds = 3000;
controller.interpolateSetup(motionDurationInMilliseconds); 
while(controller.interpolating > 0){
  controller.interpolateStep();
  delay(3);
}
First the Bioloid controller loads the target pose (controller.loadPose(goalPositions)). To compute intermediate positions and servo speeds, the controller reads current servo positions (controller.readPose()). It also requires you to provide the duration of the movement (controller.interpolateSetup(motionDurationInMilliseconds)). Last, a loop performs the intermediate rotation until the movement is done.

The Goal Positions Array Used by the Bioloid Library

The Bioloid library requires the array of goal positions to be defined as a constant (keyword const) annotated with the PROGMEM modifier. Thus, the constant is stored in flash memory with the program, instead of the SRAM where usually data resides. The array gathering goal positions must also satisfy two more conditions. The positions reference servos with consecutive IDs starting at 1. The ID of the last servo must be provided as the element at index 0.

Sequence of Synchronized Movements with the Bioloid Library

The Bioloid library also allows playing sequences of poses. The SynchronizedMoveSequences sketch provides a running example. The following code snippet gives an overview of the API.
const int MaxServoID = 4;
const PROGMEM unsigned int poseA[] = {MaxServoID, 200, 300, 400, 500};
const PROGMEM unsigned int poseB[] = {MaxServoID, 500, 400, 300, 200};

const int sequenceSteps = 4;
const PROGMEM transition_t movementSequence[] = {
  {NULL, sequenceSteps}, 
  {poseA, 3000},
  {poseB, 2000}, 
  {poseA, 4000}, 
  {poseB, 1000}, 
};

BioloidController controller = BioloidController(1000000);
controller.playSeq(movementSequence);
while(controller.playing){ 
   controller.play(); 
   delay(1);
}  
First, you need to define one or more goal position arrays as we did for synchronized start and stop above. In this example we have defined two such arrays poseA[] and poseB[]. These arrays will be used to define the single sequence movementSequence[] of our example. The first element of this array provides the number of steps of the sequence. The other elements link goal positions arrays to durations in milliseconds, to perform the moves.

I don’t have a ArbotiX-M. Can I control Dynamixel servos with an Arduino Uno board?

Although the ArbotiX-M is very handy, it is not mandatory to control Dynamixel servos. You can use any Arduino such as the Arduino Uno or the Arduino 101. But, remeber that Arduino operates with 5V, while the Dynamixel servo require 12V. So, you need to power your servos from a different electric source. You can hack it yourself as showed in the video below. Alternatively, you can use an Arduino shield for Dynamixel servos. No wiring, just plug your servos as you do with an Arbotix board.

Video 1. Controlling a Dynamixel AX-12A Servo with an Arduino

2 Comments

Leave a Reply

Your email address will not be published. Required fields are marked *

This site uses Akismet to reduce spam. Learn how your comment data is processed.