A Simpler Way to use SyncWrite with Dynamixel2Arduino Library

Community
, ,
1

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 one with IDs 1-2 is set into Position Control mode, and the one with IDs 11-12 is set into Velocity Control mode.

#include <Dynamixel2Arduino.h>

#define DXL_SERIAL   Serial1
#define DEBUG_SERIAL Serial
const int DXL_DIR_PIN = A6; // DYNAMIXEL Shield DIR PIN for MKR Shield
const uint8_t BROADCAST_ID = 254;
const float DYNAMIXEL_PROTOCOL_VERSION = 2.0;
const uint8_t GP_DXL_ID_CNT = 2;
const uint8_t GP_DXL_ID_LIST[GP_DXL_ID_CNT] = {1, 2};  // servos set in Position Control mode
const uint8_t GV_DXL_ID_CNT = 2;
const uint8_t GV_DXL_ID_LIST[GV_DXL_ID_CNT] = {11, 12};   // servos set in Velocity Control mode
const uint16_t ADDR_X_PRESENT_POSITION = 132;
const uint16_t LEN_X_PRESENT_POSITION = 4;
const uint16_t ADDR_X_GOAL_POSITION = 116;
const uint16_t LEN_X_GOAL_POSITION = 4;
const uint16_t ADDR_X_GOAL_VELOCITY = 104; 
const uint16_t LEN_X_GOAL_VELOCITY = 4;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

ParamForSyncWriteInst_t GV_sync_write_param; // Parameters structure for SW GV packet
ParamForSyncWriteInst_t GP_sync_write_param; // Parameters structure for SW GP packet

int32_t goal_velocity[GV_DXL_ID_CNT] = {100, 100};
int32_t goal_position[GP_DXL_ID_CNT] = {2048, 2048}; 

void setup() {

  DEBUG_SERIAL.println("Initial Setup & Test");
  // put your setup code here, to run once:
  uint8_t i;
  dxl.begin(1000000);
  dxl.setPortProtocolVersion(DYNAMIXEL_PROTOCOL_VERSION);
  dxl.scan();
  
  // Preparing for GV SW packet
  GV_sync_write_param.addr = ADDR_X_GOAL_VELOCITY; //Goal Velocity of DYNAMIXEL-X series
  GV_sync_write_param.length = LEN_X_GOAL_VELOCITY;
  GV_sync_write_param.id_count = GV_DXL_ID_CNT;  // Number of servos in Velocity Control Mode
  for(i = 0; i < GV_DXL_ID_CNT; i++){
    GV_sync_write_param.xel[i].id = GV_DXL_ID_LIST[i];
  }
  // Preparing for GP SW Packet
  GP_sync_write_param.addr = ADDR_X_GOAL_POSITION; //Goal Position of DYNAMIXEL-X series
  GP_sync_write_param.length = LEN_X_GOAL_POSITION;
  GP_sync_write_param.id_count = GP_DXL_ID_CNT;  // Number of servos in Position Control Mode
  for(i = 0; i < GP_DXL_ID_CNT; i++){
    GP_sync_write_param.xel[i].id = GP_DXL_ID_LIST[i];
  }

// Setting GP servos into Position Control mode
  for(i = 0; i < GP_DXL_ID_CNT; i++){
    dxl.torqueOff(GP_DXL_ID_LIST[i]);
    dxl.setOperatingMode(GP_DXL_ID_LIST[i], OP_POSITION);
  }

// Setting GV servos into Velocity Control mode
  for(i=0; i<GV_DXL_ID_CNT; i++){
    dxl.torqueOff(GV_DXL_ID_LIST[i]);
    dxl.setOperatingMode(GV_DXL_ID_LIST[i], OP_VELOCITY);
  }
  
// Torque enable all GP and GV servos  
  dxl.torqueOn(BROADCAST_ID);

 // Update actual goal velocity data
  for(i=0; i<GV_DXL_ID_CNT; i++){
    memcpy(GV_sync_write_param.xel[i].data, &goal_velocity[i], sizeof(goal_velocity[i]));
  }
  dxl.syncWrite(GV_sync_write_param);   // send GV SW Packet

  // Update actual goal position data
  for(i=0; i<GP_DXL_ID_CNT; i++){
    memcpy(GP_sync_write_param.xel[i].data, &goal_position[i], sizeof(goal_position[i]));
  }
  dxl.syncWrite(GP_sync_write_param);   // send GP SW Packet

  delay(5000);
  DEBUG_SERIAL.println("END of Initial Setup & Test");

}   // end of setup()

void loop() {
  DEBUG_SERIAL.println("Randomized Test");

   // Update actual goal velocity data with randomized values
  for(int i=0; i<GV_DXL_ID_CNT; i++){
    goal_velocity[i] = (uint32_t) random(10,200);
    memcpy(GV_sync_write_param.xel[i].data, &goal_velocity[i], sizeof(goal_velocity[i]));
  }
  dxl.syncWrite(GV_sync_write_param);   // send GV SW Packet

  // Update actual goal position data with randomized values
  for(int i=0; i<GP_DXL_ID_CNT; i++){
    goal_position[i] = (uint32_t) random(1024,3072);
    memcpy(GP_sync_write_param.xel[i].data, &goal_position[i], sizeof(goal_position[i]));
  }
  dxl.syncWrite(GP_sync_write_param);   // send GP SW Packet

  delay(1000);

}   // end of loop()

Enjoy!

2 Likes
2

Hi, thanks this is really helpful. I have a question : I would like to update both profile velocity (address 112) and goal position (address 116 - ie
next to profile velocity). Can I do this in with one
structure , or should I create two parameter structures ?

If it is possible with one structure, how to I go about seeing this up ? I’ve not used structures before so any help you can offer would be most appreciated. Nik

3

I am not sure that I understood your question properly or not. From my experience, I have never tried to use Sync-Read/Write more than 1 type of parameter across different servo IDs. So I have no idea on how to do that.

For your situation, I would create 2 SW Packets, one for Goal Positions across the servo IDs and another for Profile Velocities across the servo IDs.

In the sample code, I had 1 structure for GP and another one for GV, so you should be able to adapt the GV structure with the proper address and servo ID list and just rename it as “PV”.

4

Many thanks. That was my original plan, and then the manual below seemed to suggest you could have an 8 byte field , with the two data fields next to each other. I will go with what you suggest.

I have one further question - sometimes I want to synchronise 2 servos, other times 3. Can I use the same data structure, but update the GV_sync_write_param.id_count (in your example) ?

An alternative would be to set a dummy address to a servo.

I look forward to trying these ideas once my servos arrive in a week.

https://emanual.robotis.com/docs/en/dxl/protocol2/#sync-write-0x83

5

If it was me I would always use a setup for 3 servos and just ignore the data for the 3rd one when I don’t need it.

1 Like