Teensy 4.0 and Dynamixel

Status
Not open for further replies.

max246

New member
I am trying to get a Teensy and Dynamixel AX motor working together without a chip to switch RX and TX but I cant seems to find a proper library that just works.

Some libraries that state are for Teensy are not even compiling, is there something ready or I need to get around the source of DynamixelSDK and get some functions out?

I see these examples https://github.com/ROBOTIS-GIT/Open...es/07. DynamixelSDK/protocol1.0/ping/ping.ino but still trying to insall the proper way.
 
I am probably going to convert my stuff to new library by robotis. Dynamixel2Arduino, but default stuff depends on transmit enable pin... I have my own library https://github.com/KurtE/BioloidSerial which I earlier did some t4 testing on. Don’t remember if I added the 54 half duplex support or not
 
I am probably going to convert my stuff to new library by robotis. Dynamixel2Arduino, but default stuff depends on transmit enable pin... I have my own library https://github.com/KurtE/BioloidSerial which I earlier did some t4 testing on. Don’t remember if I added the 54 half duplex support or not

Oh cool, I mean I dont mind using the chip to select the output, I just struggle to get a library that makes the telemetry works.

At the moment I managed to use this https://github.com/jumejume1/AX-12A-servo-library and force to use Serial2 but it doesnt seems to decode the incoming messages, I can print out the bytes but it looks like there is an error coming out. I probably need to read the manual and see what it is suppose to send and receive.

I will have a look at your library :)
 
@max246 - For a long time, I never used a buffer chip(s) to work with AX servos from a Teensy 3.x.

However, most of the time I do now.

Why, a couple of reasons, which arguably may not be totally valid, but since I had some doubts, I went to buffer(s)

a) Dynamixel Spec shows that they are expecting 5v TTL. I never had issues I could pin on the fact that The T3.x and now T4 output 3.3v. I did run into issues where at times one or more AX servos would reset their ID to ID #1. Enough so, that some of the software I have for example to run a 3DOF hexapod, avoids Servo ID #1, and at sketch startup time, it searches for all of the servo IDs it expects. If it finds that one is missing and now servo ID #1 exists, it automatically changes the servo with ID #1 to the missing servo ID... If it finds that multiple servos are missing it does not do this... I personally think the resetting the servo to #1 is probably more an issue of power going into the servos and maybe they have a brown out...

b) The IO pins on the T3.6 and now T4 (earlier T3) are NOT 5v tolerant. So again depending on HOW a servo (or other devices you might put on your Dynamixel chain), generates it's response (does it drive signal high and low, or only pull low), I was afraid that maybe something will generate 5V and damage the Teensy...

Warning since I am mostly a tinkerer, I have played around doing this a few different ways. Often never fully testing the hardware, before I am off doing other things. But I plan to get back to trying my own T4 boards soon. However I am currently doing testing and making suggestions to Robotis to their library (and Arduino code) geared around their OpenCM9.04 boards. But I am doing this with the idea of better supporting the Teensy.

Example board, which I still need to try to fully assemble one and test out the connections for servos... I was tinkering with idea of making a simpler one, that removed some of the complexities. Like maybe don't worry about SD connections, or in the case of this one I had a setup for a wireless module, which could be removed...

t4-breakout-top.jpg

Problem is I get too many distractions to finish my previous distraction.
 
Hi everyone!

I'm starting a project using some (more than 20...) dynamixel XM and I was wondering if the teensy 4 will be enough to control that at a good frame rate.
Reading a SD card txt file with all the position and sync writing that to all of them.

Does robotis have a more powerful solution (like the CM or CR boards)?
Also very interested to see how far the DynamixelSDK has been ported to the teensy environment.
 
Hi @xoxu - I am not sure if there is any easy answer to your set of questions. But I will do my $.02 worth of stuff.

I have used several boards over the years to control typically up to about 20 servos. 3 DOF Hexapod, plus Pan and Tilt. I have a 4dof hexapod (RC servos run with SSC-32), but others have done 4dof Hexapods so about 26 servos. With some of these setups the main thing driving the servos was an 8 bit AVR board. So almost any of these others can control that number of servos.

A lot of things depend on your needs and what all else you wish to do and where...

First off Robotis boards: First off they are already setup to be able to handle DXL servo.
a) openCM boards are nice initial setup for simple things. The hexapod runs just fine on these. However they don't directly hook up to XL or XM servos. You can get the OpenCM485 expansion board, which adapts them to AX servos, and then you can use an AX To XL cable to connect up to X servos.
b) OpenCR - Works nicely more powerful than OpenCM direct X servo connectors.

Teensy 4 has lots going for it, with High speed and memory... Can it handle it. Sure, so can T3.5/6 and in fact I have earlier boards with T3.2 which do great.
There are others how much demos of hexapods and the like using a Teensy. Such as ones done by the user Zenta. Example:
https://www.youtube.com/watch?v=aH07qF_bhgA

Note: most of my earlier stuff, I used my own version of the Bioloid (Trossen Robotics) library, that I adapted to work with Teensy boards. It is up in my github projects (BioloidSerial)
I did/do have a version of Dynamixel SDK that I added Teensy (and AVR) support, but Robotis did not want to incorporate it.

However they are now working on some alternate libraries, that started off when they came out with the DynamixelShield. They have a library DynamixelShield that was directly coupled to AVR boards, but then to make it based on another new library Dynamixel2Arduino, which I have been working with some of their developers to allow us to easily sub-class it to allow support for other processors. You can currently install this library using Arduino library manager, or from github (https://github.com/ROBOTIS-GIT/Dynamixel2Arduino). I am mainly using github version, as working with their developer (OpusK) in a secondary branch, where we are trying out some different ideas, like APIS for SyncRead and SyncWrite...


The most interesting thing of Frame Rates, is mainly dictated by how fast stuff can go out over the Serial port. That is I typically run them at 1mhz. Could go faster... But lots of this all depends on what you are wanting the Teensy to do? Are you using it with a primary process (PC, RPI, ...), and what is the work break down...

hope that helps a little...

Kurt
 
Hey,

I’ll join this topic because I have a similar problem.

I try to run some XL320 and 2XL430-W250-T with a Teensy 3.6. Currently I´m using the following library: https://github.com/hackerspace-adelaide/XL320/tree/bene. By using an external circuit (http://nerdralph.blogspot.com/2014/01/avr-half-duplex-software-uart.html) I divide the servos RX/TX data line to be able to receive data via the teensy´s rx pin. While this approach more or less works fine with the XL320, I have problems using the 2XL430, which may be caused by the relatively old library.

Now, with the Dynamixel2Arduino library released, I’m thinking about switching.

What I currently don’t understand is how to use the “direction pin”. As far as I understand, it´s necessary to divide the rx/tx data, so it does the same as I do with the external circuit mentioned above. But I don´t really know how to use it.
Maybe I should mention that I try to control the servos with only the Teensy 3.6, no other boards involved.

@KurtE: As you seem to be working with a Teensy and the Dynamixel2Arduino library, is there a chance you can provide some example code and a quick setup plan of how to use it?
 
As for the Circuit - I have not used that form of circuit with a teensy.

I either use the built-in teensy half duplex support or I use a driver chip(or chips)... Both are supported by my older bioloidSerial library: https://github.com/KurtE/BioloidSerial

Note: For built in T3.x half duplex support you connect up to the TX pin of the Serial port.

My current board(s) that I am playing with I borrowed a circuit design from the member @jwatte, which you can see in the Trossen Robotics Thread posting:
http://forums.trossenrobotics.com/showthread.php?22956-Malum-IK&p=95055#post95055

One of the main reasons I currently prefer using the driver chips, is that the Teensy outputs 3.3v, which has worked fine for me (I think). But the specs for Dynamixels say 5V. Also with the T3.6 and T4, their pins are NOT 5V, tolerant. So having driver between, gives me a comfort level.


Dynamixel2Arduino: Allow for different code to handle serial port. Note Some of this may change, with some of the stuff we are playing with in the WIP branch, but try looking at the
example sketch: add_custom_SerialPortHandler.ino under advanced.

Right now each actual write waits until complete (does a flush) and then changes the direction pin... This can easily be changed to instead change IO registers to use native half duplex support.

As for your library working with xl320 but not XL430... I would guess that your library only supports Protocol 1, where the newer servos are running using Protocol 2.
 
Quick update: I thought I would try some of it out with the recent T4 board I assembled...

Found out I screwed up on the board and when I did a copy and paste of a couple of resistors, I forget to edit the values so I built the board with 2 wrong resistors, instead of begin something like 4.7KOhm, they were supposed to be 47 and 333 ohm...

Luckily I was able to remove the resistors and use some that were reasonably close to these values and now a servo respond...

I tried plugging in one AX-12 and did a quick set of pings to see if it would find it.

I then did a quick move the servos a little, which appears to move. Although not as smooth as I would expect... But may be simple issue with Quick and dirty test...


Code:
#include <Dynamixel2Arduino.h>

// Kurt's T4-T36 board
#define DXL_SERIAL   Serial2
const uint8_t DXL_DIR_PIN = 6; // OpenCR Board's DIR PIN.

class NewSerialPortHandler : public DYNAMIXEL::SerialPortHandler
{
  public:
    NewSerialPortHandler(HardwareSerial& port, const int dir_pin = -1)
      : SerialPortHandler(port, dir_pin), port_(port), dir_pin_(dir_pin)
    {}

    virtual size_t write(uint8_t c) override
    {
      size_t ret = 0;
      digitalWrite(dir_pin_, HIGH);

      ret = port_.write(c);

      port_.flush();
      digitalWrite(dir_pin_, LOW);

      return ret;
    }

    virtual size_t write(uint8_t *buf, size_t len) override
    {
      size_t ret;
      digitalWrite(dir_pin_, HIGH);

      ret = port_.write(buf, len);

      port_.flush();
      digitalWrite(dir_pin_, LOW);

      return ret;
    }

  private:
    HardwareSerial& port_;
    const int dir_pin_;
};

Dynamixel2Arduino dxl;
NewSerialPortHandler dxl_port(DXL_SERIAL, DXL_DIR_PIN);
uint8_t id = 0xff;
float     goal_pos = 512.0;
float     max_delta = 128.0;
float     incr = 1.0;
float     center_pos;


void setup() {
  // put your setup code here, to run once:

  // Use Serial to debug.
  while (!Serial && millis() < 5000) ;
  Serial.begin(115200);

  // Set Port instance
  dxl.setPort(dxl_port);
  // Set Port baudrate to 1000000. This has to match with DYNAMIXEL baudrate.
  dxl.begin(1000000);
  // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
  Serial.println("Setup completed");
  Serial.flush();
}

uint8_t findServo() {
  Serial.println("Search for a Servo");
  Serial.println("First try Protocol 1");
  dxl.setPortProtocolVersion(1.0);
  for (uint8_t servo_id = 0; servo_id < 253; servo_id++) {
    if (dxl.ping(servo_id) == true) {
      Serial.printf("Found ID:%u Model Number:%u\n", servo_id, dxl.getModelNumber(servo_id));
      center_pos = goal_pos = 512.0;
      max_delta = 128.0;
      incr = 1.0;
      return servo_id;
    }
  }
  Serial.println("Now try Protocol 2");
  dxl.setPortProtocolVersion(2.0);
  for (uint8_t servo_id = 0; servo_id < 253; servo_id++) {
    if (dxl.ping(servo_id) == true) {
      Serial.printf("Found ID:%u Model Number:%u\n", servo_id, dxl.getModelNumber(servo_id));
      center_pos = goal_pos = 2048.0;
      max_delta = 256.0;
      incr = 2.0;
      return servo_id;
    }
  }
  Serial.println("No Servos found");
  return 0xff;
}

void loop() {
  if (id == 0xff) {
    id = findServo();
    if (id == 0xff) {
      delay(1000);
      return;
    }
    dxl.torqueOn(id);
  }
  goal_pos += incr;
  if (goal_pos >= (center_pos + max_delta)) incr = -incr;
  if (goal_pos <= (center_pos - max_delta)) incr = -incr;
  dxl.setGoalPosition(id, goal_pos);
  if (Serial.available()) {
    Serial.println("Paused");
    while (Serial.read() != -1);
    while (Serial.read() == -1);
    while (Serial.read() != -1);
  }
}
 
Awesome.

I played around a bit, but it was really easy. Totally unexpected, everything works absolutely fine. :eek:

I still use the circuit as written in my previous post. Then adapted the SerialPortHandler as you suggested.

Code:
const uint8_t DXL_DIR_PIN = -1; // Do not use dir pin


class NewSerialPortHandler : public DYNAMIXEL::SerialPortHandler
{
  public:
    NewSerialPortHandler(HardwareSerial& port, const int dir_pin = -1)
    : SerialPortHandler(port, dir_pin), port_(port), dir_pin_(dir_pin)
    {}

    virtual size_t write(uint8_t c) override
    {
      size_t ret = 0;
      ret = port_.write(c);
      port_.flush();
      return ret;
    }

    virtual size_t write(uint8_t *buf, size_t len) override
    {
      size_t ret = 0;
      ret = port_.write(buf, len);
      port_.flush();
      return ret;     
    }

  private:
    HardwareSerial& port_;
    const int dir_pin_;
};

Tested it with a XL320 and a 2XL430. And of course, you were right. When I said I didn´t use any other boards I forgot to mention that I in fact use a level shifter to shift the 5V to 3.3V, so Teensy doesn´t take damage. The circuit mentioned is on the 3.3V side of the data line.

So thank you very much KurtE!! Made my life a lot easier ;)
 
Thank you KurtE, I started to look at the Dynamixel2Arduino library, seems to be the one that will have the most support for the moment and the most compatibility with the dynamixel range and controllers.
I plan to use Maya to do the animation, export to mov files and excelify that to transfer to sd card, not something that will change dynamically, more like a static loop display sort.
But I need smooth motion, so probably a frame rate around 50ms with maybe an interpolator for smooth travel.
 
Still playing with it...

With my own board, which I hopefully fixed some of the issues I was having running the Servos. Like wrong resistors, maybe cold solder...
And what may be a bad AX servo, I plugged in an XL430-W250 servo,

And my simple loop which increments the servos position and outputs the new location was updating > 800 times per second... So it appears to work.. Not to say there are not other issues...

I believe the current Dynamixel2Arduino only supports by default 16 servos unless you are on some specific controller, like OpenCR. The branch I am working on, I have been working with OpusK (Robotis) and I think the mapping of servos to types now support all 253 servo IDs...

The main reason for limits was to do with how the current release does things like Sync Read and Write, which is the part we are now experimenting with. Hopefully we will have a stable version that supports enough servos for the things we are talking about.

---
@xoxu - I am not at all knowing of things like Maya, Nor the formats that you will be generating, or to what level.

As how fast you can run things and how often you may need/want to tell servos stuff, really depends on how your stuff is organized and how things are done.

Example you could potentially try to store for each instant of time the exact servo positions, which could be very precise, but might take lots of space on SDCard... Then your code is as simple as reading SDCARD and directly generate a sync-write from it.

Or you could save something like: I wish to move all the servos from their current position X, the their new positions Y in time Z, and then let the driver code (or servos) handle the details, like, I am doing N updates per second, example 100. And if I wish to move a servo 50 units in a .1 seconds, than this move will take 10 servo cycles and so update the servo by 5 units per cycle...

Or maybe you are using some of the smarter servos, and you can have them do a lot of this work, by not only telling them their new positions but also telling them what velocity that should get there and control some of this by setting up some of their other parameters.

Or maybe your code is setup to do something like inverse kinematics, where you might be able to say move the unit such that the tip is at some world coordinate, and then you convert this into the angle each servo needs and then do the interpolation...

And again how fast you can update the servos depends on lots of things like:
a) What Baud rate you are using.
b) How you have the servos configured. Example return delay time which defaults to value 250 (500us), I set this to 0 so no delay. Another is the Status Return level (which you can set to 1 instead of 2) and it will not send back a status packet on commands like register writes. (Does not mater with Sync write)
c) are you only writing out information or are you also polling the servos back for information, like their current position, or load, or temp, or...
 
Hi and welcome @Fikri - Might help to know which Teensy... For now I will assume T3.x, TLC, or T4... (ie. not T2), and also which servos. i.e. some libraries only support Protocol 1, so if your servos use protocol 2...

As for the TC7WT241FU I am trying them out on my current new board... Not sure yet if I like them or not. Will see... As one circuit worked for Mega but not Teensy, maybe check to see if the parts you were trying worked with 3.3v signals or required 5v signals... And/Or that the library has code in it that works with Teensy... A

As for hardware to hook up there are a lots of different ways one can do it...

Depending on how safe you wish it to be and or within specs. Some (including myself) have gotten away driving the servos with 3.3v signals. And for example the libraries I have used: (github.com/kurte/BioloidSerial) supports both protocols, and it will support the Teensy using half duplex support built into the Serial ports. (you just need to connect to the TX pin of the Serial port)... Note: for T4, I am not sure I put in full support yet for this mode.

Next level up from this, is you could setup to use a bidirectional level shifter on the TX pin for 5v... .

Another way that I have used and may return to, is to use a setup like Robotis used on their OpenCM485 expansion board: Look at the schematic: http://support.robotis.com/en/baggage_files/opencm/schematic1___opencm_485exp.pdf

Which uses two different level shifter/buffers, one for each direction which has difference for enable (74LVC1G125, and SN74LVC1G126) chips

As a way to go down to a smaller count of parts, I am playing with TC7WT241FU, earlier I tried a different large chip which had something like 8 level shifters, with 4 with enable and 4 with NOT enable... Which worked and easy to solder, but big...

Libraries - The one I mentioned I have used up till now.

I have had a hacked up version of Dynamixel SDK supporting it.

And now the Dyanamixel2Arduino is setup (and still improving) to allow it.
 
thx for your attention KurtE
I forgot to say that I will use Teensy 4.0 for fast data processing and 18 Dynamixel AX-18A for Hexapod. I see a schematic of the expansion board. but i don't understand what is meant by AVRC5S chip. Is that logic level?
Capture.PNG
but I am not sure if using 74126 (buffer) and 7404 (not) will be safe in teensy 4.0. because when I use arduino due to control dynamixel using that circuit, my arduino is damaged. I have to be careful using teensy 4.0 because it's quite difficult to find these boards in my country
 
I believe the AVRC5S is simply another level of protection. For EMI and ESD... I have not used them yet on my own boards... http://www.1688eric.com/upload/pdf/2012-9-11/AVRC5S05Q050100R.pdf

My guess if the circuit that you used that damaged the Arduino Due would also not be good for T4 (maybe ok for T3.2 or 3.5), my guess is that it fed +5v signal to the RX pin... But again guessing without seeing the actual circuit... And again I am mainly a software guy who has fun playing with hardware. But I would double check that whatever circuit that feeds your RX pin is somehow in the range of 3.3v...
 
hi kurtE.
I have done some of the suggestions you gave before.
I have used a buffer circuit to drive Dynamixel AX-12 using teensy 4.0 with the following scheme.
Capture.PNG
I use pin 0 as RX, pin 1 as TX and pin 2 as controller. in the 74125 buffer I use a power supply of 3.3v and in 74126 I use a power supply of 5v.
I have also tried several libraries such as BioloidSerial and Dynamixel2Arduino.

previously I had adjusted the ID on dynamixel to 18 using U2D2 and it worked, but after I uploaded my program, dynamixel still didn't work.

can you help me to solve the problem above
 
Sometimes hard to debug some of these types of issues, without hardware help... That is the first thing I would do, would be to hook up a logic analyzer and see if any signals are going out and if so if anything is coming back... One can also use a scope if one has one... But there are other ways... to test... Sometimes easier if you have other processor boards to use...

You mentioned that your earlier circuit damaged an Arduino Due, so hopefully you have not damaged the T4, as again it's IO pins are not 5v tolerant.

But assuming no other hardware, some of the things I might try include: simply connecting pin 0 to pin 1 and do a simple test like:
Code:
void setup() {
    while (!Serial) ;
    Serial.begin(115200);
    Serial1.begin(1000000);
}
void loop() {
    while(Serial.available()) {
        Serial1.write(Serial1.read());
    }
    while (Serial1.available()) {
        Serial.write(Serial1.read());
    }
}
Note typed on fly, so probably typos, but entering something at the Serial monitor should echo back to you, assuming Pin 0 to pin 1 are connected and working...

If you had another processor, like UNO or T3.2/5 available that are 5v tolerant. I would hook up the output of your circuit to an RX pin of the other processor, and startup a similar program on the other board (without wires between the RX and TX). You need a common ground wire between boards. Then on T4, your program would also need to have Pin 2 state high.
(pinMode(2, OUTPUT); digitalWrite(2, HIGH);

And then either have your T4 hooked up to Serial monitor where you type something or have some simple output generated... Something like:
Code:
void setup() {
    pinMode(2, OUTPUT);
    digitalWrite(2, HIGH);
    Serial1.begin(1000000);
}
uint32_t loop_count = 0;
void loop() {
    Serial1.println(++loop_count, DEC);
    delay(500);
}

And see if you get any output on the other processor

If that works, then reverse... Have the other board do a similar SerialX output on a TX pin connected. Have it do similar like outputs like above sketch and have T4 have simple sketch, where you set pin 2 low and see if you get anything... Again simple sketch like:
Code:
void setup() {
    pinMode(2, OUTPUT);
    digitalWrite(2, LOW);
    while (!Serial) ;
    Serial.begin(115200);
    Serial1.begin(1000000);   
}
void loop() {
    while (Serial1.available()) {
        Serial.write(Serial1.read());
    }
}

If this works, then we know that your hardware is at least working somewhat. I would then try to see if I can talk to one servo. Again make sure you have it setup with 12V going to servo, common GND, signal coming from Teensy...

Maybe try something like scan_dynamixel from Dynamixel2Arduino... Stripped down like:

Code:
/*******************************************************************************
* Copyright 2016 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/

#include <Dynamixel2Arduino.h>

  #define DXL_SERIAL   Serial1
  #define DEBUG_SERIAL Serial
  const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN

#define MAX_BAUD  5
const int32_t buad[MAX_BAUD] = {57600, 115200, 1000000, 2000000, 3000000};

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

void setup() {
  // put your setup code here, to run once:
  int8_t index = 0;
  int8_t found_dynamixel = 0;

  // Use UART port of DYNAMIXEL Shield to debug.
  DEBUG_SERIAL.begin(115200);   //set debugging port baudrate to 115200bps
  while(!DEBUG_SERIAL);         //Wait until the serial port is opened
    
  for(int8_t protocol = 1; protocol < 3; protocol++) {
    // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
    dxl.setPortProtocolVersion((float)protocol);
    DEBUG_SERIAL.print("SCAN PROTOCOL ");
    DEBUG_SERIAL.println(protocol);
    
    for(index = 0; index < MAX_BAUD; index++) {
      // Set Port baudrate.
      DEBUG_SERIAL.print("SCAN BAUDRATE ");
      DEBUG_SERIAL.println(buad[index]);
      dxl.begin(buad[index]);
      for(int id = 0; id < DXL_BROADCAST_ID; id++) {
        //iterate until all ID in each buadrate is scanned.
        if(dxl.ping(id)) {
          DEBUG_SERIAL.print("ID : ");
          DEBUG_SERIAL.print(id);
          DEBUG_SERIAL.print(", Model Number: ");
          DEBUG_SERIAL.println(dxl.getModelNumber(id));
          found_dynamixel++;
        }
      }
    }
  }
  
  DEBUG_SERIAL.print("Total ");
  DEBUG_SERIAL.print(found_dynamixel);
  DEBUG_SERIAL.println(" DYNAMIXEL(s) found!");
}

void loop() {
  // put your main code here, to run repeatedly:
}

Does it find your servo... Note: I would typically update this sketch such that I can repeat running the sketch without resetting board, probably something like:

Code:
#include <Dynamixel2Arduino.h>

  #define DXL_SERIAL   Serial1
  #define DEBUG_SERIAL Serial
  const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN

#define MAX_BAUD  5
const int32_t buad[MAX_BAUD] = {57600, 115200, 1000000, 2000000, 3000000};

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

void setup() {
  // Use UART port of DYNAMIXEL Shield to debug.
  DEBUG_SERIAL.begin(115200);   //set debugging port baudrate to 115200bps
  while(!DEBUG_SERIAL);         //Wait until the serial port is opened    
 }

void loop() {
  int8_t index = 0;
  int8_t found_dynamixel = 0;

  for(int8_t protocol = 1; protocol < 3; protocol++) {
    // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
    dxl.setPortProtocolVersion((float)protocol);
    DEBUG_SERIAL.print("SCAN PROTOCOL ");
    DEBUG_SERIAL.println(protocol);
    
    for(index = 0; index < MAX_BAUD; index++) {
      // Set Port baudrate.
      DEBUG_SERIAL.print("SCAN BAUDRATE ");
      DEBUG_SERIAL.println(buad[index]);
      dxl.begin(buad[index]);
      for(int id = 0; id < DXL_BROADCAST_ID; id++) {
        //iterate until all ID in each buadrate is scanned.
        if(dxl.ping(id)) {
          DEBUG_SERIAL.print("ID : ");
          DEBUG_SERIAL.print(id);
          DEBUG_SERIAL.print(", Model Number: ");
          DEBUG_SERIAL.println(dxl.getModelNumber(id));
          found_dynamixel++;
        }
      }
    }
  }
  
  DEBUG_SERIAL.print("Total ");
  DEBUG_SERIAL.print(found_dynamixel);
  DEBUG_SERIAL.println(" DYNAMIXEL(s) found!");
  DEBUG_SERIAL.println("Press any key to run again");
  while (DEBUG_SERIAL.read() == -1) ;
  while (DEBUG_SERIAL.read() != -1);
}
Again could have typos. Also could probably remove one or two of the BAUD rates mentioned... But hopefully it will find your servo.
 
Just wanted to give a short update. With the setup and code I previously mentioned I'm able to read/write data to 4 2XL430 and 8 XL320 with a baudrate up to 115200 bps on a single uart. So I still think it is a very cheap solution without any "special" hardware that seems to work perfectly good.
 
FYI - I normally try to run my servos at 1mhz baud rate, sometimes faster. Although I don't remember if XL320 support that or not, I would suspect that they do.
 
For some reason i cant go beyond 115200 as soon as I connect more than 4 servos. Until now i couldn't figure out why. It might be due to limitations of the transistor or the diode used in my external circuit, but thats just a guess.

Btw.. do you know a built in function of dynamixel2arduino to read out values of subsequent register addresses of only one servo? Currently I use sync read for this, but as it's supposed to work with multiple servos it seems a bit overpowered to use it for only one servo.
 
I can confirm Teensy 4.0 definitely does support much faster baud rates. The max is 6 Mbit. I recently tested those speeds and fixed a bug that affected receiving at about 3 Mbit and higher.
 
Yes might be your setup. I was not sure I properly soldered up the last board I did with a T4 that had a dynamixel setup, so instead,

I hacked up a setup where I jumpered pins 0, 1, 2 of T4 to pins on an OpenCM 485 expansion board (http://emanual.robotis.com/docs/en/parts/controller/opencm485exp/#connecting-opencm904)

Teensy pins (RX)0->25, (TX)1->24, (direction) 2->22 (and a common ground)
I hooked this up to my Trossen Robotics Hexapod with AX-12 servos. in this case I applied the power 12v from wall wart to powered hub.

Note: I am using an unreleased branch of Dynamixel2Arduino code . Which I needed to update one of my test sketches:
Code:
#include <Dynamixel2Arduino.h>
//====================================================================================================
// Kurts Quick and dirty test program to find servos on either Open CM or Open CR boards
// This is a test, only a test...
//
//====================================================================================================
//============================================================================
// Global Include files
//=============================================================================

//=============================================================================
// Options...
//=============================================================================
typedef struct {
  DYNAMIXEL::SerialPortHandler *port;
  uint8_t port_number;
  uint8_t protocol_index;
} PortList;

#if defined(__OPENCM904__)
#define DXL_SERIAL0 Serial1
#define DXL_DIR_PIN0 28
DYNAMIXEL::SerialPortHandler ph1(Serial1, 28);
DYNAMIXEL::SerialPortHandler ph3(Serial3, 22);
PortList portlist[] = {
  {&ph1, 1, 1},
  {&ph1, 1, 2},
  {&ph3, 3, 1},
  {&ph3, 3, 2},
};   // Setup to handle both ports of openCR
#elif defined(__OPENCR__)
#define DXL_SERIAL0 Serial1
#define DXL_DIR_PIN0 -1
DYNAMIXEL::SerialPortHandler dxlSerialPortHandlers[] =
{DYNAMIXEL::SerialPortHandler(DXL_SERIAL0, DXL_DIR_PIN0)};
#endif
#if defined(TEENSYDUINO)
#define DXL_SERIAL0 Serial1
#define DXL_DIR_PIN0 2
DYNAMIXEL::SerialPortHandler ph1(DXL_SERIAL0, DXL_DIR_PIN0);
PortList portlist[] = {
  {&ph1, 1, 1},
  {&ph1, 1, 2}
};   // Setup to handle both ports of openCR
#define SERVO_DIRECTION_PIN 2
//#define SERVO_POWER_ENABLE_PIN  3
#else
#define SERVO_POWER_ENABLE_PIN 2
#endif

#define DEBUG_TOGGLE_PIN 13
#ifdef DEBUG_TOGGLE_PIN
#define DBGTogglePin(pin) digitalWrite(pin, !digitalRead(pin));
#else
#define DBGTogglePin(pin)
#endif
#define COUNT_PORT_LIST  (sizeof(portlist)/sizeof(portlist[0]))
Dynamixel2Arduino dxl;

//=============================================================================
// Define different robots..
//=============================================================================

// Protocol version
#define PROTOCOL_VERSION                1.0                 // See which protocol version is used in the Dynamixel
#define PROTOCOL_VERSION2                2.0                 // See which protocol version is used in the Dynamixel
#define DEVICENAME                      "3"                 // Check which port is being used on your controller
#define DXL_BAUDRATE 1000000

/** EEPROM AREA **/
#define AX_PRESENT_POSITION_L       36
//=============================================================
// Defines for X series (XL430)
//=============================================================================
#define DXL_X_MODEL_NUMBER         0    // 2 (R)
#define DXL_X_PRESENT_POSITION     132 // 4 (R)



//=============================================================================
// Globals
//=============================================================================
// Global objects
// Handle to port handler and packet handler;
uint8_t g_servo_protocol[254];
uint32_t g_baud_rate = DXL_BAUDRATE;

void wait_keyboard() {
  Serial.println("Hit any key to continue"); Serial.flush();
  while (!Serial.available()) ;
  while (Serial.read() != -1) ;
}
//====================================================================================================
// Setup
//====================================================================================================
void setup() {

  while (!Serial && (millis() < 3000)) ;  // Give time for Teensy and other USB arduinos to create serial port
  Serial.begin(38400);  // start off the serial port.
  Serial.println("\nCM9.04 Find Servos program");
  //  wait_keyboard();
#ifdef SERVO_POWER_ENABLE_PIN
  pinMode(SERVO_POWER_ENABLE_PIN, OUTPUT);
  digitalWrite(SERVO_POWER_ENABLE_PIN, HIGH);
#endif
#if defined(BOARD_OpenCM904) && (DXL_SERIAL == Serial1)
  Serial.println("*** OpenCM9.04 setDxlMode ***");
  Serial1.setDxlMode(true);
#endif

  // Set the protocol version
  // Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
  // Initialize PortHandler instances
#ifdef DEBUG_TOGGLE_PIN
  pinMode(DEBUG_TOGGLE_PIN, OUTPUT);
#endif
  // Lets start of trying to locate all servos.
  SetBaudRate(DXL_BAUDRATE);

  FindServos();


}


//====================================================================================================
// Loop
//====================================================================================================
void loop() {
  // Output a prompt

  // lets toss any charcters that are in the input queue
  Serial.println("\nFind Again, enter new baud or just hit enter");
  Serial.flush();  // make sure the complete set of prompts has been output...
  while (Serial.read() != -1) ; // Remove any stuff still in serial buffer.
  // Get a command
  uint32_t new_baud = CheckForNewBaud();
  if (new_baud) {
    SetBaudRate(new_baud);
  }
  FindServos();
}

//=======================================================================================
XelInfoFromPing_t ping_info[32];
void FindServos(void) {

  Serial.print("\nSearch for all servos at baud rate: ");
  Serial.println(g_baud_rate, DEC);

  // Initialize to no servos...
  for (int i = 0; i < 254; i++) {
    g_servo_protocol[i] = 0; // not found
  }
  for (uint8_t port_index = 0; port_index < COUNT_PORT_LIST; port_index++) {
    DBGTogglePin(DEBUG_TOGGLE_PIN);
    dxl.setPort(portlist[port_index].port);
    Serial.print("Begin Searching on Port: ");
    Serial.println(portlist[port_index].port_number, DEC);

    dxl.setPortProtocolVersionUsingIndex(portlist[port_index].protocol_index);
    Serial.println(dxl.getPortProtocolVersion(), 2);
    //  wait_keyboard();
    if (portlist[port_index].protocol_index == 1) {
      Serial.println("  Begin Protocol 1: ");
      for (int i = 0; i < 254; i++) {
        //Serial.print(".");
        if (dxl.ping(i)) {
          if (g_servo_protocol[i]) {
            Serial.println("Multiple servos found with same ID");
          }
          g_servo_protocol[i] = 1;
          Serial.printf("  %d Type:%d Position:%d\n", i,
                        dxl.getModelNumber(i), static_cast<int>(dxl.getPresentPosition(i))) ;
        }
      }

      Serial.println("  Done");
    } else {
      Serial.println("  Begin Protocol 2 Simple Ping: ");
      for (uint8_t i = 0; i < 254; i++) {
        //Serial.print("-");
        if (dxl.ping(i)) {
          if (g_servo_protocol[i] == 1) {
            Serial.println("Multiple servos found with same ID");
          }
          g_servo_protocol[i] = 2;
          Serial.printf("  %d Type:%d Position:%d\n", i,
                        dxl.getModelNumber(i), static_cast<int>(dxl.getPresentPosition(i))) ;
        }
      }
      Serial.println("  Begin Protocol 2 ping data: ");
      for (uint8_t i = 0; i < 254; i++) {
        if (dxl.ping(i, ping_info, 1)) {
          //        if (((DYNAMIXEL::Master)dxl).ping(i, ping_info, 1)) {
          if (g_servo_protocol[i] == 1) {
            Serial.println("Multiple servos found with same ID");
          }
          g_servo_protocol[i] = 2;
          Serial.printf("  %d Type:%x Ver:%x Position:%d \n", i,
                        ping_info[0].model_number, ping_info[0].firmware_version,
                        static_cast<int>(dxl.getPresentPosition(i))) ;
        }
      }
      Serial.println("  Try Protocol 2 - broadcast ping: ");
      Serial.flush(); // flush it as ping may take awhile...

      if (uint8_t count_pinged = dxl.ping(DXL_BROADCAST_ID, ping_info,
                                          sizeof(ping_info) / sizeof(ping_info[0]))) {
        Serial.print("Detected Dynamixel : \n");
        for (int i = 0; i < count_pinged; i++)
        {
          Serial.print("    ");
          Serial.print(ping_info[i].id, DEC);
          Serial.print(", Model:");
          Serial.print(ping_info[i].model_number, HEX);
          Serial.print(", Ver:");
          Serial.println(ping_info[i].firmware_version, HEX);
          g_servo_protocol[i] = 2;
        }
      } else Serial.printf("Broadcast returned no items(%x)\n", dxl.getLastLibErrCode());

      Serial.println("  Done");
    }
  }
}
//=======================================================================================
//=======================================================================================


//====================================================================================================
// Process command line, optionally return new baud rate
uint32_t CheckForNewBaud(void) {
  int ch;
  uint32_t new_baud = 0;

  for (;;) {
    // throw away any thing less than CR character...
    ch = Serial.read();
    if (ch != -1) {
      if ((ch >= '0') && (ch <= '9')) {
        new_baud = new_baud * 10 + (uint32_t)(ch - '0');
      } else {
        break;
      }
    }
  }
  while (Serial.read() != -1) ; // remove any trailing stuff.
  return new_baud;
}


//=======================================================================================
void SetBaudRate(uint32_t new_baud)
{
  Serial.print("Setting Baud to: ");
  Serial.println(new_baud);

  for (uint8_t i = 0; i < COUNT_PORT_LIST; i++) {
    DBGTogglePin(DEBUG_TOGGLE_PIN);
    Serial.printf("  Index: %d\n", i);
    dxl.setPort(portlist[i].port);
    dxl.begin(new_baud);
    delay(100);
    for (uint8_t id = 0; id < 254; id++) dxl.reboot(id);
  }
  g_baud_rate = new_baud;
}

All of the servos are configured for 1000000.

Test output:
Code:
Search for all servos at baud rate: 1000000

Begin Searching on Port: 1

1.00

  Begin Protocol 1: 

  2 Type:12 Position:630
  3 Type:12 Position:169
  5 Type:12 Position:853
  6 Type:12 Position:266
  7 Type:12 Position:531
  8 Type:12 Position:488
  9 Type:12 Position:208
  10 Type:12 Position:134
  11 Type:12 Position:913
  12 Type:12 Position:425
  13 Type:12 Position:507
  14 Type:12 Position:502
  15 Type:12 Position:452
  16 Type:12 Position:232
  17 Type:12 Position:970
  18 Type:12 Position:634
  19 Type:12 Position:369
  Done

Begin Searching on Port: 1

2.00

  Begin Protocol 2 Simple Ping: 

  Begin Protocol 2 ping data: 

  Try Protocol 2 - broadcast ping: 

Broadcast returned no items(3)
  Done
Logic Analyzer output... Note I am playing with the Saleae Alpha Logic 2 build for this.
screenshot.jpg

The good part is my own Protocol decoder for Dynamixels still works on this alpha build :D


Now how to read in multiple things... You will note that if you look at the main class definition in Dynamixel2Arduino.h you will see that it is a sub class of the class master...

Now if you go into the class definition for master.

You will find it has more lower level functions that may be what you are looking for, like:
Code:
   int32_t read(uint8_t id, uint16_t addr, uint16_t addr_length,
      uint8_t *p_recv_buf, uint16_t recv_buf_capacity, uint32_t timeout_ms = 10);
 
Looks nice!
Did you also try to get such good results with the teensys built-in half duplex support? I´m thinking about using it instead of the circuit mentioned earlier, hoping to be able to use baud rates higher than 115200. Currently, my setup at least works and because there´s not much time left on the project i´m triying to estimate the chances of success and the time effort neccessary to switch to the build-in half duplex...

The read function was exactly what i was looking for, shame on me i did't find it on my own ;) Thank you!
 
Last edited:
Hi everyone,

I came across this thread as I'm starting to build some firmware for a robotic hand with 4 fingers, each with 3 servos (so 12 dynamixels in total). I thought this would be a good opportunity to give back to the community.
I have designed and tested a custom Dynamixel shield for Teensy 4.0 with 4 TTL interfaces and 1 RS485 interface. So far I've had no problems communicating with the servos (XM430-W210) using the Dynamixel2Arduino library.
The shield design is publicly available in the CircuitMaker repository
You can use that project to export design files you can send to a PCB supplier (I use MacroFab but there are cheaper ones, MacroFab will charge about $80 for a single populated board - not counting the Teensy or any through hole components). You can probably find cheaper solutions if you do your own soldering but the board is pretty small and the components are 0603.
I have attached the schematic as well in case you are curious about the interface circuit: it uses the SN74LVC1G125DCKR and SN74LVC1G126DCKR buffers for the TTL interfaces and a MAX3485EESA+ for the RS485 interface.

View attachment Dynamixel Shield Schematic.pdf

Board.PNG

POWER:
- The shield can be powered either through a screw block connector or a DC Jack, the user can decide which one to solder onto the board. Take into account the DC Jack is only rated for 5A, whereas the block connector is rated for 17A (TE Connectivity 282841-2)
- The power trace is capable of delivering about 10A continuously, so take this into account when connecting your servos to the board. You can temporarily draw higher currents, but not for extended periods of time.
- The voltage connected to either the DC jack or block connector is directly routed to the Servos!
- If using the block connector PAY ATTENTION TO THE POLARITY, marks on the board tell you which terminal is positive (+) and which one is ground (-)
- The DC Jack should be have a positive tip (sleeve is ground). Board was designed with this particular DC Jack in mind: PJ-202AH.
- There are 2 ways to power the Teensy:
1) Using the USB Power. You might need to restore the connection between VIN and VUSB if you have cut it before. Remove the jumper on labeled "5V Power". Probably the way to go if you are streaming motor command through USB
2) Using the dedicated 5V regulator on board: In this case the Teensy will draw power from your block connector/DC Jack. To do this, you MUST CUT the trace between VIN and VUSB on the Teensy board (see Teensy documentation on external power supplies). Additionally you will need to add the jumper labeled "5V Power".

DYNAMIXEL CONNECTIONS:
- The connectors are labeled J1 through J5 and each one is directly connected to the UART of the same number: J1 is physically connected to UART1 and so on.
- J1 through J4 are TTL interfaces and J5 is an RS-485 interface for Dynamixels that support it.
 
Last edited:
Status
Not open for further replies.
Back
Top