Arbotix Firmata

Firmata is a generic protocol for communicating with microcontrollers from software on a host computer. It is typically used on Arduino boards. Once you have Firmata running on your microcontroller your master computer can query it for the available features it supports and then configure it dynamically, without any additional programming required on the microcontroller side. This allows you to dynamically configure different ports for digial or analog input/output, I2C, standard servo control, or pulse-width modulation. The AnimatLab robotics framework uses this protocol running on an Arbotix control board to allow neural networks running on an Ubuntu microcomputer to receive inputs and control motors without requiring the user to write any control code. They simply have to specify the pin where they have connected their IO device and some simple conversion functions. 

While Firmata supports control of standard servos, most of my robotics work is now done with networked Dynamixel servos. The Arbotix board is an Arduino compatible board available from Trossen Robotics that also has the ability to read and write to Dynamixel servos. I have taken the standard Firmata sketch and the Arbotix code and combined them to make a new ArbotixFirmata sketch that can be used to read and write from Dynamixel servos in addition to the other IO it supports. Also, when I started looking into using Firmata I was disappointed that there did not appear to be any stand-alone, cross-platform C++ libraries for interfacing with a Firmata microcontroller on the master computer side. There was a good cross-platform Firmata implementation available in the openFrameworks SDK, but I really did not want to add a whole new, massive framework to my application just to get that tool. So I ripped the code necessary to implement the Firmata protocol out of openFrameworks and created a new repo named openFrameworksArduino. It requires no additional dependencies, and I modified it to also support the ArbotixFirmata code. So you now have an easy way to talk to a Firmata microcontroller using C++ on the master side. This repository can be found on github here

  1. To use the ArbotixFirmata sketch you will need to get the support files for Arbotix and follow the directions for getting it set up. Make sure that you can use these files to talk to a Dynamixel servo before trying to go any further. 
  2. Once you have that working then you need to replace the existing AX library code with the modified version I have in the openFrameworksArduino/Arduino/Libraries/Bioloid. Copy the Ax12.h and cpp files into the \Documents\Arduino\libraries\Bioloid folder on a windows system. The new AX12 files are backwards compatible. I only added new functionality, I did not change any of the old functionality. So you should be able to compile and run any of the sketches that came with your Arbotix with these new files without any problems. 
  3. Once you have the libraries in place you will need to load the ArbotixFirmata sketch from openFrameworksArduino/Arduino/Sketches/ArbotixFirmata into the Arduino application. Select the correct programming port and make sure you have the Arbotix board and the AVRISP mkII programmer selected. Then you should be able to upload the sketch to your Arbotix board. Please note that this sketch will only work with an Arbotix. You will not be able to use this for any other Arduino board. When you have the sketch uploaded you can test the standard firmata using the test app off the Firmata website. Please note, that if you want to test with this app you will need to reset the default baud rate in your sketch to 57,600 and upload it again. You should be able to set digital IO, read digital and analog IO, and control servos and such using that test code. 

The Arbotix comes with a slot on it for connecting an XBee transceiver directly to it. However, currently you CANNOT use ArbotixFirmata with the XBee installed in the slot on the Arbotix board. The reason for this is that it is hardwired to go directly to the dedicated serial port. Instead, I have built a new CommanderSS sketch that allows you to connect with an XBee using the software serial port. The CommanderSS folder is located at openFrameworksArduino/Arduino/Libraries/CommanderSS. If you want to use this then copy it into this folder \Documents\Arduino\libraries\CommanderSS folder. CommanderSS is a software serial version of the Commander code. I have tested it using a different pin with ArbotixFirmata running while it gets XBee communications using the software serial port. I tried a couple of different configurations to get Firmata working. In one version I tried using the Xbee in its on-board slot communicating with the hardware serial, and then using software serial to communicate with the master computer. However, this did not work very well. The software serial just could not handle the communication speed I wanted for the Firmata protocol very well. To use Firmata for robotics applications I want the main communications between the microcontroller and the master computer to be as fast as possible. When I started trying to push this up using the software serial port I started getting a lot of corrupted data. In another version, I used the main hardware serial for Firmata communication, and then used a software serial to communicate with an Xbee. This works great, but the problem is that there is no easy way to reroute the Xbee plug on the Arbotix so it is not hard wired to the hardware serial without manually modifying the board. I did not want to propose doing that for now, so for the moment, ArbotixFirmata is not supporting use of the Xbee communicator. However, I have left the code in there to hook it up to the software serial port if others want to mess with it. One additional note on Xbee. The way it is configured in the sketch XBee is an input only, and the output is going to a USB serial port for debugging info. I use a simple serial to USB connector from Adafruit so I can see debug info from the Arbotix board as it is running. If you need to trouble shoot the sketch I highly recommend using this since you cannot use the regular serial ports because they are doing the IO communications. Both of these are turned off with flags in the sketch, but you can flip them back on if you would like to see debug messages as you send messages to ArbotixFirmata.

To test the Dynamixel code you need to bring up the openFrameworksArduino/vs/openFrameworksArduino.sln Visual Studio solution. It has two projects in it. The first is the openFrameworksArduino library, and the other is a simple test console application. The projects has a dependency on the boost libraries. You must have boost 1.54 or later installed on your system and have BOOST_ROOT configured in your environment variables and pointing to the root location of a boost 1.54 or later install for the code to compile. Also, ArduinoTest has some timing code in it that is commented out by default. If you want to use this code you will need to uncomment it out and also have an OSG_ROOT environment variable defined pointing to your osg path. If you want to use the 64 bit version of the code then you will need your BOOST_ROOT path to have a lib_x64 directory with 64 bit binary libs, and have a OSG_ROOT_X64 environment variable pointing to the 64 bit version of OSG for timing.

The ArduinoTest application assumes you have three Dynamixel AX-12 servos attached to the Arbotix with IDs 1, 2, and 3. When the system starts up it first connects on a com port in the Test.cpp class. You will need to change this to match your system. The default baud rate for ArbotixFirmata is 115,200. This is different from the default rate for Standard Firmata where they use 57,600. Also, I have had some trouble on Ubuntu systems with higher baud rates. If you run into communications issues then I suggest first lowering the baud rate to see if that fixes your problem. I plan to investigate why it has this issue on Linux systems. So keep this in mind when switching between the two types. After connecting it sends a reset to the Arbotix board and waits the board to send back a firmware version to verify it is ready and talking. When it gets the firmware version back then ArduinoTest.setupArduino is called. This is where you configure Firmata for your specific application. You can connect callbacks to receive notifications when values are changed, and you can attach to servos and configure your pins. ArbotixFirmata talks to the Dynamixel using SysEx (system extension) messages. It is capable of sending move commands to each servo individually, and also of grouping a bunch of commands together and sending them at the same time using the synchronous move command. Synchronous moves are much more efficient because you can set the position/velocity of numerous motors in one fell swoop instead of having to send them one at a time. I highly recommend you use that feature for any realistic motor control system. There are a number of new messages defined for talking with the Arbotix board. These are listed below:

New Arbotix Firmata Messages
sendDynamixelServoDetach: This detaches a servo from the update list so it no longer reads servo data and sends it back to the master computer.
sendDynamixelSetRegister: This allows you to set any of the registers in a Dynamixel servo directly.
sendDynamixelGetRegister: This sends a request to Firmata for it to query the servo for the specified register value and then send it back. This is not a synchronous request. You send the get register message and then at some later time you would get a get register message sent back to the master computer with the data you requested.
waitForSysExMessage: When you make a sendDynamixelGetRegister you will not get the data back synchronously. If you do want it to wait till you get that info then you can use this method to tell the system to wait until you get a specific SysEx message type. When you do it continues processing. It also has a timeout for waiting.
sendDynamixelMove: This is a direct command to a single Dynamixel servo telling it to move to a given position at a specified velocity.
sendDynamixelStop: This is a direct command to a single Dynamixel servo telling it to stop moving. It will first send a command to that servo telling it to slow down to the slowest possible speed, then it will query the servo for its current position and then set the goal position to be that value to stop its movement.
 sendDynamixelStopped: Transmits a command to constantly check if the servo is moving, and when it is no longer moving send a signal back and quits checking for movement.
sendDynamixelSynchMoveStart: This can be used to start a synchronous move start to move multiple servos in the same packet call. However, you typically do not need to use this method because each time you make a sendDynamixelSynchMoveExecute it clears out the data and is ready to start a new synchronous move command.
sendDynamixelSynchMoveAdd: This method adds a new servo to the list of motors that will be included in the next synchronous move command. You can add as many motors to the command as you want here. The move command will not be sent until you execute it.
sendDynamixelSynchMoveExecute: After you have added the servos that you want to take place in a synchronous move you call this method to execute the move and actually send the command packet out from the Arbotix to the Dynamixel servos.
 sendDynamixelConfigureServo:  This sends all of the key config info for a specific servo in a single message. This includes CW and CCW limts, maximum torque, and the delay time. This allows you to more quickly configure your servo without having to send multiple set register commands.


There are also several new events defined for the Arbotix controller

New Arbotix Firmata Events
EDynamixelAllReceived: This event is fired each time it receives all data for a given servo (pos, speed, load).
EDynamixelKeyReceived: This event is fired each time it receives key data for a given servo (pos, speed).
EDynamixelTransmitError: This event is fired when the Arbotix receives corrupted data. If the checksums do not match for any command it will ignore that data packet and send a transmit error packet back informing your system of the error so you can attempt to resend the data.
EDynamixelGetRegister: This event is fired when we get register data sent back from the Arbotix.
sendDynamixelMove: This is a direct command to a single Dynamixel servo telling it to move to a given position at a specified velocity.
ECommanderDataReceived: This event is fired when we get XBee commander data sent back from the Arbotix.
EDynamixelStopped: This event is fired after a sendDynamixelStopped message has been sent and the Arbotix has determined that this servo is no longer moving.


Within the ArduinoTest.setupArduino method it first uses the sendDynamixelSetRegister method to set the CW and CCW limits. It then reads those values back from the servo register and waits to get the message back from Firmata. It then attaches to each of the three servos so it will get periodic updates on servo data. It next does an immediate move of each servo to a test position, waits, and then does a synchronous move to a starting position. After that it moves to a target position and starts an update loop where it moves back and forth once it detects that the motor has reached it target location in the dynamixelRecieved method. This demonstrates the basics of how to use the C++ code to read data from Dynamxiel motors and how to control them. The other Firmata features are all standard and you can see the openFrameworks documentation to learn more.

One other note, please make sure your power supply is sufficient for moving multiple servos. If you try and move three servos at the same time at their max speed and only have a 1 Amp power supply then this will cause your motors to error out and stop moving. For my tests I use a 12 V, 5 Amp power supply.