Problem with syncRead of dynamixel workbench

[p123ad]

Hello, I am trying to sync read and write data to multiple Dynamixels.

Setup: OpenCR board 2x MX430-W350-T 2x XM540-W270-T 2x XM540-W270-R

Writing is no problem. But reading produces problems:

Can it be that the problem comes up because of the difference between TTL and RS485 ? When I sync read only the first four dynamixels which use TTL it wors fine. And only reading the 3 Motors with RS485 alone works also fine.

 #include <DynamixelWorkbench.h>

 #define DEVICE_NAME  "" 
 #define BAUDRATE     57600

#define HEAD_PITCH             1      //TTL
 #define NECK_YAW               2     //TTL
 #define RIGHT_SHOULDER_ROLL    3     //RS485
 #define RIGHT_SHOULDER_PITCH   5   //RS485
 #define RIGHT_ELBOW_PITCH      7    //RS485
 #define RIGHT_HIP_YAW          9    //TTL
 #define RIGHT_HIP_ROLL         11      //TTL

 DynamixelWorkbench dxl_wb;

const uint8_t dxl_num = 7;
uint8_t dxl_id[dxl_num] = {
                                HEAD_PITCH, 
                                NECK_YAW,                           
                                RIGHT_HIP_YAW,
                                RIGHT_HIP_ROLL,
                                RIGHT_SHOULDER_ROLL,
                                RIGHT_SHOULDER_PITCH,
                                RIGHT_ELBOW_PITCH     };

 int32_t goal_position[dxl_num] = {0, 0, 0, 0, 0, 0, 0};  //array contins the goal positions for the motors

 const uint8_t handler_index = 0;

 void setup ()
 {
  Serial.begin(57600);
  while(!Serial); // Wait for Opening Serial Monitor

  const char *log;
  bool result = false;

  uint16_t model_number = 0;

  result = dxl_wb.init(DEVICE_NAME, BAUDRATE, &log);
  if (result == false)
  {
    Serial.println(log);
    Serial.println("Failed to init");
  }
  else
  {
    Serial.print("Succeeded to init : ");
    Serial.println(BAUDRATE);  
  }

  for (int cnt = 0; cnt < dxl_num; cnt++){
    result = dxl_wb.ping(dxl_id[cnt], &model_number, &log);
    if (result == false)
    {
      Serial.println(log);
      Serial.println("Failed to ping");
    }
    else
    {
      Serial.println("Succeeded to ping");
      Serial.print("id : ");
      Serial.print(dxl_id[cnt]);
      Serial.print(" model_number : ");
      Serial.println(model_number);
    }

    result = dxl_wb.jointMode(dxl_id[cnt], 0, 0, &log);
    if (result == false)
    {
      Serial.println(log);
      Serial.println("Failed to change joint mode");
    }
    else
    {
      Serial.println("Succeed to change joint mode");
    }
  }

  result = dxl_wb.addSyncWriteHandler(dxl_id[0], "Goal_Position", &log);
  if (result == false)
  {
    Serial.println(log);
    Serial.println("Failed to add sync write handler for Goal_Position");
  }

  result = dxl_wb.addSyncReadHandler(dxl_id[0], "Present_Position", &log);
  if (result == false)
  {
    Serial.println(log);
    Serial.println("Failed to add sync read handler for Present_Position");
  }

}

/*
 * Swap function takes an array as input and swaps each entry between 0 and 1023
 */
void swap(int32_t *array)
{
  for(int cnt = 0; cnt < dxl_num; cnt++){
    if(array[cnt] == 1023){
      array[cnt] = 0;
    }
    else{
      array[cnt] = 1023;  
    }
  }
}

void loop()
{
  const char *log;
  bool result = false;
  int32_t present_position[7] = {0, 0, 0, 0, 0, 0, 0};

  result = dxl_wb.syncWrite(handler_index, goal_position, &log);
  if (result == false)
  {
    Serial.println(log);
    Serial.println("Failed to sync write position");
  }

  result = dxl_wb.syncRead(handler_index, dxl_id, 5, &log);
    if (result == false)
    {
      Serial.println(log);
      Serial.println("Failed to sync read position");
    }

    result = dxl_wb.getSyncReadData(handler_index, dxl_id, 5, present_position, &log);
    if (result == false)
    {
      Serial.println(log);
    }
    else
    {
      Serial.print("[ID ");
      Serial.print(dxl_id[0]);
      Serial.print(" ]");
      Serial.print(" Goal Position : ");
      Serial.print(goal_position[0]);
      Serial.print(" Present Position : ");
      Serial.print(present_position[0]);
      Serial.print(" [ID ");
      Serial.print(dxl_id[1]);
      Serial.print(" ]");
      Serial.print(" Goal Position : ");
      Serial.print(goal_position[1]);
      Serial.print(" Present Position : ");
      Serial.print(present_position[1]);
      Serial.print(" [ID ");
      Serial.print(dxl_id[2]);
      Serial.print(" ]");
      Serial.print(" Goal Position : ");
      Serial.print(goal_position[1]);
      Serial.print(" Present Position : ");
      Serial.println(present_position[2]);
      Serial.print(" [ID ");
      Serial.print(dxl_id[3]);
      Serial.print(" ]");
      Serial.print(" Goal Position : ");
      Serial.print(goal_position[3]);
      Serial.print(" Present Position : ");
      Serial.println(present_position[3]);
      Serial.print(" [ID ");
      Serial.print(dxl_id[4]);
      Serial.print(" ]");
      Serial.print(" Goal Position : ");
      Serial.print(goal_position[4]);
      Serial.print(" Present Position : ");
      Serial.print(present_position[4]);
      Serial.print(" [ID ");
      Serial.print(dxl_id[5]);
      Serial.print(" ]");
      Serial.print(" Goal Position : ");
      Serial.print(goal_position[5]);
      Serial.print(" Present Position : ");
      Serial.println(present_position[5]);
    }

  swap(goal_position);
  delay(200)

}

Thank you for your help!

[ROBOTIS-Will]

Hi @p123ad In OpenCR TTL and RS-485 is electrically disconnected while they are sharing the TX and RX of the MCU of OpenCR. The TX is shared to both TTL and RS-485, on the other hand, RX from TTL or RS-485 cannot be shared, therefore, reading from different physical communication types is unavailable. Thank you.

[p123ad]

Thank you for that good explanation!

But is it than possible to have two different syncReadHandler? Can I split up the motors by TTL or RS-485?

[ROBOTIS-Will]

Yes, you can create multiple handlers with different object names.

[p123ad]

hey, Unfortunately I can not change my motor setup. So I have a few Motors with TTL and some with RS-485 but I want to treat them the same. I want to read the "Present_Position" object from all motors.

But I can not find the solution for that! Do you have any sugggestions?

[ROBOTIS-Will]

The Workbench libary in OpenCR allows you to create up to 5 handlers. You can identify each handler by the handler_index. For example,

uint8_t dxl_ttl_id[4] = {
                                HEAD_PITCH, 
                                NECK_YAW,                           
                                RIGHT_HIP_YAW,
                                RIGHT_HIP_ROLL     };
uint8_t dxl_485_id[3] = {
                                RIGHT_SHOULDER_ROLL,
                                RIGHT_SHOULDER_PITCH,
                                RIGHT_ELBOW_PITCH     };

// Writing to TTL / RS-485 DYNAMIXEL with SyncWrite doesn't need to be differentiated.
result = dxl_wb.addSyncWriteHandler(dxl_id[0], "Goal_Position", &log);

// This handler with handler_index = 0 will be used to read from TTL DYNAMIXEL
// handler_index is automatically increased in addSyncReadHander()
result = dxl_wb.addSyncReadHandler(dxl_ttl_id[0], "Present_Position", &log); 

// This handler with handler_index = 1 will be used to read from RS-485 DYNAMIXEL
result = dxl_wb.addSyncReadHandler(dxl_485_id[0], "Present_Position", &log); 

...
// Read from TTL DYNAMIXEL
result = dxl_wb.syncRead(0, &log); 

...
// Read from RS-485 DYNAMIXEL
result = dxl_wb.syncRead(1, &log); 

[p123ad]

Thank you for the quick response!

That was also my idea, but it is not working:

Result:

Code:

 #include <DynamixelWorkbench.h>

 //Dynamixel Baudrate
 #define BAUDRATE     57600

//defining DXL IDs
 #define HEAD_PITCH             1     //TTL 
 #define NECK_YAW               2     //TTL 
 #define RIGHT_SHOULDER_ROLL    3     //RS-485
 #define RIGHT_SHOULDER_PITCH   5     //RS-485
 #define RIGHT_ELBOW_PITCH      7     //RS-485
 #define RIGHT_HIP_YAW          9     //TTL 
 #define RIGHT_HIP_ROLL         11    //TTL  

const uint8_t dxl_num = 7;
uint8_t dxl_id[dxl_num] = {
                                HEAD_PITCH, 
                                NECK_YAW,                           
                                RIGHT_HIP_YAW,
                                RIGHT_HIP_ROLL,
                                RIGHT_SHOULDER_ROLL, 
                                RIGHT_SHOULDER_PITCH,                           
                                RIGHT_ELBOW_PITCH };

const uint8_t dxl_ttl_num = 4;
uint8_t dxl_ttl_id[dxl_ttl_num] = {
                                HEAD_PITCH, 
                                NECK_YAW,                           
                                RIGHT_HIP_YAW,
                                RIGHT_HIP_ROLL };

const uint8_t dxl_485_num = 3;
uint8_t dxl_485_id[dxl_485_num] = {
                                RIGHT_SHOULDER_ROLL, 
                                RIGHT_SHOULDER_PITCH,                           
                                RIGHT_ELBOW_PITCH };

 DynamixelWorkbench dxl_wb;

 int32_t goal_position[dxl_num] = {0, 0, 0, 0, 0, 0, 0};  //array contains the goal positions for the motors

/* 
 * Setup dxl communication  
 *  1. Init dxl bus
 *  2. Ping all motors
 *  3. Set Joint Mode to motors
 *  4. Add a sync write handler with index 0
 *  5. Add a sync read handler for TTL with index 0
 *  6. Add a sync read handler for RS-485 with index 1
 */

 void setup()
 {
  Serial.begin(57600);
  while(!Serial); // Wait for Opening Serial Monitor

  const char *log;
  bool result = false;

  uint16_t model_number = 0;

  result = dxl_wb.init("", BAUDRATE, &log);
  if (result == false)
  {
    Serial.println(log);
    Serial.println("Failed to init");
  }
  else
  {
    Serial.print("Succeeded to init : ");
    Serial.println(BAUDRATE);  
  }

  for (int cnt = 0; cnt < dxl_num; cnt++){
    result = dxl_wb.ping(dxl_id[cnt], &model_number, &log);
    if (result == false)
    {
      Serial.println(log);
      Serial.println("Failed to ping");
    }
    else
    {
      Serial.println("Succeeded to ping");
      Serial.print("id : ");
      Serial.print(dxl_id[cnt]);
      Serial.print(" model_number : ");
      Serial.println(model_number);
    }

    result = dxl_wb.jointMode(dxl_id[cnt], 0, 0, &log);
    if (result == false)
    {
      Serial.println(log);
      Serial.println("Failed to change joint mode");
    }
    else
    {
      Serial.println("Succeed to change joint mode");
    }
  }
  //Handler with handler_index = 0 writes to TTL / RS-485 DYNAMIXEL
  result = dxl_wb.addSyncWriteHandler(dxl_id[0], "Goal_Position", &log);
  if (result == false)
  {
    Serial.println(log);
    Serial.println("Failed to add sync write handler for Goal_Position");
  }

  //Handler with handler_index = 0 reads from TTL DYNAMIXEL
  result = dxl_wb.addSyncReadHandler(dxl_ttl_id[0], "Present_Position", &log);
  if (result == false)
  {
    Serial.println(log);
    Serial.println("Failed to add sync read handler for TTL Present_Position");
  }

  //Handler with handler_index = 1 reads from RS-485 DYNAMIXEL
  result = dxl_wb.addSyncReadHandler(dxl_485_id[0], "Present_Position", &log);
  if (result == false)
  {
    Serial.println(log);
    Serial.println("Failed to add sync read handler for RS-485 Present_Position");
  } 
}

/*
 * Swap function takes an array as input and swaps each entry between 0 and 1023
 */
void swap(int32_t *array)
{
  for(int cnt = 0; cnt < dxl_num; cnt++){
    if(array[cnt] == 1023){
      array[cnt] = 0;
    }
    else{
      array[cnt] = 1023;  
    }
  }
}

void loop()
{
  const char *log;
  bool result = false;

  result = dxl_wb.syncWrite(0, goal_position, &log);
  if (result == false)
  {
    Serial.println(log);
    Serial.println("Failed to sync write position");
  }

  //Read from TTL DYNAMIXEL
  result = dxl_wb.syncRead(0, &log);
    if (result == false)
    {
      Serial.println(log);
      Serial.println("Failed to sync read TTL position");
    }

  //Read from RS-485 DYNAMIXEL
  result = dxl_wb.syncRead(1, &log);
    if (result == false)
    {
      Serial.println(log);
      Serial.println("Failed to sync read RS-485 position");
    }

  swap(goal_position);

  delay(1200);
}