Dynamixel and Half-duplex serial usage on Teensy4.1

A

avsteele

Member
I would like to use a Dynamixel servo, using their TTL serial protocol, with the Teensy4.1.

There are a number of posts covering similar topics from some time ago, but more recently the half duplex capability of the native UART was added to the Teensyduino library, and I don't see here or on GitHub any examples using this capability with the Dynamixel2 library

Using the half-duplex feature I ran a simple test (1st code block below) and can see bidirectional communication between serial2 and serial 3 by just attaching their TX lines to each other. This works fine at a baud of 1M, however i do see that a delay is needed between read and write which is longer than expected, not sure if this will cause any issue down the line.

So my question is, would talking to a Dynamixel be as simple as the code in the second code block below? Assume I will inserting a 3.3 <-> 5V bidirectional level shifter between them

Two-way on the Teensy
C++: 
#include <Arduino.h>
const uint32_t baud = 1000000;

#define SER1 Serial2 // pins 7 (RX), 8 (TX),  but just uses 8 (TX) 
#define SER2 Serial3 // pins 15 (RX), 14 (TX), but just uses 14 (TX)
void send_recieve(HardwareSerialIMXRT &sender, HardwareSerialIMXRT &rcvr);
void setup()
{
  Serial.begin(9600);
  Serial.println("Starting...");
  SER1.begin(baud, SERIAL_8N1_HALF_DUPLEX);
  SER2.begin(baud, SERIAL_8N1_HALF_DUPLEX);
}
void loop()
{
  send_recieve(SER1, SER2);
  delay(500);  
  send_recieve(SER2, SER1);
  delay(500);
}
void send_recieve(HardwareSerialIMXRT &sender, HardwareSerialIMXRT &rcvr)
{
  static byte send = 0x40;
  send++;
  if( send>126) send= 41;
  sender.write(send); 
  // DELAY IS REQUIRED
  // without this delay the data is corrupted
  // 1st term is my guess fo rminimum, but this fails without a big extra amount
  delayMicroseconds((8+1)*1000000/baud + 100);
  int got = rcvr.read();
  if (got == -1 || got == 0)
  {
    Serial.println("No data");
    return;
  }
  Serial.print(&sender == &SER1 ? "SER1 -> SER2: " : "SER2 -> SER1: ");
  Serial.print((char)send);
  Serial.print(" -> ");
  Serial.println((char)got);    
}

Proposed Dynamixel test stub

C++: 
#include <Dynamixel2Arduino.h>
#define DXL_SERIAL Serial2
#define DXL_DIR_PIN 6 // just a DUMMY for the dxl library constructor, not actually used?
const float DXL_PROTOCOL_VERSION = 2.0;
const uint8_t DXL_ID = 1; // id of the connected servo


Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

void setup() {

  DXL_SERIAL.begin(baud, SERIAL_8N1_HALF_DUPLEX);
  // should be fine for XL330-M288-T
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
}

void loop() {
  bool pinged = dxl.ping(DXL_ID);
  Serial.println(pinged);
  delay(1000);
}

Related, thanks to @KurtE for his posts on this subject
 
In answer to the above. I can report that it does in indeed work, but with two changes:

in the Dynamixel2Arduino library in
Code: 
/src/utility/port-handler.cpp -> void SerialPortHandler::begin(unsigned long baud)...
, line 53 change

C++: 
  //port_.begin(baud_); // from this, to teh below
  port_.begin(baud_, SERIAL_8N1_HALF_DUPLEX);

Then your arduino code can look like:

C++: 
#include <Dynamixel2Arduino.h>
#define DXL_SERIAL Serial2 // use pin 8 of Teensy4.1
#define DXL_DIR_PIN -1 // -1 means this isn't used/needed
const float DXL_PROTOCOL_VERSION = 2.0;
// be sure to get Id and baud correct, use dynamixel wizard to connect if needed,
//   these below are the defaults for my model XL330-M288-T
const uint8_t DXL_ID = 1;
const uint baud = 57600;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

void print_error(const char * what);

bool ping_success = false;

void setup() {

  dxl.begin(baud); 
  //dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); //not needed consturctor default is 2
  ping_success = dxl.ping(DXL_ID);
  if(!ping_success){
    Serial.print("Ping failed -> Error code: ");
    Serial.println(dxl.getLastLibErrCode());
  }
}

With the above all your need to do is hook up pin 8 to the Dynamixel data line though a 3.V -> 5V level shifter. I suggest using some limitng resistors (50-100 ohm) to prevent ringing. And of course also hook up the servo's 5V power and ground.

I have tested a few functions so far
  • ping,
  • getPresentPosition,
  • setGoalPosition
  • torqueOn
All work fine.

(PS: setting small changes in the goal position does not work unless you change the PID's `P` gain from the default of 400 to 3600.)
 
I am trying to control a Dynamixel AX-18A servo using Teensy 4.1 via Serial2 (TX: pin 8, RX: pin 7). For half-duplex communication, I am using IC HD74LS241P as a buffer between Teensy and Dynamixel.

However, I ran into a problem where the servo did not respond to the commands I sent. I've tested the same code on Blackpill's STM32F4, and there the servo can move correctly.

The following are some of the things that I have checked:

  • Baudrate is set to 1,000,000 bps.
  • IC HD74LS241P:
    I used HD74LS241P to set up half-duplex communication.
    The DIR (direction control) pin is controlled for switching between TX and RX.
    Power Supply for Dynamixel is enough (using 12V 5A adapter).
    Code to send commands to the servo:
The Full Code
C++: 
#include <Dynamixel2Arduino.h>

#define DXL_SERIAL Serial2
#define DEBUG_SERIAL Serial // Make sure this corresponds to the debugging port
const uint8_t DXL_DIR_PIN = 27;
const float DXL_PROTOCOL_VERSION = 1.0;
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

void setup() {
  DEBUG_SERIAL.begin(115200);
  while (!DEBUG_SERIAL);

  dxl.begin(1000000);
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);

  // Wait a few seconds to make sure everything is ready
  delay(3000);

  // Make sure the servo is in the correct mode
  dxl.torqueOff(19);
  dxl.setOperatingMode(19, OP_POSITION);
  dxl.torqueOn(19);

  // Try to set the initial position
  if (dxl.setGoalPosition(19, 63, UNIT_DEGREE)) {
    DEBUG_SERIAL.println("Starting position successfully set to 63 degrees");
  } else {
    DEBUG_SERIAL.println("Failed to set the starting position");
  }
}

void loop() {
  delay(3000);
  if (dxl.setGoalPosition(19, 63, UNIT_DEGREE)) {
    DEBUG_SERIAL.println("Successfully set position to 63 degrees");
  } else {
    DEBUG_SERIAL.println("Failed to set position to 63 degrees");
  }

  delay(3000);
  if (dxl.setGoalPosition(19, 165, UNIT_DEGREE)) {
    DEBUG_SERIAL.println("Successfully set position to 165 degrees");
  } else {
    DEBUG_SERIAL.println("Failed to set position to 165 degrees");
  }
}

Here's the circuit
skematik.jpg


I have also tried some other libraries such as Dynamixel Serial from :
Dynamixel Serial Zach Shiner

and BioloidSerial from :
BioloidSerial KurtE
but it doesn't work either, where my dynamixel servo doesn't move.

If allowed, can you provide the Library and code examples used and how the circuit you use.
 
Why are you powering the 74LS241 with 8V? That seems like it would be out-of-spec and is definitely dangerous to connect to the Teensy's RX pin.
 
jmarsh said:
Why are you powering the 74LS241 with 8V? That seems like it would be out-of-spec and is definitely dangerous to connect to the Teensy's RX pin.
Click to expand...
I just continued the pre-existing circuit, I don't know where they got the source, but yesterday they used STM32F407 and STM32F401 Blackpill which have the same output as teensy. They use Dynamixel serial library and work on STM32*.
I have also tried using 5V voltage ( I also read references from : Makertut Dynamixel Arduino) but it still does not work on Teensy. I have also tried using all the Serials available on teensy, but it still doesn't work.
 
Most STM32s are 5V tolerant. The Teensy 4.1 is not. You will damage it with that voltage if you haven't already.
 
You schematic can be much simpler if you get an auto direction-sensing shifter; i used https://www.ti.com/product/TXS0108E. You don't need to mess with the direction pin at all.

  1. Power the chip A side with 3.3V, B side with 5V
  2. Hook Teensy pin 8 directly to A1 though a series ~100 ohm R
  3. Hook the Dynamixel data line to B1 though a similar series resistor
That's it.

I think its possible high speeds might be a problem since the auto-direction sensing uses a internal pullup. I think I only tested Dynamixel comms at my device-specific default baud of, I think, 57600. You might try using lowering the baud with whatever comms do work from 1M to somethign lower at first.
 
And any 74LS series chip will risk being fried if the supply voltage peaks above 7V... Definitely use 5V for any TTL. 3.3V for Teensy 4 means you can't directly interface Teensy and TTL.
 
You must log in or register to reply here.
Back
Top