Need help for example for Bulk Read of Dynamixel Motors (MX106T) using Dynamixel Shield

[bryan]

Hi all, I am trying to use sync read to read different addresses (Current and position) from multiple MX106 (ID 0 and 1). I’m not sure how to add different parameters to be sent for bulk sync. This was modified from an example on bulk sync for a dynamixel shield connected to an MCU with 2 UART

#include <DynamixelShield.h>

DynamixelShield dxl(Serial2, 18);  // Using Serial2 for Dynamixel communication

const uint8_t BROADCAST_ID = 254;
const float DYNAMIXEL_PROTOCOL_VERSION = 2.0;
const uint8_t DXL_ID_CNT = 2;
const uint8_t DXL_ID_LIST[DXL_ID_CNT] = {0, 1};  // Motor IDs 0 and 1
const uint16_t user_pkt_buf_cap = 128;
uint8_t user_pkt_buf[user_pkt_buf_cap];

// Starting addresses and lengths of data to read for each Dynamixel motor
const uint16_t SR_START_ADDRS[DXL_ID_CNT] = {132, 126};  // Present Position and Present Current
const uint16_t SR_ADDR_LENGTHS[DXL_ID_CNT] = {4, 2};      // Lengths of data for each starting address

typedef struct sr_data {
  int32_t present_position;
  int16_t present_current;
} __attribute__((packed)) sr_data_t;

sr_data_t sr_data;
DYNAMIXEL::InfoSyncReadInst_t sr_infos;
DYNAMIXEL::XELInfoSyncRead_t info_xels_sr[DXL_ID_CNT];

void setup() {
  uint8_t i;
  // pinMode(LED_BUILTIN, OUTPUT);
  Serial.begin(115200);  // Using Serial for printing debug information
  dxl.begin(115200);     // Using Serial2 for Dynamixel communication
  dxl.setPortProtocolVersion(DYNAMIXEL_PROTOCOL_VERSION);

  for (i = 0; i < DXL_ID_CNT; i++) {
    dxl.torqueOff(DXL_ID_LIST[i]);
    dxl.setOperatingMode(DXL_ID_LIST[i], OP_POSITION);
  }
  dxl.torqueOn(BROADCAST_ID);

  // Fill SyncRead structure members using an external user packet buffer
  sr_infos.packet.p_buf = user_pkt_buf;
  sr_infos.packet.buf_capacity = user_pkt_buf_cap;
  sr_infos.packet.is_completed = false;
  sr_infos.p_xels = info_xels_sr;
  sr_infos.xel_count = 0;

  // Prepare SyncRead structure
  for (i = 0; i < DXL_ID_CNT; i++) {
    info_xels_sr[i].id = DXL_ID_LIST[i];
    info_xels_sr[i].p_recv_buf = (uint8_t *)&sr_data + i * sizeof(sr_data_t);  // Adjust parsing
    sr_infos.addr = SR_START_ADDRS[i];
    sr_infos.addr_length = SR_ADDR_LENGTHS[i];
    sr_infos.xel_count++;
  }
  sr_infos.is_info_changed = true;
}

void loop() {
  static uint32_t try_count = 0;
  uint8_t recv_cnt;

  // Transmit predefined SyncRead instruction packet
  // and receive a status packet from each DYNAMIXEL
  recv_cnt = dxl.syncRead(&sr_infos);
  if (recv_cnt > 0) {
    Serial.print("[SyncRead] Success, Received ID Count: ");
    Serial.println(recv_cnt);
    for (uint8_t i = 0; i < recv_cnt; i++) {
      Serial.print("  ID: ");
      Serial.print(sr_infos.p_xels[i].id);
      Serial.print(", Error: ");
      Serial.println(sr_infos.p_xels[i].error);
      Serial.print("\t Present Position (raw): ");
      Serial.println(sr_data.present_position, HEX);
      Serial.print("\t Present Current (raw): ");
      Serial.println(sr_data.present_current, HEX);
    }
  } else {
    Serial.print("[SyncRead] Fail, Lib error code: ");
    Serial.println(dxl.getLastLibErrCode());
  }

  Serial.println("=======================================================");

  delay(750);
}

[roboteer]

Can you try to use Servo IDs 10 and 11 instead of 0 and 1 to see if that helps?

[bryan]

You mean to change the original IDs from 0 and 1 to 10 and 11, then try this prog by modifying id to 10 and 11?

[roboteer]

Yes that was what I meant. Any luck when you change IDs?

[bryan]

nope. what’s the reason that changing id might help anyway?

[roboteer]

Last year, I found another way to do Sync-Read/Write that does not use that “packed data” structure in your current approach and it also uses the Dynamixel2Arduino Library directly (not via Dynamixel Shield Library). You want to try it? Here is the link to that post

A Simpler Way to use SyncWrite with Dynamixel2Arduino Library

I was comparing the programming approach used in the provided Dynamixel2Arduino example sketches: sync_read_write_position.ino and sync_read_write_velocity.ino, and the one used on the ROBOTIS E-manual (https://emanual.robotis.com/docs/en/popup/arduino_api/syncWrite/). I’ve found the approach shown on the E-manual much easier to understand and expand its use, so I created the following Arduino sketch for MKR ZERO combining SyncWrite for Goal Velocity and Goal Position. I used TWO 2XL-430s: the…

[bryan]

Yup! Thanks for your suggestion! I am actually attempting to do the bulk read so i can read data faster.

[bryan]

Update: Am using the previous depreciated code and adding more than 1 params for a single motor does not work. Only the first data will be provided.

Result: Only the first data access will be provided, (Position in this case)

#include <DynamixelShield.h>

ParamForBulkReadInst_t bulk_read_param;
ParamForBulkWriteInst_t bulk_write_param;
RecvInfoFromStatusInst_t read_result;
int DIR = 18;
DynamixelShield dxl(Serial2, DIR);

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  dxl.begin(115200);
  dxl.scan();

  // fill the members of structure for bulkWrite
  bulk_write_param.xel[0].id = 0;
  bulk_write_param.xel[1].id = 1;
  bulk_write_param.xel[0].addr = 116; //Goal Position on X serise
  bulk_write_param.xel[1].addr = 104; //Goal Velocity on X serise
  bulk_write_param.xel[0].length = 4;
  bulk_write_param.xel[1].length = 4;
  bulk_write_param.id_count = 2;

  // fill the members of structure for bulkRead
  bulk_read_param.xel[0].id = 0;
  bulk_read_param.xel[0].addr = 132; //Present Position on X serise
  bulk_read_param.xel[0].length = 4;
  bulk_read_param.xel[1].id = 1;
  bulk_read_param.xel[1].addr = 132; //Present Velocity on X serise
  bulk_read_param.xel[1].length = 4;
  bulk_read_param.xel[2].id = 0;
  bulk_read_param.xel[2].addr = 128; //Present Position on X serise
  bulk_read_param.xel[2].length = 4;
  bulk_read_param.xel[3].id = 1;
  bulk_read_param.xel[3].addr = 128; //Present Velocity on X serise
  bulk_read_param.xel[3].length = 4;
  bulk_read_param.id_count = 8;

  dxl.torqueOff(0);
  dxl.setOperatingMode(0, OP_POSITION);
  dxl.torqueOn(0);

  dxl.torqueOff(1);
  dxl.setOperatingMode(1, OP_VELOCITY);
  dxl.torqueOn(1);
}

void loop() {
  // put your main code here, to run repeatedly:
  static int32_t position = 0, velocity = 0;
  int32_t recv_position = 0, recv_velocity = 0;
  int32_t recv_position2 = 0, recv_velocity2 = 0;

  // set value to data buffer for bulkWrite
  position = position >= 4095 ? 0 : position + 409;
  memcpy(bulk_write_param.xel[0].data, &position, sizeof(position));
  velocity = velocity >= 200 ? -200 : velocity + 10;
  memcpy(bulk_write_param.xel[1].data, &velocity, sizeof(velocity));

  // send command using bulkWrite
  dxl.bulkWrite(bulk_write_param);
  //delay(100);

  // Print the read data using bulkRead
  dxl.bulkRead(bulk_read_param, read_result);
  Serial.println(F("======= Bulk Read ========"));
  memcpy(&recv_position, read_result.xel[0].data, read_result.xel[0].length);
  memcpy(&recv_position2, read_result.xel[1].data, read_result.xel[1].length);
  memcpy(&recv_velocity, read_result.xel[2].data, read_result.xel[2].length);
  memcpy(&recv_velocity2, read_result.xel[3].data, read_result.xel[3].length);
  Serial.print(F("ID: ")); Serial.print(read_result.xel[0].id); Serial.print(" ");
  Serial.print(F(", Present Position: ")); Serial.print(recv_position); Serial.print(" ");
  Serial.print(F(", Packet Error: ")); Serial.print(read_result.xel[0].error); Serial.print(" ");
  Serial.print(F(", Param Length: ")); Serial.print(read_result.xel[0].length); Serial.print(" ");
  Serial.println();
  Serial.print(F("ID: ")); Serial.print(read_result.xel[1].id); Serial.print(" ");
  Serial.print(F(", Present Position: ")); Serial.print(recv_position2); Serial.print(" ");
  Serial.print(F(", Packet Error: ")); Serial.print(read_result.xel[1].error); Serial.print(" ");
  Serial.print(F(", Param Length: ")); Serial.print(read_result.xel[1].length); Serial.print(" ");
  Serial.println();
  Serial.print(F("ID: ")); Serial.print(read_result.xel[2].id); Serial.print(" ");
  Serial.print(F(", Present Velocity: ")); Serial.print(recv_velocity); Serial.print(" ");
  Serial.print(F(", Packet Error: ")); Serial.print(read_result.xel[2].error); Serial.print(" ");
  Serial.print(F(", Param Length: ")); Serial.print(read_result.xel[2].length); Serial.print(" ");
  Serial.println();
  
  Serial.print(F("ID: ")); Serial.print(read_result.xel[3].id); Serial.print(" ");
  Serial.print(F(", Present Velocity: ")); Serial.print(recv_velocity2); Serial.print(" ");
  Serial.print(F(", Packet Error: ")); Serial.print(read_result.xel[3].error); Serial.print(" ");
  Serial.print(F(", Param Length: ")); Serial.print(read_result.xel[3].length); Serial.print(" ");
  Serial.println();
  
  //delay(100);
  
}

[roboteer]

Interesting! I only used either the OpenRB-150 or the MKR DXL Shield with MKR Zero and I did not have that issue.
I do not have the original DXL Shield and your types of Arduino board so I can’t help further - Sorry

[bryan]

Well, I have managed to identify the issue:

Bulk Read:
This instruction is used for reading values of multiple MX series DYNAMIXEL’s simultaneously by sending a single Instruction Packet. The packet length is shortened compared to sending multiple READ commands, and the idle time between the status packets being returned is also shortened to save communication time. However, this cannot be used to read a single module. If an identical ID is designated multiple times, only the first designated parameter will be processed.

Hence we would have to read through all data between the smallest to largest address for each motor.

[roboteer]

You have been using your DXLs at 115.2 Kbps so I wonder if using 1 Mbps for DXLs would help with their performance.