Sunday, 23 March 2014

Driving Robotis Dynamixel Servos with Arduino

In my continuing quest for knowledge about robotics I recently bought some Robotis Dynamixel AX-12A servos, with the intention of hooking up to an Arduino. These awesome little servos pack a real punch, with over 15 kg/cm torque. There are plenty of hobby servos that have similar torque, but what sets these apart is that they also have the ability to track and report their speed, temperature, shaft position, voltage, and load. This level of feedback is essential for building advanced robotics applications.

Robotis Dynamixal AX-12A
Unlike hobby servos, these servos operate using a serial half-duplex TTL RS232 protocol. This is actually good news, as your micro-controller doesn’t need to worry about generating individual PWM signals for each servo. Instead, all sensor management and position control is handled by the servo's built-in micro-controller. Position and speed can be controlled with a 1024 step resolution. Wiring is pretty simple with two connectors on each servo allowing a daisy chain to be constructed.

The main controller communicates with the Dynamixel servos by sending and receiving data packets. There are two types of packets; the Instruction Packet (sent from the main controller to the servos) and the Status Packet (sent from the servos to the main controller). By default the communication speed is 1Mbps. A factory fresh servo has a preset ID of 01, which can easily be updated if you intend to run more than one servo from the same micro-controller.

The next challenge was to hook these up to an Arduino. This is not quite as simple as a regular hobby servo, as you must convert the full-duplex signal coming from the Arduino RX/TX pins to the half-duplex required by the Dynamixel servos. Luckily, there is a simple piece of hardware that will do the job for you. I purchased the CDS55xx Driver Board from Robosavvy. This board integrates a half-duplex and a voltage regulator circuit and thus makes it possible to directly connect Dynamixel AX servos to an Arduino.


Next I needed some code to drive these units. If you’ve read my previous post, you’ll know I don’t like bloated software libraries, so instead I created a bare bones class that allows me to set the servo ID and read & write servo positions – enough for current needs. The code is below – hope you find it useful…

Dynamixel.h
#include "Arduino.h"
 
// Registers
#define P_ID 3                   // ID {RD/WR}
#define P_GOAL_POSITION_L 30     // Goal Position L {RD/WR}
#define P_PRESENT_POSITION_L 36  // Present Position L {RD}
 
// Instructions
#define INST_READ 0x02           // Reading values in the Control Table.
#define INST_WRITE 0x03          // Writing values to the Control Table.
 
class Dynamixel{
public:
  Dynamixel();
  void setPos(byte id, int pos, int vel);
  void setID(byte id, byte newId);
  int getPos(byte id);
 
private:
  void WriteHeader(byte id, byte length, byte type);
};

Dynamixel.cpp
#include "Dynamixel.h"
#include "Wire.h"  
 
Dynamixel::Dynamixel() {}
 
/* id = servo ID [0 - 255]
** pos = new position [0 - 1023]
** vel = servo velocity [0 - 1023] */
void Dynamixel::setPos(byte id, int pos, int vel)
{
  int writeLength = 7;
  byte pos_h = pos / 255;
  byte pos_l = pos % 255; 
  byte vel_h = vel / 255;
  byte vel_l = vel % 255;
 
  // Write standard header.
  WriteHeader(id, writeLength, INST_WRITE);
  // Starting address of where the data is to be written.
  Serial.write(P_GOAL_POSITION_L);
  // Write position low byte.
  Serial.write(pos_l);
  // Write position high byte.
  Serial.write(pos_h);
  // Write velocity low byte.
  Serial.write(vel_l);
  // Write velocity high byte.
  Serial.write(vel_h);
  // Check Sum.  
  Serial.write((~(id + writeLength + INST_WRITE + P_GOAL_POSITION_L + pos_l + pos_h + vel_l + vel_h))&0xFF);
  // Wait for instruction to be processed.
  delay(2);
  // Discard return data.
  while (Serial.read() >= 0){}
}
 
/* id = servo ID [0 - 255] */
int Dynamixel::getPos(byte id)
{
  int writeLength = 4;
  int readLength = 2;
 
  // Write standard header.
  WriteHeader(id, writeLength, INST_READ);
  // Starting address of where the data is to be read.
  Serial.write(P_PRESENT_POSITION_L);
  // The length of data to read.
  Serial.write(readLength);
  // Check Sum.  
  Serial.write((~(id + writeLength + INST_READ + P_PRESENT_POSITION_L + readLength))&0xFF);
  // Wait for instruction to be processed.
  delay(2);
  // Discard extra data.
  for (int i = 0; i < 5; i++) Serial.read();
  // Read low byte.
  int low_Byte = Serial.read();
  // Read low byte.
  int high_byte = Serial.read();
  // Discard returned checksum.
  Serial.read();
  // Return position.
  return (int)high_byte << 8 | low_Byte;
}
 
/* id = servo ID [0 - 255]
** newId = new servo ID [0 - 255] */
void Dynamixel::setID(byte id, byte newId)
{
  int writeLength = 4;
 
  // Write standard header.
  WriteHeader(id, writeLength, INST_WRITE);
  // Starting address of where the data is to be written.
  Serial.write(P_ID);
  // New ID.
  Serial.write(newId);
  // Check Sum.
  Serial.write((~(id + writeLength + INST_WRITE + P_ID + newId))&0xFF);
}
 
void Dynamixel::WriteHeader(byte id, byte length, byte type)
{
  Serial.write(0xFF);
  Serial.write(0xFF);
  Serial.write(id);
  Serial.write(length);
  Serial.write(type);
}