Dynamixel and Half-duplex serial usage on Teensy4.1

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.)
 
Back
Top