Dynamixel Support (bioloid libraries) on Teensy processors...

KurtE

Senior Member+
Elmue's thread on Rs485 half duplex, stating that he did not find any support for the Dynamixel servos running on a Teensy, made me think maybe I should should document some of the stuff I have been using since early 2014.

On the Trossen forums, I have a thread talking about converting their PhantomX hexapod using a Teensy 3.1. Actually have similar thread here as well.

My goal was to be able to use the same code base on the Teensy as I did on the Arbotix board and while I was at it also allow me to try other boards, like Arduino Mega... So I modified their bioloid libraries. In particular I mainly changed the Constructor/initializer to not just assume their logical Serial1 object, but instead allow me to pass in a pointer to the Serial object. So for a long time I have been using a copy of their BioloidController library, which I called BioloidSerial.

While the Official arbotix support is still at Arduino 1.0.4, I have versions of this project that I have migrated the code and hardware definitions to the latest Arduino 1.6.x, which Trossen is in the process of testing and hopefully adopting soon.

In this, most of the BioloidSerial support has been integrated into the Arbotix project.

There are several projects up on my github: www.github.com/kurte that use these libraries, including grab bag test program (AX12_TEST). The Phantom_Phoenix project, a project that semi emulates the Arobotix Pro / CM730 servo controller boards (Teensy_Arbotix_Pro).

Note my library is setup to either use the Teensy 3.1 serial port's half duplex mode (only the TX pin needs to be connected) or a more recent addition, I allow you to use a buffer chip with direction pin, that can be used to properly convert the 3.3v output of Teensy to 5Vs.

Unless anyone is interested, I won't go into all of the details of the Bioloid library, except to state, that there is more or less two parts. For this discussion I will use my Serial file names.

The ax12serial.h/cpp file is the low level servo communications code. It allows you to write to and read from the servo(s).

The BioloidSerial.h/cpp is the higher level stuff that provides "pose" capabilities, which includes interpolation/timing code to move servos from one position to a new position in a specific amount of time.

There is some information about the usage of these libraries up on the web including:
http://vanadiumlabs.github.io/arbotix/

Let me know if you have any questions or if you find any problems with these projects.

Thanks
Kurt
 
Hi Kurt -

I am starting to play around with these servos. Thanks for the great information on how to use these with Teensies! It looks like in order to use these with Teensy 3.6 a buffer chip might be needed since the inputs are not 5v tolerant and presumably when the ax-12 drives the serial line it could be 5v (or perhaps it does not actually drive the line that high?). Could you recommend how the ax-12 should safely be connected these new boards?

Thanks!
Brent
 
Last edited:
I used the MX-64 with a Mega2560. It's my understanding that the Arduino is the voltage source with a pullup on the RX pin. The MX-64 then pulls the line low when sending back to the Mega. At the Mega, I connected a 1K resistor between the TX and RX pins and the wire from the servo to the RX pin. Everything worked fine and I created my own functions by using ideas from the AX-12 library. If you try this then be prepared for several pages of #define statements.

In regards to the T3.6, I would use a bi-directional level shifter between the T3.6 and the AX-12. Use the T3.6 3v3 pin for the shifter LV pin and some other 5v0 source for the HV pin.

One test to try is connect 12V to the servo and see what voltage exists on the RX/TX pin. It may have it's own pullup to an internal 5v0 source. If so, you do not want to connect the two directly together but will have to use a level shifter.
 
To be honest, I am not sure if it would be safe or not to use AX or MX servos with the T3.6 directly. Like Artic_Eddie mentioned, I have used AX servos connected with a T3.2, without any additional hardware. Maybe in inline resistor to limit current and maybe a PU resistor. My library is setup that if you use the TX pin of the USART, it switches the USART into half duplex mode and properly handles communications. With the Bidirectional setup, I have to tell the system when the pin is RX and TX. So when I issue a command, it sets TX mode, waits until the command completes outputting and then switches to RX.

But with some of mu current boards I have added level shifters, as the spec for the TTL servos talks about 5v TTL levels, and for the most part I have never had an issue with the 3.3v output on the pins, but if you have a longer chain, maybe the voltage drops enough on outer nodes, that might be an issue.

So again on the T3.6 you might be fine with 3.3v, but for example if any of the servos may have PU resistor to 5v or for some reason drives the voltage to 5v or the like... Could be an issue, so I have not tried it yet. I am building in level converters into my new boards for the T3.5/6.

Hope that helps
 
That should work fine. As you mentioned, be sure any 5v0 signal from the servo does not make it into the T3.6. The T3.2 could handle it but the new board sounds really exciting.

I have a T3.6 coming from the KS project and may try using it to capture and store hi-res images from a camera onto the SD card for security logging.
 
Hello KurtE, Thank you for your work on Teensy and Dynamixel servos.

I downloaded your BioloidSerial library from GitHub, installed the library into Arduino as usual, and tried to compile the Kurts_AX12_Test_Use_Serial example sketch.

I'm getting an error when I attempt to compile it. I was hoping you could help me.

The compile is showing an error on this line:

Code:
BioloidControllerEx bioloid = BioloidControllerEx(1000000);  // may use or not... may go direct to AX12

Here is the error:

Code:
Arduino: 1.8.5 (Mac OS X), TD: 1.40, Board: "Teensy 3.6, Raw HID, 180 MHz, Faster, US English"

Kurts_AX12_Test_Use_Serial:80: error: no matching function for call to 'BioloidControllerEx::BioloidControllerEx(int)'
 BioloidControllerEx bioloid = BioloidControllerEx(1000000);  // may use or not... may go direct to AX12
                                                          ^
In file included from /var/folders/m_/g93w5c_56yj_mgm0nkkrr1z00000gn/T/arduino_modified_sketch_742257/Kurts_AX12_Test_Use_Serial.ino:2:0:
/Users/robertbeatty/Documents/Arduino/libraries/BioloidSerial/BioloidSerial.h:49:5: note: candidate: BioloidControllerEx::BioloidControllerEx()
     BioloidControllerEx();               // baud usually 1000000
     ^
/Users/robertbeatty/Documents/Arduino/libraries/BioloidSerial/BioloidSerial.h:49:5: note:   candidate expects 0 arguments, 1 provided
/Users/robertbeatty/Documents/Arduino/libraries/BioloidSerial/BioloidSerial.h:44:7: note: candidate: constexpr BioloidControllerEx::BioloidControllerEx(const BioloidControllerEx&)
 class BioloidControllerEx
       ^
/Users/robertbeatty/Documents/Arduino/libraries/BioloidSerial/BioloidSerial.h:44:7: note:   no known conversion for argument 1 from 'int' to 'const BioloidControllerEx&'
/Users/robertbeatty/Documents/Arduino/libraries/BioloidSerial/BioloidSerial.h:44:7: note: candidate: constexpr BioloidControllerEx::BioloidControllerEx(BioloidControllerEx&&)
/Users/robertbeatty/Documents/Arduino/libraries/BioloidSerial/BioloidSerial.h:44:7: note:   no known conversion for argument 1 from 'int' to 'BioloidControllerEx&&'
Kurts_AX12_Test_Use_Serial: In function 'void AllServosOff()':
Kurts_AX12_Test_Use_Serial:241: warning: comparison between signed and unsigned integer expressions 
   for (int i = 0; i < NUM_SERVOS; i++) {
                     ^
Kurts_AX12_Test_Use_Serial: In function 'void AllServosCenter()':
Kurts_AX12_Test_Use_Serial:248: warning: comparison between signed and unsigned integer expressions 
   for (int i = 0; i < NUM_SERVOS; i++) {
                     ^
Kurts_AX12_Test_Use_Serial: In function 'void HoldOrFreeServos(byte)':
Kurts_AX12_Test_Use_Serial:262: warning: comparison between signed and unsigned integer expressions 
     for (int i = 0; i < NUM_SERVOS; i++) {
                       ^
Kurts_AX12_Test_Use_Serial: In function 'void GetServoPositions()':
Kurts_AX12_Test_Use_Serial:649: warning: comparison between signed and unsigned integer expressions 
   for (int i = 0; i < NUM_SERVOS; i++) {
                     ^
Kurts_AX12_Test_Use_Serial: In function 'void TrackServos(boolean)':
Kurts_AX12_Test_Use_Serial:679: warning: comparison between signed and unsigned integer expressions 
   for (int i = 0; i < NUM_SERVOS; i++) {
                     ^
Kurts_AX12_Test_Use_Serial: In function 'void TrackPrintMinsMaxs()':
Kurts_AX12_Test_Use_Serial:713: warning: comparison between signed and unsigned integer expressions 
   for (int i = 0; i < NUM_SERVOS; i++) {
                     ^
Kurts_AX12_Test_Use_Serial: In function 'void PrintServoValues()':
Kurts_AX12_Test_Use_Serial:744: warning: comparison between signed and unsigned integer expressions 
     if (w == -1)
           ^
no matching function for call to 'BioloidControllerEx::BioloidControllerEx(int)'
 
Hi, this does not look like the current stuff?

That is up on github in the test program it should be using the version of the constructor with no parameters as seen in the line:
https://github.com/KurtE/AX12_Test/blob/master/AX12_Test.ino#L134

It then initializes the usart speed and information like which pin if any is used as a direction pin:
https://github.com/KurtE/AX12_Test/blob/master/AX12_Test.ino#L181

If the direction pin is not specified, then it uses only the TX pin of the Usart and works to switch it to half duplex
 
Hi KurtE,

Thanks for the link to the current test program. I have it in place and it is compiling OK.

My next goal is to modify the code for my very simple test situation:

Teensy 3.6 plugged into USB/computer.
Servo serial communication wire on Pin 0 (TX1).
Servo ground wire connected to 12V battery ground lead and Teensy ground pin pin.
Servo voltage wire connect to 12V battery positive lead.

Servo is set to ID 1 at 1000000 baud.

My goal is to get this one servo to move.

So, at this point, I'm trying to figure out which lines in the AX12_Test to comment out, simplify, etc.

—Robert
 
Sounds good.

You should be able to remove or edit the top define parts to simply have defined:
#define AX_BUS_USART Serial1
#define SERVO_DIRECTION_PIN -1

And the rest should work... You can get rid of the SERVO_DIRECTION_PIN parameter on the begin method as it defaults to -1...
 
I have the program working! The program is reading valid data from the servo and it's controlling it (changing its position, etc.). Very cool! :)

Thank you. :)

My next step is to simplify the program to remove the Quad, Hex, Turret, and other definitions and code unrelated to my project.
I want to get it down to a simplified generic AX12 servo tester.
 
Thank you again for the help getting my Teensy 3.6 to work with my AX-12s. I'm going to be moving into Dynamixel XL430 servos now (and eventually into the higher end X-series servos). As you know, the XL430s are part of the new X-series line. I have ordered two from Trossen Robotics. When they arrive, I'm going to start experimenting with them.

They appear to use a compatible cable and TTL Half duplex Asynchronous Serial Communication (8bit,1stop, No Parity).

But it looks like the X-series defaults to using Dynamixel Protocol 2.0. While I think the AX-12a is Protocol 1.0.

I don't know the internals of these protocols or how the serial packet communication works. (I have done typical Arduino programming to get various types of robots to function, but I haven't delved down into the details of serial packet communication libraries, etc.).

Do you think the your BioloidSerial library will work to drive the XL420s or will it need to be heavily modified?

It appears that another option MAY BE (not sure) to reconfigure the XL420s to use Protocol 1.0 (it appears that they default to 2.0, but can be changed in the EEPROM to 1.0). Not sure if that is true, but it may be an another option.
 
You are welcome,

As for the XL430s, I don't have any yet... http://www.trossenrobotics.com/dynamixel-xl430-w250-t.aspx but hopefully will in the not too distant future.
The e-manual is up at: http://support.robotis.com/en/product/actuator/dynamixel_x/xl_series/xl430-w250.htm

Note: these Servos are NOT ttl level signals, but instead or RS485 based signals, so they have 4 pin connectors instead of 3 pin connectors. So to control these with teensy, you probably need some hardware support. This is mentioned in a few different threads on the forum. Example: https://forum.pjrc.com/threads/4375...5-and-dynamixel-totally-stuck?highlight=RS485

Library support: Will need some work. The good news is that the Teensy core code can easily control an RS485 device. There is code in the Serial drivers that allows it to automatically change the direction pin to tell if you are outputting or imputing. The library code is setup to use this. I use the same support to drive buffer chips to drive the 3.3v signal to 5v.

Most of the packet level stuff is compatible. The V2.0 stuff has some additional packets and the like. There are a few other people up here who have have probably done some 2.0 protocol work and can hopefully shed some addition light.

In addition a lot of the Servo Registers have changed between the two protocols. So all of the code setup to output to some specific register needs to be updated. Also since the resolution of the registers is different, all code that assumes 0-1023 positions for a range of angles need to be updated to the new ranges...

Kurt
 
Thanks for the info.

I could definitely be wrong, but from what I can tell, the T suffix on the end of the XL430-W250-T indicates that it is a 3-wire TTL servo rather than a 4-wire RS485 servo. Some of the servos in the X-series line have the T suffix. Others have the R.
https://store-76o5u.mybigcommerce.com/product_images/uploaded_images/dxl-x-model-numbering.png

I will be receiving my XL420-W250-T servos in a day or two. I'll start experimenting and see what I can find out regarding using the library.
 
Good point, I am not sure. When I was first discussing them in email with some people at Trossen, I thought they were 485 servos, but then the -T sort of sounds like not. Their document is sort of confusing as several of the illustrations show RS485 communications, but they do say Half duplex Multi-drop TTL...

I hopefully should get some soon as well.
 
KurtE,

I received my XL420-250-T Dynamixel servos, set them to Protocol 1.0 (so they would act more like AX12s in terms of protocol), wired one up to my Teensy 3.6, and modified the Control Table in your BioloidSerial library to accommodate the X-Series control table. I then created a simplified program based on your AX12 test example to test it out.

Thus far, I am able to do the following successfully with the XL420 and the modified BioloidSerial library in my test program:

1. Read and display the servo's various register values (current position, velocity limits, torque setting, various other settings, etc.).
2. Turn on and off the LED.
3. Enable and disable torque.

My remaining problem (and it's a big one) is that it ignores all attempts to move the servo to a specified Goal Position. I can't get any movement at all even though all of the above works OK.
I'm not sure, but I think I've narrowed it down to a specific issue that I'm hoping you can help me with.

The AX12 has a Goal_Position_L and Goal_Position_H for the lowest byte and highest byte of this 2-byte value.
In your ax12Serial.cpp file, you have two functions: ax12SetRegister for setting single byte registers and ax12SetRegister2 for setting double-byte registers.

The XL420 (like all X-series servos) has single byte, 2-byte, and 4-byte registers. Here is the e-manual:
http://support.robotis.com/en/product/actuator/dynamixel_x/xl_series/xl430-w250.htm#bookmark29

On the XL420, the all-important Set Goal Position register is 4-bytes. (There is no Goal_Position_L and Goal_Position_H).
I think it is the writing to the 4-byte registers that isn't working in the BioloidSerial library (and therefore in my program). The others are working OK.

So, I'm thinking that the ax12Serial.cpp file needs a ax12SetRegister4 function.
But understanding and manipulating bytes/hex, etc., is way beyond my skills/knowledge.

I'm hoping you could send me an ax12SetRegister4 function. I'll put it in the library and try it out here.

Does this sound like I'm on the right track?

Your time and expertise is much appreciated.


—Robert

Here is your original ax12SetRegister2 function.

Code:
/* Set the value of a double-byte register. */
void ax12SetRegister2(int id, int regstart, int data){
    setTX(id);
    int checksum = ~((id + 5 + AX_WRITE_DATA + regstart + (data&0xFF) + ((data&0xFF00)>>8)) % 256);
    ax12writeB(0xFF);
    ax12writeB(0xFF);
    ax12writeB(id);
    ax12writeB(5);    // length
    ax12writeB(AX_WRITE_DATA);
    ax12writeB(regstart);
    ax12writeB(data&0xff);
    ax12writeB((data&0xff00)>>8);
    // checksum =
    ax12writeB(checksum);
    setRX(id);
    //ax12ReadPacket();
}
 
There are a few different things about this, that I am totally guessing at. But maybe one of these things will work...

The code in this library is Protocol 1 and the servos support Protocol 2 which has a different packet format... Hopefully in the near future will get some of these to try out... So not sure do Protocol 2 servos respond to protocol 1 packets...

Goal position: Wondering why they went to 4 byte values as they normally have a range of 0-4095 which will fit into 16 bits... But I see they have an extended mode...
My assumption is that they still write in LSB... MSB order so would think writing two bytes would work....

But: if you need 4 byte values here... Could try in a couple of different ways... Again if assuming only 0-4095 values...
Hacked up verson 2e4... 2 extended to 4...
Code:
/* Set the value of a double-byte register. */
void ax12SetRegister2e4(int id, int regstart, int data){
    setTX(id);
    int checksum = ~((id + 7 + AX_WRITE_DATA + regstart + (data&0xFF) + ((data&0xFF00)>>8)) % 256);
    ax12writeB(0xFF);
    ax12writeB(0xFF);
    ax12writeB(id);
    ax12writeB(7);    // length
    ax12writeB(AX_WRITE_DATA);
    ax12writeB(regstart);
    ax12writeB(data&0xff);
    ax12writeB((data&0xff00)>>8);
    ax12writeB(0);   // output the two extra bytes as 0's... 
    ax12writeB(0);
    // checksum =
    ax12writeB(checksum);
    setRX(id);
    //ax12ReadPacket();
}

Or maybe a true 4 byte output:
Code:
/* Set the value of a double-byte register. */
void ax12SetRegister4(int id, int regstart, int data){
    setTX(id);
    uint32_t udata = (uint32_t)data;  // maybe not needed, but remove sign extension stuff... 
    int checksum = ~((id + 7 + AX_WRITE_DATA + regstart + (udata&0xFF) 
            + ((udata>>16) & 0xff) + ((udata>>24) & 0xff)) % 256);
    ax12writeB(0xFF);
    ax12writeB(0xFF);
    ax12writeB(id);
    ax12writeB(7);    // length
    ax12writeB(AX_WRITE_DATA);
    ax12writeB(regstart);
    ax12writeB(udata&0xff);
    ax12writeB((udata>>8)&0xff);
    ax12writeB((udata>>16)&0xff);  
    ax12writeB((udata>>24)&0xff);
    ax12writeB(checksum);
    setRX(id);
    //ax12ReadPacket();
}

Hopefully I updated the checksum properly here...

Other possible issues? As you did not show your test code. Did you set the torque enable register to 1, to enable it?

Again not sure what else yet.

Hope that helps
Kurt
 
Thanks. I will try out these functions and see how they work.

To answer your questions:

The X-series servos default to Protocol 2. But they can be configured to use Protocol 1 (It's an EEPROM setting). I've done that. So we are working entirely with Protocol 1.0 here.

Yes, the position values go from 0-4095. I've used the library to read the Current Portion registers in real-time as I move the non-torque-enabled servo around with my hand. It's reading values from 0 to 4095.

Yes, when I try to move the servo, I enable the torque. And I've read the torque register to confirm that it's working. And once I enable the torque, I can no longer move the servo with my hand.

Thank you for the functions. I'll keep you posted.
 
Again I am only guessing, but wonder if in you are in protocol 1, that maybe trying to write to the Register numbers associated with the older servos might work? That is what happens if you try to write the Goal position to to Registers (0x1e and 0x1f)?
 
I haven't tried that. The control table is very different. None of the addresses match up.

I'm currently putting your function into the modified library.
 
I have copied and pasted the two new functions into the ax12Serial.h file. When I compile, it's saying "'ax12SetRegister4' was not declared in this scope".

Do I need to modify one of the other library files so that it knows the new functions are there?
 
I add the two new function names to the keywords.txt file. The IDE is now highlighting the function call in orange, like it does the other functions, so that's good, but I'm getting the same error. Still looking.
 
Back
Top